from cozmo_fsm import *
from cozmo.lights import *

#Runs the light cycle FSM as descibed in the write up
class LightCycle(StateMachineProgram):
    $setup {
        #Set lights to their initial state
        StateNode() =N=> {startred, startblue, startgreen}

        startred:  SetLights(cube1, red_light)
        startgreen: SetLights(cube2, green_light)
        startblue: SetLights(cube3, blue_light)
        
        #Run the first light cycle after a tap
        {startred, startblue, startgreen} =Tap()=> {next1, next2, next3}

    next1: SetLights(cube1, green_light)
	next2: SetLights(cube2, blue_light)
	next3: SetLights(cube3, red_light)

        #Runs the second light cycle after a tap
	{next1, next2, next3} =Tap()=> {last1, last2, last3}
        
        last1: SetLights(cube1, blue_light)
	last2: SetLights(cube2, red_light)
	last3: SetLights(cube3, green_light)

        #Cycles back to initial colors on tap
        {last1, last2, last3} =Tap()=> {startred, startblue, startgreen}

    }