#include "Behaviors/StateMachine.h"

$nodeclass ParallelLEDBehavior : StateNode {
	$setupmachine {
		//Set up nodes for state machine	
		launch: StateNode
		noblink: LedNode [setPriority(MotionManager::kBackgroundPriority);
	              	 getMC()->set(RobotInfo::AllLEDMask,0.0)]
		//Set of nodes for transitioning forward
		powerflashf:LedNode[getMC()->cflash(RobotInfo::PowerRedLEDMask, 1.0, 1000)]
		playflashf: LedNode[getMC()->cflash(RobotInfo::PlayLEDMask, 1.0, 1000)]
		advflashf: LedNode[getMC()->cflash(RobotInfo::AdvanceLEDMask, 1.0, 1000)]
		//Set of nodes for transitioning backward
		powerflashb:LedNode[getMC()->cflash(RobotInfo::PowerRedLEDMask, 1.0, 1000)]
		playflashb: LedNode[getMC()->cflash(RobotInfo::PlayLEDMask, 1.0, 1000)]
		advflashb: LedNode[getMC()->cflash(RobotInfo::AdvanceLEDMask, 1.0, 1000)]
		//Set of nodes for barking forward
		barklowf: SoundNode("barklow.wav")
		barkmedf: SoundNode("barkmed.wav")
		yipperf: SoundNode("yipper.wav")
		//Set of nodes for barking backward
		barklowb: SoundNode("barklow.wav")
		barkmedb: SoundNode("barkmed.wav")
		yipperb: SoundNode("yipper.wav")

		//Set up launch transition
		launch =N=> {noblink, powerflashf, barklowf}
		//Set up forward transitions
		powerflashf =C=> playflashf =C=> advflashf =C=> powerflashf
		barklowf =C=> barkmedf =C=> yipperf =C=> barklowf
		//Set up backward transitions
		powerflashb =C=> advflashb =C=> playflashb =C=> powerflashb
		barklowb =C=> yipperb =C=> barkmedb =C=> barklowb

		//Set up bumper transitions from forward to backward
		powerflashf =B(BumpLeftButOffset)=> advflashb
		playflashf =B(BumpLeftButOffset)=> powerflashb
		advflashf =B(BumpLeftButOffset)=> playflashb
		barklowf  =B(BumpLeftButOffset)=> yipperb
		barkmedf  =B(BumpLeftButOffset)=> barklowb
		yipperf =B(BumpLeftButOffset)=> barkmedb
		//Set up bumper transitions from backward to forward
		powerflashb =B(BumpLeftButOffset)=> playflashf
		advflashb =B(BumpLeftButOffset)=> powerflashf
		playflashb =B(BumpLeftButOffset)=> advflashf
		barklowb =B(BumpLeftButOffset)=> barkmedf
		barkmedb =B(BumpLeftButOffset)=> yipperf
		yipperb =B(BumpLeftButOffset)=> barklowf
	}
}
REGISTER_BEHAVIOR(ParallelLEDBehavior);