/*
 * 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 java.util.HashMap;
import java.util.Map;

import com.ctre.phoenix6.spns.*;
import com.ctre.phoenix6.signals.*;

/**
 * Gains for the specified slot.
 * <p>
 * If this slot is selected, these gains are used in closed loop
 * control requests.
 */
public class SlotConfigs implements ParentConfiguration
{
    /**
     * Proportional Gain.
     * <p>
     * The units for this gain is dependent on the control mode. Since
     * this gain is multiplied by error in the input, the units should be
     * defined as units of output per unit of input error. For example,
     * when controlling velocity using a duty cycle closed loop, the units
     * for the proportional gain will be duty cycle per rps of error, or
     * 1/rps.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 3.4e+38
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     */
    public double kP = 0;
    /**
     * Integral Gain.
     * <p>
     * The units for this gain is dependent on the control mode. Since
     * this gain is multiplied by error in the input integrated over time
     * (in units of seconds), the units should be defined as units of
     * output per unit of integrated input error. For example, when
     * controlling velocity using a duty cycle closed loop, integrating
     * velocity over time results in rps * s = rotations. Therefore, the
     * units for the integral gain will be duty cycle per rotation of
     * accumulated error, or 1/rot.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 3.4e+38
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     */
    public double kI = 0;
    /**
     * Derivative Gain.
     * <p>
     * The units for this gain is dependent on the control mode. Since
     * this gain is multiplied by the derivative of error in the input
     * with respect to time (in units of seconds), the units should be
     * defined as units of output per unit of the differentiated input
     * error. For example, when controlling velocity using a duty cycle
     * closed loop, the derivative of velocity with respect to time is rot
     * per sec², which is acceleration. Therefore, the units for the
     * derivative gain will be duty cycle per unit of acceleration error,
     * or 1/(rot per sec²).
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 3.4e+38
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     */
    public double kD = 0;
    /**
     * Static Feedforward Gain.
     * <p>
     * This is added to the closed loop output. The unit for this constant
     * is dependent on the control mode, typically fractional duty cycle,
     * voltage, or torque current.
     * <p>
     * The sign is typically determined by reference velocity when using
     * position, velocity, and Motion Magic® closed loop modes. However,
     * when using position closed loop with zero velocity reference (no
     * motion profiling), the application can instead use the position
     * closed loop error by setting the Static Feedforward Sign
     * configuration parameter.  When doing so, we recommend the minimal
     * amount of kS, otherwise the motor output may dither when closed
     * loop error is near zero.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -512
     *   <li> <b>Maximum Value:</b> 511
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     */
    public double kS = 0;
    /**
     * Velocity Feedforward Gain.
     * <p>
     * The units for this gain is dependent on the control mode. Since
     * this gain is multiplied by the requested velocity, the units should
     * be defined as units of output per unit of requested input velocity.
     * For example, when controlling velocity using a duty cycle closed
     * loop, the units for the velocity feedfoward gain will be duty cycle
     * per requested rps, or 1/rps.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 3.4e+38
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     */
    public double kV = 0;
    /**
     * Acceleration Feedforward Gain.
     * <p>
     * The units for this gain is dependent on the control mode. Since
     * this gain is multiplied by the requested acceleration, the units
     * should be defined as units of output per unit of requested input
     * acceleration. For example, when controlling velocity using a duty
     * cycle closed loop, the units for the acceleration feedfoward gain
     * will be duty cycle per requested rot per sec², or 1/(rot per sec²).
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 3.4e+38
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     */
    public double kA = 0;
    /**
     * Gravity Feedforward/Feedback Gain.
     * <p>
     * This is added to the closed loop output. The sign is determined by
     * GravityType. The unit for this constant is dependent on the control
     * mode, typically fractional duty cycle, voltage, or torque current.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -512
     *   <li> <b>Maximum Value:</b> 511
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     */
    public double kG = 0;
    /**
     * Gravity Feedforward/Feedback Type.
     * <p>
     * This determines the type of the gravity feedforward/feedback.
     * <p>
     * Choose Elevator_Static for systems where the gravity feedforward is
     * constant, such as an elevator. The gravity feedforward output will
     * always have the same sign.
     * <p>
     * Choose Arm_Cosine for systems where the gravity feedback is
     * dependent on the angular position of the mechanism, such as an arm.
     * The gravity feedback output will vary depending on the mechanism
     * angular position. Note that the sensor offset and ratios must be
     * configured so that the sensor reports a position of 0 when the
     * mechanism is horizonal (parallel to the ground), and the reported
     * sensor position is 1:1 with the mechanism.
     * 
     */
    public GravityTypeValue GravityType = GravityTypeValue.Elevator_Static;
    /**
     * Static Feedforward Sign during position closed loop.
     * <p>
     * This determines the sign of the applied kS during position
     * closed-loop modes. The default behavior uses the velocity reference
     * sign. This works well with velocity closed loop, Motion Magic®
     * controls, and position closed loop when velocity reference is
     * specified (motion profiling).
     * <p>
     * However, when using position closed loop with zero velocity
     * reference (no motion profiling), the application may want to apply
     * static feedforward based on the sign of closed loop error instead.
     * When doing so, we recommend using the minimal amount of kS,
     * otherwise the motor output may dither when closed loop error is
     * near zero.
     * 
     */
    public StaticFeedforwardSignValue StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign;
    
