/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
package com.ctre.phoenix6.configs;

import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.configs.jni.ConfigJNI;
import com.ctre.phoenix6.spns.*;
import com.ctre.phoenix6.signals.*;
import com.ctre.phoenix6.hardware.core.CoreCANcoder;

/**
 * Configs that affect the feedback of this motor controller.
 * <p>
 * Includes feedback sensor source, any offsets for the feedback
 * sensor, and various ratios to describe the relationship between the
 * sensor and the mechanism for closed looping.
 */
public class FeedbackConfigs implements ParentConfiguration
{
    /**
     * This offset is applied to the absolute integrated rotor sensor. 
     * This can be used to zero the rotor in applications that are within
     * one rotor rotation.
     * 
     *   <ul>
     *   <li> <b>Minimum Value:</b> -1
     *   <li> <b>Maximum Value:</b> 1
     *   <li> <b>Default Value:</b> 0.0
     *   <li> <b>Units:</b> rotations
     *   </ul>
     */
    public double FeedbackRotorOffset = 0.0;
    /**
     * This is the ratio of sensor rotations to the mechanism's output. 
     * This is equivalent to the mechanism's gear ratio if the sensor is
     * located on the input of a gearbox.  If sensor is on the output of a
     * gearbox, then this is typically set to 1.  Note if this is set to
     * zero, device will reset back to one.
     * 
     *   <ul>
     *   <li> <b>Minimum Value:</b> -1000
     *   <li> <b>Maximum Value:</b> 1000
     *   <li> <b>Default Value:</b> 1.0
     *   <li> <b>Units:</b> scalar
     *   </ul>
     */
    public double SensorToMechanismRatio = 1.0;
    /**
     * Talon FX is capable of fusing a remote CANcoder with its rotor
     * sensor to produce a high-bandwidth sensor source.  This feature
     * requires specifying the ratio between the remote sensor and the
     * motor rotor.  Note if this is set to zero, device will reset back
     * to one.
     * 
     *   <ul>
     *   <li> <b>Minimum Value:</b> -1000
     *   <li> <b>Maximum Value:</b> 1000
     *   <li> <b>Default Value:</b> 1.0
     *   <li> <b>Units:</b> scalar
     *   </ul>
     */
    public double RotorToSensorRatio = 1.0;
    /**
     * Choose what sensor source is reported via API and used by
     * closed-loop and limit features.  The default is RotorSensor, which
     * uses the internal rotor sensor in the Talon FX.  Choose
     * RemoteCANcoder to use another CANcoder on the same CAN bus (this
     * also requires setting FeedbackRemoteSensorID).  Talon FX will
     * update its position and velocity whenever CANcoder publishes its
     * information on CAN bus.  Choose FusedCANcoder (requires Phoenix
     * Pro) and Talon FX will fuse another CANcoder's information with the
     * internal rotor, which provides the best possible position and
     * velocity for accuracy and bandwidth (note this requires setting
     * FeedbackRemoteSensorID).  FusedCANcoder was developed for
     * applications such as swerve-azimuth.  Choose SyncCANcoder (requires
     * Phoenix Pro) and Talon FX will synchronize its internal rotor
     * position against another CANcoder, then continue to use the rotor
     * sensor for closed loop control (note this requires setting
     * FeedbackRemoteSensorID).  The TalonFX will report if its internal
     * position differs significantly from the reported CANcoder position.
     *  SyncCANcoder was developed for mechanisms where there is a risk of
     * the CANcoder failing in such a way that it reports a position that
     * does not match the mechanism, such as the sensor mounting assembly
     * breaking off.  Choose RemotePigeon2_Yaw, RemotePigeon2_Pitch, and
     * RemotePigeon2_Roll to use another Pigeon2 on the same CAN bus (this
     * also requires setting FeedbackRemoteSensorID).  Talon FX will
     * update its position to match the selected value whenever Pigeon2
     * publishes its information on CAN bus. Note that the Talon FX
     * position will be in rotations and not degrees.
     * <p>
     * Note: When the Talon Source is changed to FusedCANcoder, the Talon
     * needs a period of time to fuse before sensor-based (soft-limit,
     * closed loop, etc.) features are used. This period of time is
     * determined by the update frequency of the CANcoder's Position
     * signal.
     * 
     */
    public FeedbackSensorSourceValue FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
    /**
     * Device ID of which remote device to use.  This is not used if the
     * Sensor Source is the internal rotor sensor.
     * 
     *   <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 62
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     *   </ul>
     */
    public int FeedbackRemoteSensorID = 0;
    
