package frc.robot;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class SwerveModule {

    private TalonSRX steer;
    private CANSparkMax drive;

    private String name;

    private double offset;

    public SwerveModule(int steerPort, int drivePort, String name, double offset) {
        this.steer = new TalonSRX(steerPort);
        this.drive = new CANSparkMax(drivePort, MotorType.kBrushless);
        this.name = name;
        this.offset = offset / 360.0 * 4096;
        configure();
    }

    public void configure() {

        int kTimeoutMs = 10;
        int kPIDLoopIdx = 0;
        int rampvelocity = 15000;
        int rampaccel = 16000;
        int absolutePosition = steer.getSelectedSensorPosition(kTimeoutMs) & 0xFFF;
        int P = 3;
        int I = 0;
        int D = 0;
        int F = 0;

        steer.setSelectedSensorPosition(absolutePosition, kPIDLoopIdx, kTimeoutMs);
        steer.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, kPIDLoopIdx, kTimeoutMs);
        steer.setSensorPhase(true);
        steer.setInverted(false);

        steer.selectProfileSlot(0, kPIDLoopIdx);
        steer.configNominalOutputForward(0, kTimeoutMs);
        steer.configNominalOutputReverse(0, kTimeoutMs);
        steer.configPeakOutputForward(1, kTimeoutMs);
        steer.configPeakOutputReverse(-1, kTimeoutMs);
        steer.configAllowableClosedloopError(0, kPIDLoopIdx, kTimeoutMs);
        steer.configMotionCruiseVelocity(rampvelocity, kTimeoutMs);
        steer.configMotionAcceleration(rampaccel, kTimeoutMs);

        steer.config_kF(kPIDLoopIdx, F, kTimeoutMs);
        steer.config_kP(kPIDLoopIdx, P, kTimeoutMs);
        steer.config_kI(kPIDLoopIdx, I, kTimeoutMs);
        steer.config_kD(kPIDLoopIdx, D, kTimeoutMs);
    }


    public void setAngle(double angle){
        steer.set(ControlMode.MotionMagic,  ( angle / 360 * 4096 ) );
    }

    public void stop() {
        steer.set(ControlMode.PercentOutput, 0);
        drive.set(0);
    }

    public double getAngle() {
        double angle = steer.getSelectedSensorPosition(0) / (4096.0 / 360.0);
        SmartDashboard.putNumber(name + " Angle", angle);
        return angle;
    }

    /**
     * @angle in degrees
     * @speed in percent output
     */
    public void drive(double speed, double angle) {
        drive.set(speed);
        setAngle(angle);
    }

    public void reset() {
        steer.setSelectedSensorPosition(0);
    }
}