# VEX EDR Python-Project
import sys
import vex

#region config
l_encoder  = vex.Encoder(1, True)
r_encoder  = vex.Encoder(3, True)
gyro       = vex.GyroSensor(1)
l_motor    = vex.Motor(1, True)
arm_motor  = vex.Motor(4)
claw_motor = vex.Motor(5)
r_motor    = vex.Motor(10)
joystick   = vex.Joystick()
#endregion config

class PID:
    """A class implementing a PID controller."""
    def __init__(self, p, i, d, get_current_time, get_feedback_value):
        # p, i and d values
        self.p, self.i, self.d = p, i, d

        # saves the functions that return the time and the feedback
        self.get_current_time = get_current_time
        self.get_feedback_value = get_feedback_value

        self.reset()


    def reset(self):
        """Resets/creates all of the non-constant variables of the class."""
        # reset PID values
        self.proportional, self.integral, self.derivative = 0, 0, 0

        # reset previous time and error variables
        self.previous_time, self.previous_error = 0, 0


    def get_value(self):
        """Calculates and returns the PID value."""
        # calculate the error (how far off the goal are we)
        error = self.goal - self.get_feedback_value()

        current_time = self.get_current_time()

        # variables used to calculate proportional and integral
        delta_time = current_time - self.previous_time
        delta_error = error - self.previous_error

        self.proportional = self.p * error      # calculate proportional
        self.integral += error * delta_time     # calculate integral

        self.derivative = 0
        if (delta_time > 0):
            self.derivative = delta_error / delta_time  # calculate derivative

        # update error and time values
        self.previous_time = current_time
        self.previous_error = error

        # return the "normalized" PID value
        pid = self.proportional + (self.i * self.integral) + (self.d * self.derivative)
        if pid > 1:
            return 1
        elif pid < -1:
            return -1
        else:
            return pid


    def set_goal(self, goal):
        """Sets the goal of the PID controller."""
        self.goal = goal


def arcade_drive(rotate, drive, left_motor, right_motor):
    # variables to determine the quadrants
    maximum = max(abs(drive), abs(rotate))
    total, difference = drive + rotate, drive - rotate

    # go by quadrants and set the speed accordingly
    if drive >= 0:
        if rotate >= 0:
            left_motor(maximum)
            right_motor(difference)
        else:
            left_motor(total)
            right_motor(maximum)
    else:
        if rotate >= 0:
            left_motor(total)
            right_motor(-maximum)
        else:
            left_motor(-maximum)
            right_motor(difference)


class PolynomialFunction:
    """A class implementing a polynomial function controller."""

    def __init__(self, coefficients, get_feedback_value):
        """Note that the valid x values for the polynomial function coefficients
        should be from 0 to 1 (the function is scaled by the goal)."""
        self.coefficients = coefficients    # the coefficients of the function
        self.get_feedback_value = get_feedback_value   # the feedback function


    def get_value(self):
        """Returns the polynomial function value at feedback function value."""
        # calculate the x coordinate (by "stretching" the function by goal)
        x = self.get_feedback_value() / abs(self.goal)

        if x > 1:
            return 0

        # calculate function value using Horner's method
        value = self.coefficients[0]
        for i in range(1, len(self.coefficients)):
            value = x * value + self.coefficients[i]

        # make sure that value isn't above 1
        if value > 1:
            value = 1

        # if goal is negative, function value is negative
        return value if self.goal > 0 else -value


    def set_goal(self, goal):
        """Sets the goal of the controller."""
        self.goal = goal


angle_pid = PID(0.2, 0.002, 0.015, sys.clock, gyro.gyro_degrees)
angle_pid.set_goal(0)

drive_controller = PolynomialFunction([-14.30556, 28.61111, -21.28194, 6.976389, 0.15], l_encoder.value)
drive_controller.set_goal(100 / 0.091776)

while True:
    arm_motor.run(joystick.axis3())
    claw_motor.run(joystick.axis4())

    if joystick.b6down():
        arcade_drive(joystick.axis1(), -joystick.axis2(), l_motor.run, r_motor.run)

    if joystick.b6up():
        sys.sleep(0.05)
        pid_value = angle_pid.get_value()
        arcade_drive(pid_value * 35, 0, l_motor.run, r_motor.run)

    if joystick.b5up():
        sys.sleep(0.05)
        pid_value = angle_pid.get_value()
        drive_value = drive_controller.get_value()

        arcade_drive(pid_value * 35, drive_value * 80, l_motor.run, r_motor.run)