#
# This file is autogenerated, do not edit manually. Instead,
# edit gen/data.yml
#

import typing

import hal
from hal_impl.data import hal_data, NotifyDict

from .sim_enums import *


class CANDigitalInput:
    def __init__(self, device: 'CANSparkMax', limitSwitch: LimitSwitch, polarity: LimitSwitchPolarity) -> None:
        """Constructs a CANDigitalInput.

        :param device: The Spark Max to which the limit switch is attached.

        :param limitSwitch: Whether this is forward or reverse limit switch.

        :param polarity: Whether the limit switch is normally open or normally
                   closed.
        """
        assert isinstance(device, CANSparkMax)
        assert isinstance(limitSwitch, LimitSwitch)
        assert isinstance(polarity, LimitSwitchPolarity)
        self._device = device
        self._limitSwitch = limitSwitch
        polarity_param = (
            "limitSwitchFwdPolarity"
            if limitSwitch is LimitSwitch.kForward
            else "limitSwitchRevPolarity"
        )
        device._hal_data[polarity_param] = polarity

    def get(self) -> bool:
        """Get the value from a digital input channel.

        Retrieve the value of a single digital input channel from a motor
        controller. This method will return the state of the limit input
        based on the selected polarity, whether or not it is enabled.
        """
        retval = self._device.getFault(
            FaultID.kHardLimitFwd
            if self._limitSwitch is LimitSwitch.kForward
            else FaultID.kHardLimitRev
        )
        assert isinstance(retval, bool)
        return retval

    def enableLimitSwitch(self, enable: bool) -> CANError:
        """Enables or disables controller shutdown based on limit switch.
        """
        assert isinstance(enable, bool)
        retval = CANError.kOK
        param = (
            "hardLimitFwdEn"
            if self._limitSwitch is LimitSwitch.kForward
            else "hardLimitRevEn"
        )
        self._device._hal_data[param] = enable
        return retval

    def isLimitSwitchEnabled(self) -> bool:
        """Returns true if limit switch is enabled.
        """
        param = (
            "hardLimitFwdEn"
            if self._limitSwitch is LimitSwitch.kForward
            else "hardLimitRevEn"
        )
        retval = self._device._hal_data[param]
        assert isinstance(retval, bool)
        return retval


class CANEncoder:
    def __init__(self, device: 'CANSparkMax') -> None:
        """Constructs a CANPIDController.

        :param device: The Spark Max to which the encoder is attached.
        """
        assert isinstance(device, CANSparkMax)
        self._device = device
        self._hal_data = device._hal_data

    def getPosition(self) -> float:
        """Get the position of the motor. This returns the native units
        of 'rotations'.

        :returns: Number of rotations of the motor
        """
        retval = self._hal_data['position']
        assert isinstance(retval, float)
        return retval

    def getVelocity(self) -> float:
        """Get the velocity of the motor. This returns the native units
        of 'RPM'.

        :returns: Number the RPM of the motor
        """
        retval = self._hal_data['velocity']
        assert isinstance(retval, float)
        return retval