    /**
     * Modifies this configuration's kP parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Proportional Gain.
     * <p>
     * The units for this gain is dependent on the control mode. Since
     * this gain is multiplied by error in the input, the units should be
     * defined as units of output per unit of input error. For example,
     * when controlling velocity using a duty cycle closed loop, the units
     * for the proportional gain will be duty cycle per rps of error, or
     * 1/rps.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 3.4e+38
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     *
     * @param newKP Parameter to modify
     * @return Itself
     */
    public SlotConfigs withKP(double newKP)
    {
        kP = newKP;
        return this;
    }
    
    /**
     * Modifies this configuration's kI parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Integral Gain.
     * <p>
     * The units for this gain is dependent on the control mode. Since
     * this gain is multiplied by error in the input integrated over time
     * (in units of seconds), the units should be defined as units of
     * output per unit of integrated input error. For example, when
     * controlling velocity using a duty cycle closed loop, integrating
     * velocity over time results in rps * s = rotations. Therefore, the
     * units for the integral gain will be duty cycle per rotation of
     * accumulated error, or 1/rot.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 3.4e+38
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     *
     * @param newKI Parameter to modify
     * @return Itself
     */
    public SlotConfigs withKI(double newKI)
    {
        kI = newKI;
        return this;
    }
    
    /**
     * Modifies this configuration's kD parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Derivative Gain.
     * <p>
     * The units for this gain is dependent on the control mode. Since
     * this gain is multiplied by the derivative of error in the input
     * with respect to time (in units of seconds), the units should be
     * defined as units of output per unit of the differentiated input
     * error. For example, when controlling velocity using a duty cycle
     * closed loop, the derivative of velocity with respect to time is rot
     * per sec², which is acceleration. Therefore, the units for the
     * derivative gain will be duty cycle per unit of acceleration error,
     * or 1/(rot per sec²).
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 3.4e+38
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     *
     * @param newKD Parameter to modify
     * @return Itself
     */
    public SlotConfigs withKD(double newKD)
    {
        kD = newKD;
        return this;
    }
    
    /**
     * Modifies this configuration's kS parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Static Feedforward Gain.
     * <p>
     * This is added to the closed loop output. The unit for this constant
     * is dependent on the control mode, typically fractional duty cycle,
     * voltage, or torque current.
     * <p>
     * The sign is typically determined by reference velocity when using
     * position, velocity, and Motion Magic® closed loop modes. However,
     * when using position closed loop with zero velocity reference (no
     * motion profiling), the application can instead use the position
     * closed loop error by setting the Static Feedforward Sign
     * configuration parameter.  When doing so, we recommend the minimal
     * amount of kS, otherwise the motor output may dither when closed
     * loop error is near zero.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -512
     *   <li> <b>Maximum Value:</b> 511
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     *
     * @param newKS Parameter to modify
     * @return Itself
     */
    public SlotConfigs withKS(double newKS)
    {
        kS = newKS;
        return this;
    }
    
    /**
     * Modifies this configuration's kV parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Velocity Feedforward Gain.
     * <p>
     * The units for this gain is dependent on the control mode. Since
     * this gain is multiplied by the requested velocity, the units should
     * be defined as units of output per unit of requested input velocity.
     * For example, when controlling velocity using a duty cycle closed
     * loop, the units for the velocity feedfoward gain will be duty cycle
     * per requested rps, or 1/rps.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 3.4e+38
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     *
     * @param newKV Parameter to modify
     * @return Itself
     */
    public SlotConfigs withKV(double newKV)
    {
        kV = newKV;
        return this;
    }
    
    /**
     * Modifies this configuration's kA parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Acceleration Feedforward Gain.
     * <p>
     * The units for this gain is dependent on the control mode. Since
     * this gain is multiplied by the requested acceleration, the units
     * should be defined as units of output per unit of requested input
     * acceleration. For example, when controlling velocity using a duty
     * cycle closed loop, the units for the acceleration feedfoward gain
     * will be duty cycle per requested rot per sec², or 1/(rot per sec²).
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 3.4e+38
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     *
     * @param newKA Parameter to modify
     * @return Itself
     */
    public SlotConfigs withKA(double newKA)
    {
        kA = newKA;
        return this;
    }
    
