import RoboTX
import time

# Edit this line to use the correct COM port
RoboTX.Initialize(4)
RoboTX.LampOn(8,256)
loc = [(0,75), (3150,80), (3150, 990), (3150, 1950)]
anti = [(0,-75), (-3150,-80), (-3150, -990), (-3150, -1950)]

def MoveIn():
    while RoboTX.GetSwitchInput(2) == 0:
        RoboTX.StartMotor(2, RoboTX.Right)
    RoboTX.StopMotor(2)

def MoveOut():
    while RoboTX.GetSwitchInput(4) == 0:
        RoboTX.StartMotor(2, RoboTX.Left)
    RoboTX.StopMotor(2)

def MoveBottom():
    MoveIn()
    while RoboTX.GetSwitchInput(3) == 0:
        RoboTX.StartMotor(3, RoboTX.Right)
    RoboTX.StopMotor(3)

def MoveNearEnd():
    MoveIn()
    while RoboTX.GetSwitchInput(1) == 0:
        RoboTX.StartMotor(1, RoboTX.Left)
    RoboTX.StopMotor(1)

def MoveV(d):
    RoboTX.ResetCounter(3)
    time.sleep(0.25)
    Direction = RoboTX.Left
    if d < 0:
        d = -d
        Direction = RoboTX.Right
    while RoboTX.GetCounter(3) < d:
        RoboTX.StartMotor(3, Direction)
    RoboTX.StopMotor(3)

def MoveH(d):
    RoboTX.ResetCounter(1)
    time.sleep(0.25)
    Direction = RoboTX.Right
    if d < 0:
        d = -d
        Direction = RoboTX.Left
    while RoboTX.GetCounter(1) < d:
        RoboTX.StartMotor(1, Direction)
    RoboTX.StopMotor(1)

def MoveHome():
    MoveBottom()
    MoveNearEnd()

def Grab():
    MoveOut()
    MoveV(150)
    MoveIn()
    MoveV(-150)

def Drop():
    MoveV(150)
    MoveOut()
    MoveV(-150)
    MoveIn()

def MovePoint(l):
    MoveV(l[1])
    MoveH(l[0])

def ShelveObject(i):
    Grab()
    MovePoint(anti[0])
    MovePoint(loc[i])
    Drop()
    MovePoint(anti[i])
    MovePoint(loc[0])

def ObjectIsPresent():
    RoboTX.LampOn(8, 512)
    a = not bool(RoboTX.GetSwitchInput(6))
    RoboTX.LampOff(8)
    print a

def Color():
    a = RoboTX.GetColorSensor(5)

    if (a > 50 and a < 300):
        return 2#yellow
    elif (a > 900 and a < 2000):
        return 3#blue
    elif (a > 400 and a < 800):
        return 1#red
    else:
        return "ERROR"

def SortObjects():
    MoveHome()
    MovePoint(loc[0])

    for i in range(3):
        print "You have 2 second to place the next block"
        time.sleep(2)

        a = Avg(100)

        if (a == "ERROR"):
            print "Oops. Please restart"
            time.sleep(10)
        else:
            ShelveObject(a)

def Avg(f):
    r = 0
    b = 0
    y = 0
    for i in range(f):
        a = Color()

        if a == 1:
            r = r + 1
        elif a == 2:
            y = y + 1
        elif a == 3:
            b = b + 1
    g = [r, y, b]
    g.sort()

    if (g[2] == r):
        return 1
    elif (g[2] == b):
        return 3
    else:
        return 2