/*
 * 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.mechanisms;

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

import edu.wpi.first.units.measure.*;
import static edu.wpi.first.units.Units.*;

public class DifferentialMotorConstants<MotorConfigsT extends ParentConfiguration> {
    /**
     * Sensor sources for a differential Pigeon 2.
     */
    public enum DifferentialPigeon2Source {
        /**
         * Use the yaw component of the Pigeon 2.
         */
        Yaw,
        /**
         * Use the pitch component of the Pigeon 2.
         */
        Pitch,
        /**
         * Use the roll component of the Pigeon 2.
         */
        Roll,
    }

    /**
     * Sensor sources for a differential CTR Electronics' CANdi™ branded device.
     */
    public enum DifferentialCANdiSource {
        /**
         * Use a pulse-width encoder remotely attached to the Sensor Input 1 (S1IN).
         */
        PWM1,
        /**
         * Use a pulse-width encoder remotely attached to the Sensor Input 2 (S2IN).
         */
        PWM2,
        /**
         * Use a quadrature encoder remotely attached to the two Sensor Inputs.
         */
        Quadrature,
    }

    /**
     * Name of the CAN bus the mechanism is on. Possible CAN bus strings are:
     * 
     * <ul>
     *   <li> "rio" for the native roboRIO CAN bus
     *   <li> CANivore name or serial number
     *   <li> SocketCAN interface (non-FRC Linux only)
     *   <li> "*" for any CANivore seen by the program
     *   <li> empty string (default) to select the default for the system:
     *   <ul>
     *     <li> "rio" on roboRIO
     *     <li> "can0" on Linux
     *     <li> "*" on Windows
     *   </ul>
     * </ul>
     * 
     * Note that all devices must be on the same CAN bus.
     */
    public String CANBusName = "";
    /**
     * CAN ID of the leader motor in the differential mechanism. The leader will
     * have the differential output added to its regular output.
     */
    public int LeaderId = 0;
    /**
     * CAN ID of the follower motor in the differential mechanism. The follower will
     * have the differential output subtracted from its regular output.
     */
    public int FollowerId = 0;
    /**
     * The alignment of the differential leader and follower motors, ignoring the
     * configured inverts.
     */
    public MotorAlignmentValue Alignment = MotorAlignmentValue.Aligned;
    /**
     * The ratio of sensor rotations to the differential mechanism's difference
     * output, where a ratio greater than 1 is a reduction.
     * <p>
     * When not using a separate sensor on the difference axis, the sensor is
     * considered half of the difference between the two motor controllers'
     * mechanism positions/velocities. As a result, this should be set to the gear
     * ratio on the difference axis in that scenario, or any gear ratio between the
     * sensor and the mechanism differential when using another sensor source.
     */
    public double SensorToDifferentialRatio = 1.0;
    /**
     * The update rate of the closed-loop controllers. This determines the update
     * rate of the differential leader's DifferentialOutput status signal, the
     * follower's Position and Velocity signals, and the relevant signals for any
     * other selected differential sensor.
     */
    public double ClosedLoopRate = 100;
    /**
     * The initial configs used to configure the differential leader. The default
     * value is the factory-default.
     * <p>
     * Users may change the initial configuration as they need. Any config that's
     * not referenced in the {@link DifferentialMotorConstants} class is available
     * to be changed.
     * <p>
     * The list of configs that will be overwritten is as follows:
     * 
     * <ul>
     *   <li> {@link DifferentialSensorsConfigs} (automatic based on the devices
     *        used)
     * </ul>
     * 
     */
    public MotorConfigsT LeaderInitialConfigs = null;
    /**
     * The initial configs used to configure the differential follower. The default
     * value is the factory-default.
     * <p>
     * Users may change the initial configuration as they need. Any config that's
     * not referenced in the {@link DifferentialMotorConstants} class is available
     * to be changed.
     * <p>
     * The list of configs that will be overwritten is as follows:
     * 
     * <ul>
     *   <li> {@link DifferentialSensorsConfigs} (factory defaulted)
     *   <li> {@link MotorOutputConfigs#Inverted} (determined from {@link
     *        #Alignment} and the {@link #LeaderInitialConfigs} invert)
     * </ul>
     * 
     * If {@link #FollowerUsesCommonLeaderConfigs} is set to true (default), the
     * following configs are copied from {@link #LeaderInitialConfigs}:
     * 
     * <ul>
     *   <li> {@link AudioConfigs}
     *   <li> {@link CurrentLimitsConfigs}
     *   <li> {@link MotorOutputConfigs} (except {@link
     *        MotorOutputConfigs#Inverted})
     *   <li> {@link TorqueCurrentConfigs}
     *   <li> {@link VoltageConfigs}
     * </ul>
     * 
     */
    public MotorConfigsT FollowerInitialConfigs = null;
    /**
     * Whether the follower should overwrite some of its initial configs with common
     * configs from the {@link #LeaderInitialConfigs}, such as current limits. The
     * list of configs that are copied is documented in {@link
     * #FollowerInitialConfigs}.
     */
    public boolean FollowerUsesCommonLeaderConfigs = true;
    
    /**
     * Modifies the CANBusName parameter and returns itself.
     * <p>
     * Name of the CAN bus the mechanism is on. Possible CAN bus strings are:
     * 
     * <ul>
     *   <li> "rio" for the native roboRIO CAN bus
     *   <li> CANivore name or serial number
     *   <li> SocketCAN interface (non-FRC Linux only)
     *   <li> "*" for any CANivore seen by the program
     *   <li> empty string (default) to select the default for the system:
     *   <ul>
     *     <li> "rio" on roboRIO
     *     <li> "can0" on Linux
     *     <li> "*" on Windows
     *   </ul>
     * </ul>
     * 
     * Note that all devices must be on the same CAN bus.
     *
     * @param newCANBusName Parameter to modify
     * @return this object
     */
    public DifferentialMotorConstants<MotorConfigsT> withCANBusName(String newCANBusName) {
        this.CANBusName = newCANBusName;
        return this;
    }
    
    /**
     * Modifies the LeaderId parameter and returns itself.
     * <p>
     * CAN ID of the leader motor in the differential mechanism. The leader will
     * have the differential output added to its regular output.
     *
     * @param newLeaderId Parameter to modify
     * @return this object
     */
    public DifferentialMotorConstants<MotorConfigsT> withLeaderId(int newLeaderId) {
        this.LeaderId = newLeaderId;
        return this;
    }
    
    /**
     * Modifies the FollowerId parameter and returns itself.
     * <p>
     * CAN ID of the follower motor in the differential mechanism. The follower will
     * have the differential output subtracted from its regular output.
     *
     * @param newFollowerId Parameter to modify
     * @return this object
     */
    public DifferentialMotorConstants<MotorConfigsT> withFollowerId(int newFollowerId) {
        this.FollowerId = newFollowerId;
        return this;
    }
    
    /**
     * Modifies the Alignment parameter and returns itself.
     * <p>
     * The alignment of the differential leader and follower motors, ignoring the
     * configured inverts.
     *
     * @param newAlignment Parameter to modify
     * @return this object
     */
    public DifferentialMotorConstants<MotorConfigsT> withAlignment(MotorAlignmentValue newAlignment) {
        this.Alignment = newAlignment;
        return this;
    }
    
    /**
     * Modifies the SensorToDifferentialRatio parameter and returns itself.
     * <p>
     * The ratio of sensor rotations to the differential mechanism's difference
     * output, where a ratio greater than 1 is a reduction.
     * <p>
     * When not using a separate sensor on the difference axis, the sensor is
     * considered half of the difference between the two motor controllers'
     * mechanism positions/velocities. As a result, this should be set to the gear
     * ratio on the difference axis in that scenario, or any gear ratio between the
     * sensor and the mechanism differential when using another sensor source.
     *
     * @param newSensorToDifferentialRatio Parameter to modify
     * @return this object
     */
    public DifferentialMotorConstants<MotorConfigsT> withSensorToDifferentialRatio(double newSensorToDifferentialRatio) {
        this.SensorToDifferentialRatio = newSensorToDifferentialRatio;
        return this;
    }
    
    /**
     * Modifies the ClosedLoopRate parameter and returns itself.
     * <p>
     * The update rate of the closed-loop controllers. This determines the update
     * rate of the differential leader's DifferentialOutput status signal, the
     * follower's Position and Velocity signals, and the relevant signals for any
     * other selected differential sensor.
     *
     * @param newClosedLoopRate Parameter to modify
     * @return this object
     */
    public DifferentialMotorConstants<MotorConfigsT> withClosedLoopRate(double newClosedLoopRate) {
        this.ClosedLoopRate = newClosedLoopRate;
        return this;
    }
    
    /**
     * Modifies the ClosedLoopRate parameter and returns itself.
     * <p>
     * The update rate of the closed-loop controllers. This determines the update
     * rate of the differential leader's DifferentialOutput status signal, the
     * follower's Position and Velocity signals, and the relevant signals for any
     * other selected differential sensor.
     *
     * @param newClosedLoopRate Parameter to modify
     * @return this object
     */
    public DifferentialMotorConstants<MotorConfigsT> withClosedLoopRate(Frequency newClosedLoopRate) {
        this.ClosedLoopRate = newClosedLoopRate.in(Hertz);
        return this;
    }
    
    /**
     * Modifies the LeaderInitialConfigs parameter and returns itself.
     * <p>
     * The initial configs used to configure the differential leader. The default
     * value is the factory-default.
     * <p>
     * Users may change the initial configuration as they need. Any config that's
     * not referenced in the {@link DifferentialMotorConstants} class is available
     * to be changed.
     * <p>
     * The list of configs that will be overwritten is as follows:
     * 
     * <ul>
     *   <li> {@link DifferentialSensorsConfigs} (automatic based on the devices
     *        used)
     * </ul>
     * 
     *
     * @param newLeaderInitialConfigs Parameter to modify
     * @return this object
     */
    public DifferentialMotorConstants<MotorConfigsT> withLeaderInitialConfigs(MotorConfigsT newLeaderInitialConfigs) {
        this.LeaderInitialConfigs = newLeaderInitialConfigs;
        return this;
    }
    
    /**
     * Modifies the FollowerInitialConfigs parameter and returns itself.
     * <p>
     * The initial configs used to configure the differential follower. The default
     * value is the factory-default.
     * <p>
     * Users may change the initial configuration as they need. Any config that's
     * not referenced in the {@link DifferentialMotorConstants} class is available
     * to be changed.
     * <p>
     * The list of configs that will be overwritten is as follows:
     * 
     * <ul>
     *   <li> {@link DifferentialSensorsConfigs} (factory defaulted)
     *   <li> {@link MotorOutputConfigs#Inverted} (determined from {@link
     *        #Alignment} and the {@link #LeaderInitialConfigs} invert)
     * </ul>
     * 
     * If {@link #FollowerUsesCommonLeaderConfigs} is set to true (default), the
     * following configs are copied from {@link #LeaderInitialConfigs}:
     * 
     * <ul>
     *   <li> {@link AudioConfigs}
     *   <li> {@link CurrentLimitsConfigs}
     *   <li> {@link MotorOutputConfigs} (except {@link
     *        MotorOutputConfigs#Inverted})
     *   <li> {@link TorqueCurrentConfigs}
     *   <li> {@link VoltageConfigs}
     * </ul>
     * 
     *
     * @param newFollowerInitialConfigs Parameter to modify
     * @return this object
     */
    public DifferentialMotorConstants<MotorConfigsT> withFollowerInitialConfigs(MotorConfigsT newFollowerInitialConfigs) {
        this.FollowerInitialConfigs = newFollowerInitialConfigs;
        return this;
    }
    
    /**
     * Modifies the FollowerUsesCommonLeaderConfigs parameter and returns itself.
     * <p>
     * Whether the follower should overwrite some of its initial configs with common
     * configs from the {@link #LeaderInitialConfigs}, such as current limits. The
     * list of configs that are copied is documented in {@link
     * #FollowerInitialConfigs}.
     *
     * @param newFollowerUsesCommonLeaderConfigs Parameter to modify
     * @return this object
     */
    public DifferentialMotorConstants<MotorConfigsT> withFollowerUsesCommonLeaderConfigs(boolean newFollowerUsesCommonLeaderConfigs) {
        this.FollowerUsesCommonLeaderConfigs = newFollowerUsesCommonLeaderConfigs;
        return this;
    }
}