import time
import board
import busio
import sshkeyboard
import sys
import scipy.stats
import json

from adafruit_motor import servo
from adafruit_pca9685 import PCA9685

i2c = busio.I2C(board.SCL, board.SDA)
pca = PCA9685(i2c)

pca.frequency = 50

servo_pwm = pca.channels[0]

#servo = servo.Servo(pca.channels[0], actuation_range = 180, min_pulse=1000, max_pulse=2000)
#servo.angle = 90

def usec_to_duty_cycle(usec):
    return int(usec * 0xffff / 20000)


print("""Calibrate Servo:

s - center (to 1500 usec)
a - move CCW (100 usec)
q - move CCW ( 10 usec)
d - move CW  (100 usec)
e - move CW  ( 10 usec)

<enter> - store value

Move Servo to 0 degrees now.""")

usec = 1500

angle = 0
angle_values = []
usec_values = []

servo_pwm.duty_cycle = usec_to_duty_cycle(usec)

def press(key):
    global usec
    global angle
    global angle_values
    global usec_values
    
    if key == 'a':
        usec -= 100
    if key == 'd':
        usec += 100

    if key == 's':
        usec = 1500
        
    if key == 'q':
        usec -= 10
    if key == 'e':
        usec += 10
        
    if key == 'enter':
        print(f"Angle: {angle} degrees")
        print(f"Pulse length: {usec} usec")
        angle_values.append(angle)
        usec_values.append(usec)
        angle += 30
        if angle > 180:
            sshkeyboard.stop_listening()
            return
        print(f"Move Servo to {angle} degress now.")
    
    print(usec)
    servo_pwm.duty_cycle = usec_to_duty_cycle(usec)
    
sshkeyboard.listen_keyboard( on_press = press )


result = scipy.stats.linregress(angle_values, usec_values)

print(f"0 degree value: {round(result.intercept)}")
print(f"180 degree value: {round(result.intercept + 180 * result.slope)}")

filename = input("Save as: ")

if filename != "":
    description = input("Description: ")
    
    if not filename.endswith(".json"):
        filename += ".json"

    calibration = {
        'description': description,
        'intercept': result.intercept,
        'slope': result.slope
    }

    with open(filename, "w") as out:
        out.write(json.dumps(calibration, indent = 4))