# This program demonstrates usage of the servos.
# Keep the robot in a safe location before running this program,
# as it will immediately begin moving.
# See https://learn.adafruit.com/adafruit-16-channel-pwm-servo-hat-for-raspberry-pi/ for more details.

import time
import Adafruit_PCA9685
import signal
import math

from encoders import resetCounts, getSpeeds, getCounts, initEncoders


# The servo hat uses its own numbering scheme within the Adafruit library.
# 0 represents the first servo, 1 for the second, and so on.
LSERVO = 0
RSERVO = 1

# This function is called when Ctrl+C is pressed.
# It's intended for properly exiting the program.
def ctrlC(signum, frame):
    print("Exiting")

    # Stop the servos
    pwm.set_pwm(LSERVO, 0, 0);
    pwm.set_pwm(RSERVO, 0, 0);

    exit()

# Attach the Ctrl+C signal interrupt
signal.signal(signal.SIGINT, ctrlC)

# Initialize  the servo hat library.
pwm = Adafruit_PCA9685.PCA9685()

# 50Hz is used for the frequency of the servos.
pwm.set_pwm_freq(50)

# Write an initial value of 1.5, which keeps the servos stopped.
# Due to how servos work, and the design of the Adafruit library,
# the value must be divided by 20 and multiplied by 4096.
pwm.set_pwm(LSERVO, 0, math.floor(1.5 / 20 * 4096));
pwm.set_pwm(RSERVO, 0, math.floor(1.5 / 20 * 4096));

def setSpeedsPWM(pwmLeft, pwmRight, t):
    pwm.set_pwm(LSERVO, 0, math.floor(pwmLeft / 20 * 4096));
    pwm.set_pwm(RSERVO, 0, math.floor(pwmRight / 20 * 4096));
    time.sleep(t)
    pwm.set_pwm(LSERVO, 0, math.floor(1.5 / 20 * 4096));
    pwm.set_pwm(RSERVO, 0, math.floor(1.5 / 20 * 4096));



def calibrateSpeeds():
    pwmLeft = 1.5
    pwmRight = 1.5
    pwm.set_pwm(LSERVO, 0, math.floor(1.5 / 20 * 4096));
    pwm.set_pwm(RSERVO, 0, math.floor(1.5 / 20 * 4096));
    time.sleep(1)
    resetCounts()
    file2write=open("calibrate.txt", 'w')
    file2write.write("LeftPWM    RightPWM  LeftSpeed(RPS)  RightSpeed(RPS) \n")
    while pwmLeft < 1.7:
        resetCounts()
        pwmLeft += 0.01
        pwmRight += 0.01
        pwm.set_pwm(LSERVO, 0, math.floor(pwmLeft / 20 * 4096));
        pwm.set_pwm(RSERVO, 0, math.floor(pwmRight / 20 * 4096));
        time.sleep(3)
        if pwmLeft == 1.51:
            print()
        else:
                file2write.write('%.4f    ' % pwmLeft)
                file2write.write('%.4f    ' % pwmRight)
                out = getSpeeds()
                file2write.write('%.4f    %.4f \n' % out)
                resetCounts()

    time.sleep(1.5)
    pwmLeft = 1.5
    pwmRight = 1.5
    pwm.set_pwm(LSERVO, 0, math.floor(pwmLeft / 20 * 4096));
    pwm.set_pwm(RSERVO, 0, math.floor(pwmRight / 20 * 4096));
    time.sleep(1.5)
    resetCounts()

    while pwmLeft > 1.3:
        resetCounts()
        pwmLeft -= 0.01
        pwmRight -= 0.01
        pwm.set_pwm(LSERVO, 0, math.floor(pwmLeft / 20 * 4096));
        pwm.set_pwm(RSERVO, 0, math.floor(pwmRight / 20 * 4096));
        time.sleep(3)
        if pwmLeft == 1.49:
            print()
        else:
            file2write.write('%.4f    ' % pwmLeft)
            file2write.write('%.4f    ' % pwmRight)
            out = getSpeeds()
            file2write.write('%.4f    %.4f \n' % out)
            resetCounts()

    pwm.set_pwm(LSERVO, 0, math.floor(1.5 / 20 * 4096));
    pwm.set_pwm(RSERVO, 0, math.floor(1.5 / 20 * 4096));

    file2write.close()





def setSpeedsRPS(rpsLeft, rpsRight, t):
    pwmLeft = 1.5
    pwmRight = 1.5
    count = 0
    if rpsLeft > 0.815 or rpsRight > 0.815:
        print("The robot cannot move this quickly.")

    else:
        array = []
        file2read = open("calibrate.txt", "r")
        next(file2read)
        for line in file2read:
            fields = line.split("    ")
            pwmL = float(fields[0])
            pwmR = float(fields[1])
            speedL = float(fields[2])
            speedR = float(fields[3])
            count += 1




            if  pwmL > 1.50:
                pwmRight = pwmR
                if speedL == round(rpsLeft, 4):
                        pwmLeft = pwmL
                        print("speedl: ", speedL)
                        print("rpsLeft: ", rpsLeft)
                        print("left: ", pwmLeft)
                        count = 0

            if count == 19:
                pwmRight = pwmR
                if rpsRight == 0:
                    pwmRight = 0
                    print("Right: ", pwmRight)
            if rpsLeft == 0:
                pwmRight = pwmL
                pwmLeft = 0

        if pwmLeft == 1.5:
            print("left: Invalid RPS")
        if pwmRight == 1.5:
            print("right: Invalid RPS")
        else:
            setSpeedsPWM(pwmLeft, pwmRight, t)

           #Rectangle function
           # setSpeedsPWM(pwmRight, pwmRight - .4, t)



        '''

            if direction == 1 and pwmL > 1.5:
                if round(speedL, 1) == round(rpsLeft, 1):
                    pwmLeft = round(pwmL, 3)
                    print("left: ", pwmLeft)
                if round(speedR, 1) == round(rpsRight, 1):
                    pwmRight = round(pwmR, 3)
                    print("right: ", pwmRight)
            elif direction == 0 and pwmR > 1.5:
                if round(speedL, 1) == round(rpsLeft, 1):
                    pwmLeft = round(pwmL, 3)
                    print("left: ", pwmLeft)
                if round(speedR, 1) == round(rpsRight, 1):
                    pwmRight = round(pwmR, 3)
                    print("right: ", pwmRight)

        if pwmLeft == 1.5:
            print("left: Invalid RPS")
        if pwmRight == 1.5:
            print("right: Invalid RPS")
        else:
            setSpeedsPWM(pwmLeft, pwmRight)

        time.sleep(4)
        pwm.set_pwm(LSERVO, 0, math.floor(1.5 / 20 * 4096));
        pwm.set_pwm(RSERVO, 0, math.floor(1.5 / 20 * 4096));
        '''

def setSpeedsIPS(ipsLeft, ipsRight, time):
    ipsLeft = ipsLeft / 8.2
    left = round(ipsLeft, 5)
    ipsRight = ipsRight / 8.2
    right = round(ipsRight, 5)
    if ipsLeft > .8125 or ipsRight > .8125:
        print("Speed is too great")
    if ipsLeft < .125 or ipsRight < .125:
        print("Speed is too slow.")
    else:
        setSpeedsRPS(ipsLeft, ipsRight, time)

#def setSpeedVW(v, w):


    #v = (ipsRight + ipsLeft)/2
    #w = (ipsRight - ipsLeft)/3.95

initEncoders()