import RPi.GPIO as GPIO
from RpiMotorLib import RpiMotorLib #sudo pip3 install rpimotorlib
import time
import statistics

s0 = 27
s1 = 22
s2 = 23
s3 = 24
sig = 25
cycles = 10

#Stepper motor setup w/ BCM for rpi motor lib
MOTOR1_BCM = [4,15,14,18]
MOTOR2_BCM = [10,11,9,8]

#Set up steppers with library
motor1 = RpiMotorLib.BYJMotor("RackPinion", "28BYJ")
motor2 = RpiMotorLib.BYJMotor("Carousel", "28BYJ")

STEP_DELAY = 0.001
VERBOSE = False
STEP_TYPE = "half"

#variables for motor postion tracking
#lift
POS_BOTTOM=0
POS_MIDDLE=-20
POS_TOP=-38

#Carousel
CAROUSEL_POSITIONS = 4
STEPS_PER_POSITION = 12

#postion tracking, start at bottom
lift_position = 0
carousel_position=0

# Setup GPIO and pins (using BCM)
GPIO.setup(s0, GPIO.OUT)
GPIO.setup(s1, GPIO.OUT)
GPIO.setup(s2, GPIO.OUT)
GPIO.setup(s3, GPIO.OUT)
GPIO.setup(sig, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)

# Set frequency scaling
GPIO.output(s0, GPIO.HIGH)
GPIO.output(s1, GPIO.LOW)

#move lift
def move_lift(target):
    global lift_position
    steps_to_move=target - lift_position

    if steps_to_move !=0:
        motor1.motor_run(MOTOR1_BCM, STEP_DELAY, abs(steps_to_move), steps_to_move < 0, VERBOSE, STEP_TYPE, 0.05)
        lift_position=target
        time.sleep(.1)

#move carousel - find fasest route to next pos
def rotate_carousel(position):
    global carousel_position

    positions_to_move = (position - carousel_position) % CAROUSEL_POSITIONS
    if positions_to_move > 2:
        positions_to_move = positions_to_move - CAROUSEL_POSITIONS
    
    steps_to_move = abs(positions_to_move * STEPS_PER_POSITION)

    if steps_to_move != 0:
        motor2.motor_run(MOTOR2_BCM, .01, steps_to_move, positions_to_move < 0, VERBOSE, "full", 0.05)
        carousel_position = position
        time.sleep(0.1)

def clear():
    for i in range(11):
        move_lift(POS_BOTTOM)
        time.sleep(1)
        move_lift(POS_TOP)
        time.sleep(1)
    move_lift(POS_BOTTOM)

def normalize(red, green, blue):
    total = red + green + blue
    if total == 0:
        return 0, 0, 0
    return red/total, green/total, blue/total

def readcolor():
    # Detect red values
    GPIO.output(s2, GPIO.LOW)
    GPIO.output(s3, GPIO.LOW)
    time.sleep(0.1)
    start_time = time.time()
    for count in range(cycles):
        GPIO.wait_for_edge(sig, GPIO.FALLING)
    duration = time.time() - start_time
    red = cycles / duration
    #print("red value - ", red)


    # Detect blue values
    GPIO.output(s2, GPIO.LOW)
    GPIO.output(s3, GPIO.HIGH)
    time.sleep(0.1)
    start_time = time.time()
    for count in range(cycles):
        GPIO.wait_for_edge(sig, GPIO.FALLING)
    duration = time.time() - start_time
    blue = cycles / duration
    #print("blue value - ", blue)

    # Detect green values
    GPIO.output(s2, GPIO.HIGH)
    GPIO.output(s3, GPIO.HIGH)
    time.sleep(0.1)
    start_time = time.time()
    for count in range(cycles):
        GPIO.wait_for_edge(sig, GPIO.FALLING)
    duration = time.time() - start_time
    green = cycles / duration
    #print("green value - ", green)

    return(red, green, blue)  


Rcnum = 0
Gcnum = 0
Bcnum = 0
Ycnum = 0

ListRinR = []
ListGinR = []
ListBinR = []

ListRinB = []
ListGinB = []
ListBinB = []

ListRinG = []
ListGinG = []
ListBinG = []

ListRinY = []
ListGinY = []
ListBinY = []

