#include "Behaviors/StateMachine.h"

$nodeclass CycleLEDBark : StateNode {
  // A node which flashes an LED node (specified by a bitmask) for 1 second
  $nodeclass FlashLED(LEDBitMask_t bitmask) : LedNode : doStart {
    getMC()->flash(bitmask, 1.0, 1000);
  }
  // A sound node that stops playing its sound immediately when deactivated
  $nodeclass MySoundNode(string filename) : SoundNode(filename) : doStart {
    setAutoStop(true);
  }

  $setupmachine {
    // A startup node which immediately transitions to the normal LED and sound loops
    startNode : StateNode
    startNode =N=> {flashPower, playLowBark}

    // The LED nodes which play in the following order: power LED, play LED, advance LED
    flashPower   : FlashLED(RobotInfo::PowerRedLEDMask)
    flashPlay    : FlashLED(RobotInfo::PlayLEDMask)
    flashAdvance : FlashLED(RobotInfo::AdvanceLEDMask)
    
    // The transition loop for the LED nodes which play in normal order
    flashPower =C=> flashPlay =C=> flashAdvance =C=> flashPower
    
    // The button transitions from the LED nodes which play in normal order to the
    // LED nodes which play in reverse order
    flashPower   =B(RobotInfo::BumpLeftButOffset, activateETID)=> revFlashAdvance
    flashPlay    =B(RobotInfo::BumpLeftButOffset, activateETID)=> revFlashPower
    flashAdvance =B(RobotInfo::BumpLeftButOffset, activateETID)=> revFlashPlay

    // The LED nodes which play in the reverse order: power LED, advance LED, play LED
    revFlashPower   : FlashLED(RobotInfo::PowerRedLEDMask)
    revFlashPlay    : FlashLED(RobotInfo::PlayLEDMask)
    revFlashAdvance : FlashLED(RobotInfo::AdvanceLEDMask)
    
    // The transition loop for the LED nodes which play in reverse order
    revFlashPower =C=> revFlashAdvance =C=> revFlashPlay =C=> revFlashPower

    // The button transitions from the LED nodes which play in reverse order to the
    // LED nodes which play in normal order
    revFlashPower   =B(RobotInfo::BumpLeftButOffset, activateETID)=> flashPlay
    revFlashPlay    =B(RobotInfo::BumpLeftButOffset, activateETID)=> flashAdvance
    revFlashAdvance =B(RobotInfo::BumpLeftButOffset, activateETID)=> flashPower

    // The sound nodes which play in the following order: low bark, medium bark, yipper
    playLowBark : MySoundNode("barklow.wav")
    playMedBark : MySoundNode("barkmed.wav")
    playYipBark : MySoundNode("yipper.wav")

    // The transition loop for the sound nodes which play in normal order
    playLowBark =C=> playMedBark =C=> playYipBark =C=> playLowBark

    // The button transitions from the sound nodes which play in normal order to the
    // sound nodes which play in reverse order
    playLowBark =B(RobotInfo::BumpLeftButOffset, activateETID)=> revPlayYipBark
    playMedBark =B(RobotInfo::BumpLeftButOffset, activateETID)=> revPlayLowBark
    playYipBark =B(RobotInfo::BumpLeftButOffset, activateETID)=> revPlayMedBark

    // The sound nodes which play in the reverse order: low bark, yipper, medium bark
    revPlayLowBark : MySoundNode("barklow.wav")
    revPlayMedBark : MySoundNode("barkmed.wav")
    revPlayYipBark : MySoundNode("yipper.wav")

    // The transition loop for the sound nodes which play in reverse order
    revPlayLowBark =C=> revPlayYipBark =C=> revPlayMedBark =C=> revPlayLowBark

    // The button transitions from the sound nodes which play in reverse order to the
    // sound nodes which play in normal order
    revPlayLowBark =B(RobotInfo::BumpLeftButOffset, activateETID)=> playMedBark
    revPlayMedBark =B(RobotInfo::BumpLeftButOffset, activateETID)=> playYipBark
    revPlayYipBark =B(RobotInfo::BumpLeftButOffset, activateETID)=> playLowBark
  }
}

REGISTER_BEHAVIOR(CycleLEDBark);