    /**
     * Modifies this configuration's kG parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Gravity Feedforward/Feedback Gain.
     * <p>
     * This is added to the closed loop output. The sign is determined by
     * GravityType. The unit for this constant is dependent on the control
     * mode, typically fractional duty cycle, voltage, or torque current.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> -512
     *   <li> <b>Maximum Value:</b> 511
     *   <li> <b>Default Value:</b> 0
     *   <li> <b>Units:</b> 
     * </ul>
     *
     * @param newKG Parameter to modify
     * @return Itself
     */
    public SlotConfigs withKG(double newKG)
    {
        kG = newKG;
        return this;
    }
    
    /**
     * Modifies this configuration's GravityType parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Gravity Feedforward/Feedback Type.
     * <p>
     * This determines the type of the gravity feedforward/feedback.
     * <p>
     * Choose Elevator_Static for systems where the gravity feedforward is
     * constant, such as an elevator. The gravity feedforward output will
     * always have the same sign.
     * <p>
     * Choose Arm_Cosine for systems where the gravity feedback is
     * dependent on the angular position of the mechanism, such as an arm.
     * The gravity feedback output will vary depending on the mechanism
     * angular position. Note that the sensor offset and ratios must be
     * configured so that the sensor reports a position of 0 when the
     * mechanism is horizonal (parallel to the ground), and the reported
     * sensor position is 1:1 with the mechanism.
     * 
     *
     * @param newGravityType Parameter to modify
     * @return Itself
     */
    public SlotConfigs withGravityType(GravityTypeValue newGravityType)
    {
        GravityType = newGravityType;
        return this;
    }
    
    /**
     * Modifies this configuration's StaticFeedforwardSign parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Static Feedforward Sign during position closed loop.
     * <p>
     * This determines the sign of the applied kS during position
     * closed-loop modes. The default behavior uses the velocity reference
     * sign. This works well with velocity closed loop, Motion Magic®
     * controls, and position closed loop when velocity reference is
     * specified (motion profiling).
     * <p>
     * However, when using position closed loop with zero velocity
     * reference (no motion profiling), the application may want to apply
     * static feedforward based on the sign of closed loop error instead.
     * When doing so, we recommend using the minimal amount of kS,
     * otherwise the motor output may dither when closed loop error is
     * near zero.
     * 
     *
     * @param newStaticFeedforwardSign Parameter to modify
     * @return Itself
     */
    public SlotConfigs withStaticFeedforwardSign(StaticFeedforwardSignValue newStaticFeedforwardSign)
    {
        StaticFeedforwardSign = newStaticFeedforwardSign;
        return this;
    }


    /**
     * Chooses which slot these configs are for.
     */
    public int SlotNumber = 0;

    private class SlotSpns
    {
        int kPSpn;
        int kISpn;
        int kDSpn;
        int kSSpn;
        int kVSpn;
        int kASpn;
        int kGSpn;
        int GravityTypeSpn;
        int StaticFeedforwardSignSpn;
    }

    private Map<Integer, SlotSpns> genericMap = new HashMap<Integer, SlotSpns>() {{
        put(0, new SlotSpns() {{
            kPSpn = SpnValue.Slot0_kP.value;
            kISpn = SpnValue.Slot0_kI.value;
            kDSpn = SpnValue.Slot0_kD.value;
            kSSpn = SpnValue.Slot0_kS.value;
            kVSpn = SpnValue.Slot0_kV.value;
            kASpn = SpnValue.Slot0_kA.value;
            kGSpn = SpnValue.Slot0_kG.value;
            GravityTypeSpn = SpnValue.Slot0_kG_Type.value;
            StaticFeedforwardSignSpn = SpnValue.Slot0_kS_Sign.value;
        }});
        put(1, new SlotSpns() {{
            kPSpn = SpnValue.Slot1_kP.value;
            kISpn = SpnValue.Slot1_kI.value;
            kDSpn = SpnValue.Slot1_kD.value;
            kSSpn = SpnValue.Slot1_kS.value;
            kVSpn = SpnValue.Slot1_kV.value;
            kASpn = SpnValue.Slot1_kA.value;
            kGSpn = SpnValue.Slot1_kG.value;
            GravityTypeSpn = SpnValue.Slot1_kG_Type.value;
            StaticFeedforwardSignSpn = SpnValue.Slot1_kS_Sign.value;
        }});
        put(2, new SlotSpns() {{
            kPSpn = SpnValue.Slot2_kP.value;
            kISpn = SpnValue.Slot2_kI.value;
            kDSpn = SpnValue.Slot2_kD.value;
            kSSpn = SpnValue.Slot2_kS.value;
            kVSpn = SpnValue.Slot2_kV.value;
            kASpn = SpnValue.Slot2_kA.value;
            kGSpn = SpnValue.Slot2_kG.value;
            GravityTypeSpn = SpnValue.Slot2_kG_Type.value;
            StaticFeedforwardSignSpn = SpnValue.Slot2_kS_Sign.value;
        }});
    }};