class CANPIDController:
    def __init__(self, device: 'CANSparkMax') -> None:
        """Constructs a CANPIDController.

        :param device: The Spark Max this object configures.
        """
        assert isinstance(device, CANSparkMax)
        self._device = device
        self._hal_data = device._hal_data

    def setReference(self, value: float, ctrl: ControlType, pidSlot: int = 0, arbFeedforward: float = 0) -> CANError:
        """Set the controller reference value based on the selected control mode.

        :param value: The value to set depending on the control mode. For basic
                      duty cycle control this should be a value between -1 and 1
                      Otherwise: Voltage Control: Voltage (volts) Velocity Control: Velocity
                      (RPM) Position Control: Position (Rotations) Current Control: Current
                      (Amps)

        :param ctrl:  Is the control type

        :param pidSlot: for this command

        :param arbFeedforward: A value from -32.0 to 32.0 which is a voltage
                      applied to the motor after the result of the specified control mode. The
                      units for the parameter is Volts. This value is set after the control mode,
                      but before any current limits or ramp rates.

        :returns: CANError Set to REV_OK if successful
        """
        assert isinstance(value, (int, float))
        assert isinstance(pidSlot, int)
        assert isinstance(arbFeedforward, (int, float))
        retval = CANError.kOK
        self._hal_data["value"] = float(value)  # TODO
        self._hal_data["ctrlType"] = ctrl
        self._hal_data["pid_slot"] = pidSlot
        self._hal_data["arb_ff"] = arbFeedforward
        return retval

    def setP(self, gain: float, slotID: int = 0) -> CANError:
        """Set the Proportional Gain constant of the PIDF controller on the SPARK
        MAX. This uses the Set Parameter API and should be used infrequently. The
        parameter does not presist unless burnFlash() is called.  The recommended
        method to configure this parameter is use to SPARK MAX GUI to tune and
        save parameters.

        :param gain: The proportional gain value, must be positive

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: CANError Set to REV_OK if successful
        """
        assert isinstance(gain, (int, float))
        assert isinstance(slotID, int)
        retval = CANError.kOK
        assert 0 <= slotID <= 3
        self._hal_data["p_%d" % slotID] = float(gain)
        return retval

    def setI(self, gain: float, slotID: int = 0) -> CANError:
        """Set the Integral Gain constant of the PIDF controller on the SPARK MAX.
        This uses the Set Parameter API and should be used infrequently. The
        parameter does not presist unless burnFlash() is called.  The recommended
        method to configure this parameter is use to SPARK MAX GUI to tune and
        save parameters.

        :param gain: The integral gain value, must be positive

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: CANError Set to REV_OK if successful
        """
        assert isinstance(gain, (int, float))
        assert isinstance(slotID, int)
        retval = CANError.kOK
        assert 0 <= slotID <= 3
        self._hal_data["i_%d" % slotID] = float(gain)
        return retval

    def setD(self, gain: float, slotID: int = 0) -> CANError:
        """Set the Derivative Gain constant of the PIDF controller on the SPARK MAX.
        This uses the Set Parameter API and should be used infrequently. The
        parameter does not presist unless burnFlash() is called.  The recommended
        method to configure this parameter is use to SPARK MAX GUI to tune and
        save parameters.

        :param gain: The derivative gain value, must be positive

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: CANError Set to REV_OK if successful
        """
        assert isinstance(gain, (int, float))
        assert isinstance(slotID, int)
        retval = CANError.kOK
        assert 0 <= slotID <= 3
        self._hal_data["d_%d" % slotID] = float(gain)
        return retval

    def setFF(self, gain: float, slotID: int = 0) -> CANError:
        """Set the Feed-froward Gain constant of the PIDF controller on the SPARK
        MAX. This uses the Set Parameter API and should be used infrequently. The
        parameter does not presist unless burnFlash() is called.  The recommended
        method to configure this parameter is use to SPARK MAX GUI to tune and
        save parameters.

        :param gain: The feed-forward gain value

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: CANError Set to REV_OK if successful
        """
        assert isinstance(gain, (int, float))
        assert isinstance(slotID, int)
        retval = CANError.kOK
        assert 0 <= slotID <= 3
        self._hal_data["f_%d" % slotID] = float(gain)
        return retval

    def setIZone(self, IZone: float, slotID: int = 0) -> CANError:
        """Set the IZone range of the PIDF controller on the SPARK MAX. This value
        specifies the range the |error| must be within for the integral constant
        to take effect.

        This uses the Set Parameter API and should be used infrequently.
        The parameter does not presist unless burnFlash() is called.
        The recommended method to configure this parameter is to use the
        SPARK MAX GUI to tune and save parameters.

        :param gain: The IZone value, must be positive. Set to 0 to disable

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: CANError Set to REV_OK if successful
        """
        assert isinstance(IZone, (int, float))
        assert isinstance(slotID, int)
        retval = CANError.kOK
        assert 0 <= slotID <= 3
        self._hal_data["iZone_%d" % slotID] = float(IZone)
        return retval

    def setOutputRange(self, min: float, max: float, slotID: int = 0) -> CANError:
        """Set the min amd max output for the closed loop mode.

        This uses the Set Parameter API and should be used infrequently.
        The parameter does not presist unless burnFlash() is called.
        The recommended method to configure this parameter is to use the
        SPARK MAX GUI to tune and save parameters.

        :param min: Reverse power minimum to allow the controller to output

        :param max: Forward power maximum to allow the controller to output

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: CANError Set to REV_OK if successful
        """
        assert isinstance(min, (int, float))
        assert isinstance(max, (int, float))
        assert isinstance(slotID, int)
        retval = CANError.kOK
        self._hal_data["outputMin_%d" % slotID] = float(min)
        self._hal_data["outputMax_%d" % slotID] = float(max)
        return retval

    def getP(self, slotID: int = 0) -> float:
        """Get the Proportional Gain constant of the PIDF controller on the SPARK
        MAX.

        This uses the Get Parameter API and should be used infrequently. This
        function uses a non-blocking call and will return a cached value if the
        parameter is not returned by the timeout. The timeout can be changed by
        calling SetCANTimeout(int milliseconds)

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: double P Gain value
        """
        assert isinstance(slotID, int)
        assert 0 <= slotID <= 3
        retval = self._hal_data["p_%d" % slotID]
        assert isinstance(retval, float)
        return retval

    def getI(self, slotID: int = 0) -> float:
        """Get the Integral Gain constant of the PIDF controller on the SPARK MAX.

        This uses the Get Parameter API and should be used infrequently. This
        function uses a non-blocking call and will return a cached value if the
        parameter is not returned by the timeout. The timeout can be changed by
        calling SetCANTimeout(int milliseconds)

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: double I Gain value
        """
        assert isinstance(slotID, int)
        assert 0 <= slotID <= 3
        retval = self._hal_data["i_%d" % slotID]
        assert isinstance(retval, float)
        return retval

    def getD(self, slotID: int = 0) -> float:
        """Get the Derivative Gain constant of the PIDF controller on the SPARK MAX.

        This uses the Get Parameter API and should be used infrequently. This
        function uses a non-blocking call and will return a cached value if the
        parameter is not returned by the timeout. The timeout can be changed by
        calling SetCANTimeout(int milliseconds)

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: double D Gain value
        """
        assert isinstance(slotID, int)
        assert 0 <= slotID <= 3
        retval = self._hal_data["d_%d" % slotID]
        assert isinstance(retval, float)
        return retval

    def getFF(self, slotID: int = 0) -> float:
        """Get the Feed-forward Gain constant of the PIDF controller on the SPARK
        MAX.

        This uses the Get Parameter API and should be used infrequently. This
        function uses a non-blocking call and will return a cached value if the
        parameter is not returned by the timeout. The timeout can be changed by
        calling SetCANTimeout(int milliseconds)

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: double F Gain value
        """
        assert isinstance(slotID, int)
        assert 0 <= slotID <= 3
        retval = self._hal_data["f_%d" % slotID]
        assert isinstance(retval, float)
        return retval

    def getIZone(self, slotID: int = 0) -> float:
        """Get the IZone constant of the PIDF controller on the SPARK MAX.

        This uses the Get Parameter API and should be used infrequently. This
        function uses a non-blocking call and will return a cached value if the
        parameter is not returned by the timeout. The timeout can be changed by
        calling SetCANTimeout(int milliseconds)

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: double IZone value
        """
        assert isinstance(slotID, int)
        assert 0 <= slotID <= 3
        retval = self._hal_data["iZone_%d" % slotID]
        assert isinstance(retval, float)
        return retval

    def getOutputMin(self, slotID: int = 0) -> float:
        """Get the min output of the PIDF controller on the SPARK MAX.

        This uses the Get Parameter API and should be used infrequently. This
        function uses a non-blocking call and will return a cached value if the
        parameter is not returned by the timeout. The timeout can be changed by
        calling SetCANTimeout(int milliseconds)

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: double min value
        """
        assert isinstance(slotID, int)
        assert 0 <= slotID <= 3
        retval = self._hal_data["outputMin_%d" % slotID]
        assert isinstance(retval, float)
        return retval

    def getOutputMax(self, slotID: int = 0) -> float:
        """Get the max output of the PIDF controller on the SPARK MAX.

        This uses the Get Parameter API and should be used infrequently. This
        function uses a non-blocking call and will return a cached value if the
        parameter is not returned by the timeout. The timeout can be changed by
        calling SetCANTimeout(int milliseconds)

        :param slotID: Is the gain schedule slot, the value is a number
              between 0 and 3. Each slot has its own set of gain values and
              can be changed in each control frame using SetReference().

        :returns: double max value
        """
        assert isinstance(slotID, int)
        assert 0 <= slotID <= 3
        retval = self._hal_data["outputMax_%d" % slotID]
        assert isinstance(retval, float)
        return retval


