###################################################################################
# MIT License
#
# Copyright (c) PhotonVision
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
###################################################################################

import math

import wpilib
import wpilib.simulation
import wpimath.controller
import wpimath.filter
import wpimath.geometry
import wpimath.kinematics
import wpimath.trajectory
import wpimath.units

kWheelRadius = 0.0508
kEncoderResolution = 4096
kModuleMaxAngularVelocity = math.pi
kModuleMaxAngularAcceleration = math.tau


class SwerveModule:
    def __init__(
        self,
        driveMotorChannel: int,
        turningMotorChannel: int,
        driveEncoderChannelA: int,
        driveEncoderChannelB: int,
        turningEncoderChannelA: int,
        turningEncoderChannelB: int,
        moduleNumber: int,
    ) -> None:
        """Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder.

        :param driveMotorChannel:      PWM output for the drive motor.
        :param turningMotorChannel:    PWM output for the turning motor.
        :param driveEncoderChannelA:   DIO input for the drive encoder channel A
        :param driveEncoderChannelB:   DIO input for the drive encoder channel B
        :param turningEncoderChannelA: DIO input for the turning encoder channel A
        :param turningEncoderChannelB: DIO input for the turning encoder channel B
        """
        self.moduleNumber = moduleNumber
        self.desiredState = wpimath.kinematics.SwerveModuleState()

        self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
        self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)

        self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB)
        self.turningEncoder = wpilib.Encoder(
            turningEncoderChannelA, turningEncoderChannelB
        )

        # Gains are for example purposes only - must be determined for your own robot!
        self.drivePIDController = wpimath.controller.PIDController(10, 0, 0)

        # Gains are for example purposes only - must be determined for your own robot!
        self.turningPIDController = wpimath.controller.PIDController(30, 0, 0)

        # Gains are for example purposes only - must be determined for your own robot!
        self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3)
        self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7)

        # Set the distance per pulse for the drive encoder. We can simply use the
        # distance traveled for one rotation of the wheel divided by the encoder
        # resolution.
        self.driveEncoder.setDistancePerPulse(
            math.tau * kWheelRadius / kEncoderResolution
        )

        # Set the distance (in this case, angle) in radians per pulse for the turning encoder.
        # This is the the angle through an entire rotation (2 * pi) divided by the
        # encoder resolution.
        self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution)

        # Limit the PID Controller's input range between -pi and pi and set the input
        # to be continuous.
        self.turningPIDController.enableContinuousInput(-math.pi, math.pi)

        # Simulation Support
        self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder)
        self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder)
        self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor)
        self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor)
        self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
            0.1, 0.02
        )
        self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
            0.0001, 0.02
        )
        self.simTurningEncoderPos = 0
        self.simDrivingEncoderPos = 0

    def getState(self) -> wpimath.kinematics.SwerveModuleState:
        """Returns the current state of the module.

        :returns: The current state of the module.
        """
        return wpimath.kinematics.SwerveModuleState(
            self.driveEncoder.getRate(),
            wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
        )

    def getPosition(self) -> wpimath.kinematics.SwerveModulePosition:
        """Returns the current position of the module.

        :returns: The current position of the module.
        """
        return wpimath.kinematics.SwerveModulePosition(
            self.driveEncoder.getDistance(),
            wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
        )

    def setDesiredState(
        self, desiredState: wpimath.kinematics.SwerveModuleState
    ) -> None:
        """Sets the desired state for the module.

        :param desiredState: Desired state with speed and angle.
        """
        self.desiredState = desiredState

        encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())

        # Optimize the reference state to avoid spinning further than 90 degrees
        state = wpimath.kinematics.SwerveModuleState.optimize(
            self.desiredState, encoderRotation
        )

        # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
        # direction of travel that can occur when modules change directions. This results in smoother
        # driving.
        state.speed *= (state.angle - encoderRotation).cos()

        # Calculate the drive output from the drive PID controller.
        driveOutput = self.drivePIDController.calculate(
            self.driveEncoder.getRate(), state.speed
        )

        driveFeedforward = self.driveFeedforward.calculate(state.speed)

        # Calculate the turning motor output from the turning PID controller.
        turnOutput = self.turningPIDController.calculate(
            self.turningEncoder.getDistance(), state.angle.radians()
        )

        turnFeedforward = self.turnFeedforward.calculate(
            self.turningPIDController.getSetpoint()
        )

        self.driveMotor.setVoltage(driveOutput + driveFeedforward)
        self.turningMotor.setVoltage(turnOutput + turnFeedforward)

    def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d:
        return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())

    def log(self) -> None:
        state = self.getState()

        table = "Module " + str(self.moduleNumber) + "/"
        wpilib.SmartDashboard.putNumber(
            table + "Steer Degrees",
            math.degrees(wpimath.angleModulus(state.angle.radians())),
        )
        wpilib.SmartDashboard.putNumber(
            table + "Steer Target Degrees",
            math.degrees(self.turningPIDController.getSetpoint()),
        )
        wpilib.SmartDashboard.putNumber(table + "Drive Velocity Feet", state.speed_fps)
        wpilib.SmartDashboard.putNumber(
            table + "Drive Velocity Target Feet", self.desiredState.speed_fps
        )
        wpilib.SmartDashboard.putNumber(
            table + "Drive Voltage", self.driveMotor.get() * 12.0
        )
        wpilib.SmartDashboard.putNumber(
            table + "Steer Voltage", self.turningMotor.get() * 12.0
        )

    def simulationPeriodic(self) -> None:
        driveVoltage = (
            self.simDrivingMotor.getSpeed() * wpilib.RobotController.getBatteryVoltage()
        )
        turnVoltage = (
            self.simTurningMotor.getSpeed() * wpilib.RobotController.getBatteryVoltage()
        )
        driveSpdRaw = (
            driveVoltage / 12.0 * self.driveFeedforward.maxAchievableVelocity(12.0, 0)
        )
        turnSpdRaw = (
            turnVoltage / 12.0 * self.turnFeedforward.maxAchievableVelocity(12.0, 0)
        )
        driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw)
        turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw)
        self.simDrivingEncoderPos += 0.02 * driveSpd
        self.simTurningEncoderPos += 0.02 * turnSpd
        self.simDriveEncoder.setDistance(self.simDrivingEncoderPos)
        self.simDriveEncoder.setRate(driveSpd)
        self.simTurningEncoder.setDistance(self.simTurningEncoderPos)
        self.simTurningEncoder.setRate(turnSpd)
