"""This file implements an accurate motor model."""
import numpy as np

VOLTAGE_CLIPPING = 50
OBSERVED_TORQUE_LIMIT = 5.7
MOTOR_VOLTAGE = 16.0
MOTOR_RESISTANCE = 0.186
MOTOR_TORQUE_CONSTANT = 0.0954
MOTOR_VISCOUS_DAMPING = 0
MOTOR_SPEED_LIMIT = MOTOR_VOLTAGE / (MOTOR_VISCOUS_DAMPING + MOTOR_TORQUE_CONSTANT)


class MotorModel(object):
    """
    The accurate motor model, which is based on the physics of DC motors.

    The motor model support two types of control: position control and torque
    control. In position control mode, a desired motor angle is specified, and a
    torque is computed based on the internal motor model. When the torque control
    is specified, a pwm signal in the range of [-1.0, 1.0] is converted to the
    torque.

    The internal motor model takes the following factors into consideration:
    pd gains, viscous friction, back-EMF voltage and current-torque profile.
    """

    def __init__(self, torque_control_enabled=False, kp=1.2, kd=0):
        self._torque_control_enabled = torque_control_enabled
        self._kp = kp
        self._kd = kd
        self._resistance = MOTOR_RESISTANCE
        self._voltage = MOTOR_VOLTAGE
        self._torque_constant = MOTOR_TORQUE_CONSTANT
        self._viscous_damping = MOTOR_VISCOUS_DAMPING
        self._current_table = [0, 10, 20, 30, 40, 50, 60]
        self._torque_table = [0, 1, 1.9, 2.45, 3.0, 3.25, 3.5]

    def set_voltage(self, voltage):
        self._voltage = voltage

    def get_voltage(self):
        return self._voltage

    def set_viscous_damping(self, viscous_damping):
        self._viscous_damping = viscous_damping

    def get_viscous_dampling(self):
        return self._viscous_damping

    def convert_to_torque(self, motor_commands, current_motor_angle, current_motor_velocity):
        """Convert the commands (position control or torque control) to torque.

        :param motor_commands: The desired motor angle if the motor is in position
            control mode. The pwm signal if the motor is in torque control mode.
        :param current_motor_angle: The motor angle at the current time step.
        :param current_motor_velocity: The motor velocity at the current time step.
        :return actual_torque: The torque that needs to be applied to the motor.
        :return observed_torque: The torque observed by the sensor.
        """
        if self._torque_control_enabled:
            pwm = motor_commands
        else:
            pwm = -self._kp * (current_motor_angle - motor_commands) - self._kd * current_motor_velocity
        pwm = np.clip(pwm, -1.0, 1.0)
        return self._convert_to_torque_from_pwm(pwm, current_motor_velocity)

    def _convert_to_torque_from_pwm(self, pwm, current_motor_velocity):
        """Convert the pwm signal to torque.
        :param pwm: The pulse width modulation.
        :param current_motor_velocity: The motor velocity at the current time step.
        :return actual_torque: The torque that needs to be applied to the motor.
        :return observed_torque: The torque observed by the sensor.
        """
        observed_torque = np.clip(
            self._torque_constant * (pwm * self._voltage / self._resistance),
            -OBSERVED_TORQUE_LIMIT,
            OBSERVED_TORQUE_LIMIT,
        )

        # Net voltage is clipped at 50V by diodes on the motor controller.
        voltage_net = np.clip(
            pwm * self._voltage - (self._torque_constant + self._viscous_damping) * current_motor_velocity,
            -VOLTAGE_CLIPPING,
            VOLTAGE_CLIPPING,
        )
        current = voltage_net / self._resistance
        current_sign = np.sign(current)
        current_magnitude = np.absolute(current)

        # Saturate torque based on empirical current relation.
        actual_torque = np.interp(current_magnitude, self._current_table, self._torque_table)
        actual_torque = np.multiply(current_sign, actual_torque)
        # print("Actual torque", actual_torque)
        return actual_torque, observed_torque