class CANSparkMax:
    def __init__(self, deviceID: int, type: MotorType) -> None:
        """Create a new SPARK MAX Controller

        :param deviceID: The device ID.

        :param type: The motor type connected to the controller. Brushless
                motors must be connected to their matching color and the
                hall sensor plugged in. Brushed motors must be connected
                to the Red and Black terminals only.
        """
        assert isinstance(deviceID, int)
        # super().__init__(deviceID, type)
        self._faults = NotifyDict({})  # type: typing.Dict[FaultID, bool]
        # camel case for stored parameters (see CANSparkMaxLowLevel::ConfigParameter)
        self._hal_data = NotifyDict({
            "motorType": type,
            "can_timeout": 20,
            "value": 0.0,
            "inverted": False,
            "position": 0.0,
            "velocity": 0.0,
            "ctrlType": ControlType.kDutyCycle,
            "pid_slot": 0,
            "arb_ff": 0.0,
            "faults": self._faults,
            "limitSwitchFwdPolarity": LimitSwitchPolarity.kNormallyOpen,
            "limitSwitchRevPolarity": LimitSwitchPolarity.kNormallyOpen,
            "hardLimitFwdEn": False,
            "hardLimitRevEn": False,
            "currentChop": 60.0,  # TODO
            "currentChopCycles": 0,
            "idleMode": IdleMode.kCoast,
            "rampRate": 0.0,
            "bus_voltage": 12.0,
            "output_current": 0.1,
            "motor_temp": 25.0,  # ~room temperature
        })
        for i in range(4):
            self._hal_data.update({
                "p_%d" % i: 0.0,
                "i_%d" % i: 0.0,
                "d_%d" % i: 0.0,
                "f_%d" % i: 0.0,
                "iZone_%d" % i: 0.0,
                "outputMin_%d" % i: 0.0,
                "outputMax_%d" % i: 0.0,
            })
        hal_data["CAN"]["sparkmax-%d" % deviceID] = self._hal_data
        hal.report(hal.UsageReporting.kResourceType_RevSparkMaxCAN, deviceID)
        self.clearFaults()

    def set(self, speed: float) -> None:
        """Common interface for setting the speed of a speed controller.

        :param speed: The speed to set.  Value should be between -1.0 and 1.0.
        """
        assert isinstance(speed, (int, float))
        self._hal_data["value"] = float(speed)
        self._hal_data["ctrlType"] = ControlType.kDutyCycle
        self._hal_data["pid_slot"] = 0
        self._hal_data["arb_ff"] = 0.0

    def get(self) -> float:
        """Common interface for getting the current set speed of a speed controller.

        :returns: The current set speed.  Value is between -1.0 and 1.0.
        """
        retval = self._hal_data['value']
        assert isinstance(retval, float)
        return retval

    def setInverted(self, isInverted: bool) -> None:
        """Common interface for inverting direction of a speed controller.
        This has no effect if the controller is a follower.

        :param isInverted: The state of inversion, true is inverted.
        """
        assert isinstance(isInverted, bool)
        self._hal_data['inverted'] = isInverted

    def getInverted(self) -> bool:
        """Common interface for returning the inversion state of a speed controller.

        :returns: isInverted The state of inversion, true is inverted.
        """
        retval = self._hal_data['inverted']
        assert isinstance(retval, bool)
        return retval

    def disable(self) -> None:
        """Common interface for disabling a motor.
        """
        self.set(0)

    def stopMotor(self) -> None:
        """Common interface to stop the motor until Set is called again.
        """
        self.disable()

    def pidWrite(self, output: float) -> None:
        assert isinstance(output, (int, float))
        self.set(output)

    def getEncoder(self) -> CANEncoder:
        """Returns and object for interfacing with the integrated encoder.
        """
        retval = CANEncoder(self)
        return retval

    def getPIDController(self) -> CANPIDController:
        """Returns an object for interfacing with the integrated PID controller.
        """
        retval = CANPIDController(self)
        return retval

    def getForwardLimitSwitch(self, polarity: LimitSwitchPolarity) -> CANDigitalInput:
        """Returns an object for interfacing with the integrated forward limit
        switch.

        :param polarity: Whether the limit switch is normally open or normally
                closed.
        """
        retval = CANDigitalInput(self, LimitSwitch.kForward, polarity)
        return retval

    def getReverseLimitSwitch(self, polarity: LimitSwitchPolarity) -> CANDigitalInput:
        """Returns an object for interfacing with the integrated reverse limit
        switch.

        :param polarity: Whether the limit switch is normally open or normally
                closed.
        """
        retval = CANDigitalInput(self, LimitSwitch.kReverse, polarity)
        return retval

    @typing.overload
    def setSmartCurrentLimit(self, limit: int) -> CANError:
        ...

    @typing.overload
    def setSmartCurrentLimit(self, stallLimit: int, freeLimit: int, limitRPM: int = 20000) -> CANError:
        ...

    def setSecondaryCurrentLimit(self, limit: float, limitCycles: int = 0) -> CANError:
        """Sets the secondary current limit in Amps.

        The motor controller will disable the output of the controller briefly
        if the current limit is exceeded to reduce the current. This limit is
        a simplified 'on/off' controller. This limit is enabled by default
        but is set higher than the default Smart Current Limit.

        The time the controller is off after the current limit is reached
        is determined by the parameter limitCycles, which is the number of
        PWM cycles (20kHz). The recommended value is the default of 0 which
        is the minimum time and is part of a PWM cycle from when the over
        current is detected. This allows the controller to regulate the current
        close to the limit value.

        The total time is set by the equation::

          t = (50us - t0) + 50us * limitCycles
          t = total off time after over current
          t0 = time from the start of the PWM cycle until over current is detected

        :param limit: The current limit in Amps.
        :param limitCycles: The number of additional PWM cycles to turn
                            the driver off after overcurrent is detected.

        """
        assert isinstance(limit, (int, float))
        assert isinstance(limitCycles, int)
        retval = CANError.kOK
        self._hal_data["currentChop"] = float(limit)
        self._hal_data["currentChopCycles"] = limitCycles
        return retval

    def setIdleMode(self, mode: IdleMode) -> CANError:
        """Sets the idle mode setting for the SPARK MAX.

        :param mode: Idle mode (coast or brake).
        """
        assert isinstance(mode, IdleMode)
        retval = CANError.kOK
        self._hal_data['idleMode'] = mode
        return retval

    def getIdleMode(self) -> IdleMode:
        """Gets the idle mode setting for the SPARK MAX.

        This uses the Get Parameter API and should be used infrequently. This
        function uses a non-blocking call and will return a cached value if the
        parameter is not returned by the timeout. The timeout can be changed by
        calling SetCANTimeout(int milliseconds)

        :returns: IdleMode Idle mode setting
        """
        retval = self._hal_data['idleMode']
        return retval

    def setRampRate(self, rate: float) -> CANError:
        """Sets the ramp rate for all control modes.

        This is the maximum rate at which the motor controller's output
        is allowed to change.

        :param rate: Time in seconds to go from 0 to full throttle.
        """
        assert isinstance(rate, (int, float))
        retval = CANError.kOK
        self._hal_data['rampRate'] = rate
        return retval

    def getRampRate(self) -> float:
        """Get the configured ramp rate

        This is the maximum rate at which the motor controller's output
        is allowed to change.

        :returns: rampte rate time in seconds to go from 0 to full throttle.
        """
        retval = self._hal_data['rampRate']
        assert isinstance(retval, float)
        return retval

    def isFollower(self) -> bool:
        """Returns whether the controller is following another controller

        :returns: True if this device is following another controller
                  false otherwise

        .. note:: This function works on a real robot, but has not yet
                  been implemented in simulation mode. See :ref:`api_support`
                  for more details.

        """
        raise NotImplementedError("isFollower not implemented in simulation (file a bug on github if you need it)")

    def getFaults(self) -> int:
        """Returns fault bits.
        """
        retval = 0
        for faultID, value in self._faults.items():
            retval |= value << faultID.value
        assert isinstance(retval, int) and 0 <= retval < 1<<16
        return retval

    def getStickyFaults(self) -> int:
        """Returns sticky fault bits.
        """
        retval = 0
        assert isinstance(retval, int) and 0 <= retval < 1<<16
        return retval

    def getFault(self, faultID: FaultID) -> bool:
        """Returns whether the fault with the given ID occurred.
        """
        assert isinstance(faultID, FaultID)
        retval = self._faults.get(faultID, False)
        assert isinstance(retval, bool)
        return retval

    def getStickyFault(self, faultID: FaultID) -> bool:
        """Returns whether the sticky fault with the given ID occurred.
        """
        assert isinstance(faultID, FaultID)
        retval = False
        assert isinstance(retval, bool)
        return retval

    def getBusVoltage(self) -> float:
        """Returns the voltage fed into the motor controller.
        """
        retval = self._hal_data['bus_voltage']
        assert isinstance(retval, float)
        return retval

    def getAppliedOutput(self) -> float:
        """Returns motor controller's output voltage.
        """
        retval = self._hal_data['value']
        assert isinstance(retval, float)
        return retval

    def getOutputCurrent(self) -> float:
        """Returns motor controller's output current in Amps.
        """
        retval = self._hal_data['output_current']
        assert isinstance(retval, float)
        return retval

    def getMotorTemperature(self) -> float:
        """Returns the motor temperature in Celsius.
        """
        retval = self._hal_data['motor_temp']
        assert isinstance(retval, float)
        return retval

    def clearFaults(self) -> CANError:
        """Clears all non-sticky faults.

        Sticky faults must be cleared by resetting the motor controller.
        """
        retval = CANError.kOK
        faults = self._hal_data["faults"]
        faults.clear()
        faults.update({
            FaultID.kHardLimitFwd: False,
            FaultID.kHardLimitRev: False,
        })
        return retval

    def burnFlash(self) -> CANError:
        """Writes all settings to flash.
        """
        retval = CANError.kOK
        
        return retval

    def setCANTimeout(self, milliseconds: int) -> CANError:
        """Sets timeout for sending CAN messages.

        :param milliseconds: The timeout in milliseconds.
        """
        assert isinstance(milliseconds, int)
        retval = CANError.kOK
        self._hal_data['can_timeout'] = milliseconds
        return retval

