from motor import Motor
from time import sleep

DELTA = 10
DELTA_TURN = 5
DELTA_TURN_MAX = 60

class Esc:
    def __init__(self, pin_m1_ena, pin_m1_in1, pin_m1_in2,
                 pin_m2_ena, pin_m2_in1, pin_m2_in2):
        self.m1 = Motor("m1", pin_m1_ena, pin_m1_in1, pin_m1_in2)
        self.m2 = Motor("m2", pin_m2_ena, pin_m2_in1, pin_m2_in2)

    ## regular movement
    def accel(self):
        speed = min((self.m1.speed + self.m2.speed)/2 + DELTA, 100)
        self.m1.set_speed(speed)
        self.m2.set_speed(speed)

    def decel(self):
        speed = max((self.m1.speed + self.m2.speed)/2 - DELTA, -100)
        self.m1.set_speed(speed)
        self.m2.set_speed(speed)

    # m1: left, m2: right
    def left(self):
        s1 = self.m1.speed
        s2 = self.m2.speed
        avg = (s1 + s2)/2
        delta = s2 - s1

        if delta <= (DELTA_TURN_MAX - 2*DELTA_TURN):
            self.m1.dec(DELTA_TURN)
            self.m2.inc(DELTA_TURN)
        elif delta > (DELTA_TURN_MAX):
            # print("delta exceeded, resetting")
            self.m1.set_speed(avg - DELTA_TURN)
            self.m2.set_speed(avg + DELTA_TURN)
        # elif delta == DELTA_TURN_MAX:
        #     print("delta reached, not changing")

    def right(self):
        s1 = self.m1.speed
        s2 = self.m2.speed
        avg = (s1 + s2)/2
        delta = s1 - s2
        if delta <= (DELTA_TURN_MAX - 2*DELTA_TURN):
            self.m1.inc(DELTA_TURN)
            self.m2.dec(DELTA_TURN)
        elif delta > (DELTA_TURN_MAX):
            # print("delta exceeded, resetting")
            self.m1.set_speed(avg + DELTA_TURN)
            self.m2.set_speed(avg - DELTA_TURN)
        # elif delta == DELTA_TURN_MAX:
        #     print("delta reached, not changing")

    ## extra
    def neutral(self):
        speed = (self.m1.speed + self.m2.speed)/2
        self.m1.set_speed(speed)
        self.m2.set_speed(speed)

    def stop(self):
        self.m1.set_speed(0)
        self.m2.set_speed(0)

    def rotate(self):
        if self.m1.speed >= 0:
            self.m1.set_speed(0)
            self.m2.set_speed(0)
            sleep(0.25)
            self.m1.set_speed(-50)
            self.m2.set_speed(50)
            sleep(0.75)
        else:
            self.m1.set_speed(0)
            self.m2.set_speed(0)
            sleep(0.25)
            self.m1.set_speed(50)
            self.m2.set_speed(-50)
            sleep(0.75)


    def get_status(self):
        s = "{}, {}".format(self.m1.get_status(), self.m2.get_status())
        return s