package com.team364.frc2020;

import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Units;

import com.team364.frc2020.States.*;
import static com.team364.lib.math.Boundaries.*;
import static com.team364.lib.math.Conversions.*;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.CANCoderConfiguration;

public class SwerveMod {
    public int moduleNumber;
    private double offset;
    private TalonFX mAngleMotor;
    private TalonFX mDriveMotor;
    private CANCoder angleEncoder;
    private TalonFXConfiguration angleMotorConfig;
    private TalonFXConfiguration driveMotorConfig;
    private CANCoderConfiguration canCoderConfig;
    private boolean invertDrive;
    private boolean invertAngle;
    private double lastAngle;

    public SwerveMod(int moduleNumber, double offset, int angleMotorID, int angleEncoderID, int driveMotorID,
    boolean invertDrive, boolean invertAngle, TalonFXConfiguration angleMotorConfig, TalonFXConfiguration driveMotorConfig, CANCoderConfiguration canCoderConfig){
        this.moduleNumber = moduleNumber;
        this.offset = offset;
        this.angleMotorConfig = angleMotorConfig;
        this.driveMotorConfig = driveMotorConfig;
        this.canCoderConfig = canCoderConfig;
        this.invertDrive = invertDrive;
        this.invertAngle = invertAngle;

        /* Angle Encoder Config */
        angleEncoder = new CANCoder(angleEncoderID);
        configAngleEncoder();

        /* Angle Motor Config */
        mAngleMotor = new TalonFX(angleMotorID);
        configAngleMotor();

        /* Drive Motor Config */
        mDriveMotor = new TalonFX(driveMotorID);
        configDriveMotor();

        lastAngle = getAngle();
    }

    public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop){
        SwerveModuleState state = desiredState;
        state = SwerveModuleState.optimize(desiredState, newGetAngle());

        if(isOpenLoop){
            double percentOutput = state.speedMetersPerSecond / Constants.Swerve.maxSpeed;
            percentOutput = (States.driveState == DriveStates.openLoopDampened) ? (percentOutput *= Constants.Swerve.driveDampenRatio) : percentOutput;
            mDriveMotor.set(ControlMode.PercentOutput, 0);
            SmartDashboard.putNumber("percentOutput " + moduleNumber + ": ", percentOutput);
        }
        else {
            mDriveMotor.set(ControlMode.Velocity, toFalconFromMetersPerSecond(state.speedMetersPerSecond));
        }

        double angle = (Math.abs(state.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.1)) ? lastAngle : state.angle.getDegrees();
        mAngleMotor.set(ControlMode.Position, toFalconFromDegrees(angle, Constants.Swerve.angleGearRatio));
        lastAngle = angle;

        SmartDashboard.putNumber("sAngle " + moduleNumber + ": ", angle);

        SmartDashboard.putNumber("sCAngle " + moduleNumber + ": ", getAngle());
    }

    private void resetToAbsolute(){
        int absolutePosition = toFalconFromDegrees(getCanCoder() + offset, Constants.Swerve.angleGearRatio);
        mAngleMotor.setSelectedSensorPosition(absolutePosition);
    }

    private void configAngleEncoder(){
        angleEncoder.configFactoryDefault();
        angleEncoder.configAllSettings(canCoderConfig);
    }

    private void configAngleMotor(){
        mAngleMotor.configFactoryDefault();
        mAngleMotor.configAllSettings(angleMotorConfig, Constants.ctreTimeout);
        mAngleMotor.setInverted(invertAngle);
        mAngleMotor.setNeutralMode(Constants.Swerve.angleNeutralMode);
        mAngleMotor.setStatusFramePeriod(StatusFrame.Status_1_General, Constants.Swerve.angleStatus1, Constants.ctreTimeout);
        mAngleMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, Constants.Swerve.angleStatus2, Constants.ctreTimeout);
        resetToAbsolute();
    }

    private void configDriveMotor(){
        mDriveMotor.configFactoryDefault();
        mDriveMotor.configAllSettings(driveMotorConfig);
        mDriveMotor.setInverted(invertDrive);
        mDriveMotor.setNeutralMode(Constants.Swerve.driveNeutralMode);
        mDriveMotor.setStatusFramePeriod(StatusFrame.Status_1_General, Constants.Swerve.driveStatus1);
        mDriveMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, Constants.Swerve.driveStatus2);
    }

    public double getCanCoder(){
        return angleEncoder.getAbsolutePosition();
    }

    public double getAngle(){
        return toDegreesFromFalcon(mAngleMotor.getSelectedSensorPosition(), Constants.Swerve.angleGearRatio);
    }

    public Rotation2d newGetAngle() {
        Rotation2d angle = Rotation2d.fromDegrees(
                toDegreesFromFalcon(mAngleMotor.getSelectedSensorPosition(), Constants.Swerve.angleGearRatio) % 360)
                .rotateBy(new Rotation2d());
        return angle;
    }

    public double getVelocity(){
        double wheelRPM = toRPMFromFalcon(mDriveMotor.getSelectedSensorVelocity(), Constants.Swerve.driveGearRatio);
        double wheelIPS = (wheelRPM * Constants.Swerve.wheelCircumference) / 60; //Inches per Second
        double wheelMPS = Units.inchesToMeters(wheelIPS); //Meters per Second
        return wheelMPS;
    }

    public SwerveModuleState getState(){
        return new SwerveModuleState(getVelocity(), Rotation2d.fromDegrees(getAngle()));
    }

}