    public static SlotConfigs from(Slot0Configs value)
    {
        return new SlotConfigs() {{
            kP = value.kP;
            kI = value.kI;
            kD = value.kD;
            kS = value.kS;
            kV = value.kV;
            kA = value.kA;
            kG = value.kG;
            GravityType = value.GravityType;
            StaticFeedforwardSign = value.StaticFeedforwardSign;
            SlotNumber = 0;
        }};
    }
    public static SlotConfigs from(Slot1Configs value)
    {
        return new SlotConfigs() {{
            kP = value.kP;
            kI = value.kI;
            kD = value.kD;
            kS = value.kS;
            kV = value.kV;
            kA = value.kA;
            kG = value.kG;
            GravityType = value.GravityType;
            StaticFeedforwardSign = value.StaticFeedforwardSign;
            SlotNumber = 1;
        }};
    }
    public static SlotConfigs from(Slot2Configs value)
    {
        return new SlotConfigs() {{
            kP = value.kP;
            kI = value.kI;
            kD = value.kD;
            kS = value.kS;
            kV = value.kV;
            kA = value.kA;
            kG = value.kG;
            GravityType = value.GravityType;
            StaticFeedforwardSign = value.StaticFeedforwardSign;
            SlotNumber = 2;
        }};
    }

    @Override
    public String toString()
    {
        String ss = "Config Group: Slot\n";
        ss += "    kP: " + kP + "\n";
        ss += "    kI: " + kI + "\n";
        ss += "    kD: " + kD + "\n";
        ss += "    kS: " + kS + "\n";
        ss += "    kV: " + kV + "\n";
        ss += "    kA: " + kA + "\n";
        ss += "    kG: " + kG + "\n";
        ss += "    GravityType: " + GravityType + "\n";
        ss += "    StaticFeedforwardSign: " + StaticFeedforwardSign + "\n";
        return ss;
    }

    /**
     *
     */
    public StatusCode deserialize(String to_deserialize)
    {
        SlotSpns currentSpns = genericMap.get(SlotNumber);
        kP = ConfigJNI.Deserializedouble(currentSpns.kPSpn, to_deserialize);
        kI = ConfigJNI.Deserializedouble(currentSpns.kISpn, to_deserialize);
        kD = ConfigJNI.Deserializedouble(currentSpns.kDSpn, to_deserialize);
        kS = ConfigJNI.Deserializedouble(currentSpns.kSSpn, to_deserialize);
        kV = ConfigJNI.Deserializedouble(currentSpns.kVSpn, to_deserialize);
        kA = ConfigJNI.Deserializedouble(currentSpns.kASpn, to_deserialize);
        kG = ConfigJNI.Deserializedouble(currentSpns.kGSpn, to_deserialize);
        GravityType = GravityTypeValue.valueOf(ConfigJNI.Deserializeint(currentSpns.GravityTypeSpn, to_deserialize));
        StaticFeedforwardSign = StaticFeedforwardSignValue.valueOf(ConfigJNI.Deserializeint(currentSpns.StaticFeedforwardSignSpn, to_deserialize));
        return  StatusCode.OK;
    }

    /**
     *
     */
    public String serialize()
    {
        String ss = "";
        SlotSpns currentSpns = genericMap.get(SlotNumber);
        ss += ConfigJNI.Serializedouble(currentSpns.kPSpn, kP);
        ss += ConfigJNI.Serializedouble(currentSpns.kISpn, kI);
        ss += ConfigJNI.Serializedouble(currentSpns.kDSpn, kD);
        ss += ConfigJNI.Serializedouble(currentSpns.kSSpn, kS);
        ss += ConfigJNI.Serializedouble(currentSpns.kVSpn, kV);
        ss += ConfigJNI.Serializedouble(currentSpns.kASpn, kA);
        ss += ConfigJNI.Serializedouble(currentSpns.kGSpn, kG);
        ss += ConfigJNI.Serializeint(currentSpns.GravityTypeSpn, GravityType.value);
        ss += ConfigJNI.Serializeint(currentSpns.StaticFeedforwardSignSpn, StaticFeedforwardSign.value);
        return ss;
    }
}