def clear_calibration_data():
    global Rcnum, Gcnum, Bcnum, Ycnum
    global ListRinR, ListGinR, ListBinR, ListRinB, ListGinB, ListBinB
    global ListGinG, ListBinG, ListRinG, ListRinY, ListBinY, ListGinY
    
    Rcnum = Gcnum = Bcnum = Ycnum = 0
    ListRinR = ListGinR = ListBinR = []
    ListRinB = ListGinB = ListBinB = []
    ListRinG = ListGinG = ListBinG = []
    ListRinY = ListGinY = ListBinY = []

def Cal_Red():
    global Rcnum, Gcnum, Bcnum, Ycnum, ListRinR, ListGinR, ListBinR, ListRinB, ListGinB, ListBinB, ListGinG, ListBinG,ListRinG, ListRinY, ListBinY, ListGinY
    red, green, blue =readcolor()
    ListRinR.append(red)
    ListGinR.append(green)
    ListBinR.append(blue)
    Rcnum = Rcnum +1    

def Cal_Blue():
    global Rcnum, Gcnum, Bcnum, Ycnum, ListRinR, ListGinR, ListBinR, ListRinB, ListGinB, ListBinB, ListGinG, ListBinG,ListRinG, ListRinY, ListBinY, ListGinY
    red, green, blue = readcolor()
    ListRinB.append(red)
    ListGinB.append(green)
    ListBinB.append(blue)
    Bcnum = Bcnum +1

def Cal_Green():
    global Rcnum, Gcnum, Bcnum, Ycnum, ListRinR, ListGinR, ListBinR, ListRinB, ListGinB, ListBinB, ListGinG, ListBinG,ListRinG, ListRinY, ListBinY, ListGinY
    red, green, blue = readcolor()
    ListRinG.append(red)
    ListGinG.append(green)
    ListBinG.append(blue)
    Gcnum = Gcnum +1

def Cal_Yellow():
    global Rcnum, Gcnum, Bcnum, Ycnum, ListRinR, ListGinR, ListBinR, ListRinB, ListGinB, ListBinB, ListGinG, ListBinG,ListRinG, ListRinY, ListBinY, ListGinY
    red, green, blue = readcolor()
    ListRinY.append(red)
    ListGinY.append(green)
    ListBinY.append(blue)
    Ycnum = Ycnum +1
    
def Done_Cal():
    global ARinR, AGinR, ABinR, ARinG, AGinG, ABinG, ARinB, AGinB, ABinB, ARinY, AGinY, ABinY 
    global ListRinR, ListGinR, ListBinR, ListRinB, ListGinB, ListBinB, ListGinG, ListBinG,ListRinG, ListRinY, ListBinY, ListGinY
    ARinR = sum(ListRinR)/len(ListRinR)
    AGinR = sum(ListGinR)/len(ListGinR)
    ABinR = sum(ListBinR)/len(ListBinR)

    print(ARinR, AGinR, ABinR)
    #ARinR, AGinR, ABinR = normalize(ARinR, AGinR, ABinR)
    print(ARinR, AGinR, ABinR)

    ARinG = sum(ListRinG)/len(ListRinG)
    AGinG = sum(ListGinG)/len(ListGinG)
    ABinG = sum(ListBinG)/len(ListBinG)

    #ARinG, AGinG, ABinG = normalize(ARinG, AGinG, ABinG)
    print(ARinG, AGinG, ABinG)

    ARinB = sum(ListRinB)/len(ListRinB)
    AGinB = sum(ListGinB)/len(ListGinB)
    ABinB = sum(ListBinB)/len(ListBinB)

    #ARinB, AGinB, ABinB = normalize(ARinB, AGinB, ABinB)
    print(ARinB, AGinB, ABinB )

    ARinY = sum(ListRinY)/len(ListRinY)
    AGinY = sum(ListGinY)/len(ListGinY)
    ABinY = sum(ListBinY)/len(ListBinY)

    #ARinY, AGinY, ABinY = normalize(ARinY, AGinY, ABinY)
    print(ARinY, AGinY, ABinY)

    #print(ARinR, AGinR, ABinR, ARinG, AGinG, ABinG, ARinB, AGinB, ABinB, ARinY, AGinY, ABinY)
    print("Callibration complete")

