#include "Behaviors/StateMachine.h"

$nodeclass Parallel : StateNode {

  $setupmachine{

    launch: StateNode =N=> {noblink, forward_power_led, forward_barklow}

    noblink: LedNode [setPriority(MotionManager::kBackgroundPriority); getMC()->set(RobotInfo::PlayLEDMask, 0.0)]

    forward_power_led: LedNode [getMC()->set(RobotInfo::PowerRedLEDMask, 1.0)]
    forward_play_led: LedNode [getMC()->set(RobotInfo::PlayLEDMask, 1.0)]
    forward_adv_led: LedNode [getMC()->set(RobotInfo::AdvanceLEDMask, 1.0)]

    forward_power_led =T(1000)=> forward_play_led
    forward_play_led =T(1000)=> forward_adv_led
    forward_adv_led =T(1000)=> forward_power_led

    backward_power_led: LedNode [getMC()->set(RobotInfo::PowerRedLEDMask, 1.0)]
    backward_play_led: LedNode [getMC()->set(RobotInfo::PlayLEDMask, 1.0)]
    backward_adv_led: LedNode [getMC()->set(RobotInfo::AdvanceLEDMask, 1.0)]

    backward_power_led =T(1000)=> backward_adv_led
    backward_play_led =T(1000)=> backward_power_led
    backward_adv_led =T(1000)=> backward_play_led

    forward_power_led =B(BumpLeftButOffset)=> backward_adv_led
    forward_play_led =B(BumpLeftButOffset)=> backward_power_led
    forward_adv_led =B(BumpLeftButOffset)=> backward_play_led
    backward_adv_led =B(BumpLeftButOffset)=> forward_power_led
    backward_play_led =B(BumpLeftButOffset)=> forward_adv_led
    backward_power_led =B(BumpLeftButOffset)=> forward_play_led

    
    forward_barklow: SoundNode("barklow.wav")
    forward_barkmed: SoundNode("barkmed.wav")
    forward_yipper: SoundNode("yipper.wav")

    forward_barklow =C=> forward_barkmed
    forward_barkmed =C=> forward_yipper
    forward_yipper =C=> forward_barklow

    backward_barklow: SoundNode("barklow.wav")
    backward_barkmed: SoundNode("barkmed.wav")
    backward_yipper: SoundNode("yipper.wav")

    backward_barklow =C=> backward_yipper
    backward_barkmed =C=> backward_barklow
    backward_yipper =C=> backward_barkmed

    forward_barklow =B(BumpLeftButOffset)=> backward_yipper
    forward_barkmed =B(BumpLeftButOffset)=> backward_barklow
    forward_yipper =B(BumpLeftButOffset)=> backward_barkmed
    backward_yipper =B(BumpLeftButOffset)=> forward_barklow
    backward_barkmed =B(BumpLeftButOffset)=> forward_yipper
    backward_barklow =B(BumpLeftButOffset)=> forward_barkmed



  }

}

REGISTER_BEHAVIOR(Parallel);