from future import *
import time
import robotbit
import sugar
import gc

robot = robotbit.RobotBit()
joystick = sugar.Joystick()
btnMode = sugar.Button('P1')
btnCatch = sugar.Button('P2')


class dollMachine():
    def __init__(self):
        screen.sync = 0
        self.mode = 0 # 0: move horizontally / 1: move vertically
        self.dir = None
        self.claw_angle_list = [30, 140] # claw extreme angle [grab, release]
        self.text = ['horizontal', 'up and down']
        self.catch_flag = -1
        robot.servo(1,self.claw_angle_list[1])
        self.time_left = 0
        self.setTime = 0
        self.display()
    
    def display(self):
        screen.fill((166,63,244))
        screen.textCh(self.text[self.mode],x=32, y=48,ext=2)
        screen.text(round((self.setTime-self.time_left)/1000),64,16,ext=2)
        screen.refresh()
        
    def horizontal(self,cmd):
        if not cmd==self.dir: 
            robot.motor(1,0)
            if cmd == 'right':
                robot.motor(4, 255)
            elif cmd == 'left':
                robot.motor(4, -255)
            else:
                robot.motor(4,0)
            if cmd == 'up':
                robot.motor(3,-255)
            elif cmd == 'down':
                robot.motor(3, 255)
            else:
                robot.motor(3,0)
            self.dir = cmd
            
    def virtical(self,cmd):
        if not cmd==self.dir:  
            robot.motor(3, 0)
            robot.motor(4, 0)
            if cmd == 'up':
                robot.motor(1,255) # go up
            elif cmd == 'down':
                robot.motor(1,-255) # go down
            else:
                robot.motor(1,0)
            self.dir = cmd   
    
    def changeMode(self):
        if btnMode.value()==0:
            time.sleep(0.2)
            if self.mode==0:
                self.mode = 1
            else:
                self.mode = 0 
        self.display()
    
    def run(self,t):
        start = time.ticks_ms()
        self.setTime = t
        while (self.time_left <= t):
            self.time_left = time.ticks_diff(time.ticks_ms(),start)
            self.changeMode()
            self.catch()
            dir = joystick.state()
            if self.mode == 0:
                self.horizontal(dir)
            else:
                self.virtical(dir)
        screen.clear()
        robot.motor(1,0)
        robot.motor(3,0)
        robot.motor(4,0)
        robot.servo(1,self.claw_angle_list[1])
        buzzer.melody(ERROR)
        screen.fill((255,0,0))
        screen.textCh('End',x = 56, y = 48, ext = 2)
        screen.refresh()
        time.sleep(2)
        
    def catch(self):
        if btnCatch.value()==0:
            time.sleep(0.2)
            self.catch_flag*=-1
        if self.catch_flag==1:
            robot.servo(1,self.claw_angle_list[1])
        else:
            robot.servo(1,self.claw_angle_list[0])

while 1:
    screen.sync=1
    screen.clear()
    screen.textCh('Press to start',x = 36,y = 48,ext=2)
    while not(btnCatch.value()==0):
        pass
    buzzer.melody(CORRECT)
    screen.fill((0,188,0))
    screen.textCh('Start in 3s',x = 24,y = 48, ext=2)
    time.sleep(3)
    screen.sync=0
    doll=dollMachine()
    doll.run(45*1000) # 45s
    gc.collect()