def Read_Color():
    #take 10 readings
    detectred1, detectgreen1, detectblue1 = readcolor()
    time.sleep(.15)
    detectred2, detectgreen2, detectblue2 = readcolor()
    time.sleep(.15)
    detectred3, detectgreen3, detectblue3 = readcolor()
    time.sleep(.15)
    detectred4, detectgreen4, detectblue4 = readcolor()
    time.sleep(.15)
    detectred5, detectgreen5, detectblue5 = readcolor()
    time.sleep(.15)
    detectred6, detectgreen6, detectblue6 = readcolor()
    time.sleep(.15)
    detectred7, detectgreen7, detectblue7 = readcolor()
    time.sleep(.15)
    detectred8, detectgreen8, detectblue8 = readcolor()
    time.sleep(.15)
    detectred9, detectgreen9, detectblue9 = readcolor()
    time.sleep(.15)
    detectred10, detectgreen10, detectblue10 = readcolor()

    #take mean of 10 readings
    detectred = statistics.mean([detectred1, detectred2, detectred3, detectred4, detectred5, 
                                  detectred6, detectred7, detectred8, detectred9, detectred10])
    detectgreen = statistics.mean([detectgreen1, detectgreen2, detectgreen3, detectgreen4, detectgreen5,
                                    detectgreen6, detectgreen7, detectgreen8, detectgreen9, detectgreen10])
    detectblue = statistics.mean([detectblue1, detectblue2, detectblue3, detectblue4, detectblue5,
                                   detectblue6, detectblue7, detectblue8, detectblue9, detectblue10])

    #normalize
    #detectred, detectgreen, detectblue = normalize(detectred, detectgreen, detectblue)
    print(detectred,detectgreen,detectblue)

    #root square mean to RGBY
    distred = ((ARinR-detectred)**2 +(ABinR-detectblue)**2 + (AGinR-detectgreen)**2 )**.5
    distblue = ((ARinB-detectred)**2 +(ABinB-detectblue)**2 + (AGinB-detectgreen)**2 )**.5
    distgreen = ((ARinG-detectred)**2 +(ABinG-detectblue)**2 + (AGinG-detectgreen)**2 )**.5
    distyellow = ((ARinY-detectred)**2 +(ABinY-detectblue)**2 + (AGinY-detectgreen)**2 )**.5

    #determine closest color
    closest_fit = min(distred, distblue, distgreen, distyellow)
    if closest_fit == distred:
        print("red")
        color = 0
    elif closest_fit == distblue:
        print("blue")
        color = 1
    elif closest_fit == distgreen:
        print("green")
        color = 2
    elif closest_fit == distyellow:
        print("yellow")
        color = 3
    return color

def Calibrate(color,lifts,readings_per_lift):
    #lift, read, drop x times - write to list 
    if color=="red":
        rotate_carousel(0)
    elif color =="blue":
        rotate_carousel(1)
    elif color=="green":
        rotate_carousel(2)
    elif color=="yellow":
        rotate_carousel(3)
    for i in range(lifts):
        move_lift(POS_BOTTOM)
        time.sleep(.2)
        move_lift(POS_MIDDLE)
        time.sleep(.5)
        for j in range(readings_per_lift):
            if color=="red":
                Cal_Red()
            elif color =="blue":
                Cal_Blue()
            elif color=="green":
                Cal_Green()
            elif color=="yellow":
                Cal_Yellow()
            time.sleep(.05)
    move_lift(POS_TOP)
    time.sleep(.5)
    move_lift(POS_BOTTOM)

def Sort(balls):
    #lift, read, determine color, rotate, drop, repeat
    for i in range(balls):
        move_lift(POS_BOTTOM)
        time.sleep(1)
        move_lift(POS_MIDDLE)
        time.sleep(1)
        rotate_carousel(Read_Color())
        move_lift(POS_TOP)
        time.sleep(1)
    move_lift(POS_BOTTOM)

try:  
    Calibrate("red", 4, 10)
    Calibrate("blue", 4, 10)
    Calibrate("green", 4, 10)
    Calibrate("yellow", 4, 10)
    Done_Cal()

    while True:
        comment = input("'g' to sort, 'c'to clear ")
        if comment == "g":
            Sort(10)
        elif comment == 'c':
            clear()
        
except KeyboardInterrupt:
        print("\nExiting Program")
finally:
        GPIO.cleanup()