    /**
     * Helper method to configure this feedback group to use Fused
     * CANcoder by passing in the CANcoder object
     * 
     * @param device    CANcoder reference to use for FusedCANcoder
     */
    FeedbackConfigs withFusedCANcoder(CoreCANcoder device)
    {
        this.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
        this.FeedbackRemoteSensorID = device.getDeviceID();
        return this;
    }
    
    /**
     * Helper method to configure this feedback group to use Remote
     * CANcoder by passing in the CANcoder object
     * 
     * @param device    CANcoder reference to use for FusedCANcoder
     */
    FeedbackConfigs withRemoteCANcoder(CoreCANcoder device)
    {
        this.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
        this.FeedbackRemoteSensorID = device.getDeviceID();
        return this;
    }
    
    

    @Override
    public String toString()
    {
        String ss = "Config Group: Feedback\n";
        ss += "Name: \"FeedbackRotorOffset\" Value: \"" + FeedbackRotorOffset + "rotations\"" + "\n";
        ss += "Name: \"SensorToMechanismRatio\" Value: \"" + SensorToMechanismRatio + "scalar\"" + "\n";
        ss += "Name: \"RotorToSensorRatio\" Value: \"" + RotorToSensorRatio + "scalar\"" + "\n";
        ss += "Name: \"FeedbackSensorSource\" Value: \"" + FeedbackSensorSource + "\"" + "\n";
        ss += "Name: \"FeedbackRemoteSensorID\" Value: \"" + FeedbackRemoteSensorID + "\"" + "\n";
        return ss;
    }

    /**
     *
     */
    public StatusCode deserialize(String to_deserialize)
    {
        FeedbackRotorOffset = ConfigJNI.Deserializedouble(SpnValue.Config_FeedbackRotorOffset.value, to_deserialize);
        SensorToMechanismRatio = ConfigJNI.Deserializedouble(SpnValue.Config_SensorToMechanismRatio.value, to_deserialize);
        RotorToSensorRatio = ConfigJNI.Deserializedouble(SpnValue.Config_RotorToSensorRatio.value, to_deserialize);
        FeedbackSensorSource = FeedbackSensorSourceValue.valueOf(ConfigJNI.Deserializeint(SpnValue.Config_FeedbackSensorSource.value, to_deserialize));
        FeedbackRemoteSensorID = ConfigJNI.Deserializeint(SpnValue.Config_FeedbackRemoteSensorID.value, to_deserialize);
        return  StatusCode.OK;
    }

    /**
     *
     */
    public String serialize()
    {
        String ss = "";
        ss += ConfigJNI.Serializedouble(SpnValue.Config_FeedbackRotorOffset.value, FeedbackRotorOffset);
        ss += ConfigJNI.Serializedouble(SpnValue.Config_SensorToMechanismRatio.value, SensorToMechanismRatio);
        ss += ConfigJNI.Serializedouble(SpnValue.Config_RotorToSensorRatio.value, RotorToSensorRatio);
        ss += ConfigJNI.Serializeint(SpnValue.Config_FeedbackSensorSource.value, FeedbackSensorSource.value);
        ss += ConfigJNI.Serializeint(SpnValue.Config_FeedbackRemoteSensorID.value, FeedbackRemoteSensorID);
        return ss;
    }
}