#include "Behaviors/StateMachine.h"
#include "Behaviors/Demos/Navigation/VeeTags.h"

$nodeclass VeeNavigate : VeeTags {

	virtual void postStart() {
		VeeTags::postStart();
		particleFilter->resetFilter(100);
	}

  $nodeclass ReportPositionNode : VisualRoutinesStateNode : doStart {
    AngTwoPi heading = theAgent->getOrientation();
    cout << "Agent now at " << theAgent->getCentroid() << " hdg " << heading
	 << " (= " << float(heading)*180/M_PI << " deg.)" 
	 << "  std. dev. " << sqrt(((ShapeBasedParticleFilter*)particleFilter)->getVariance().x) << " mm, "
	 << float(((ShapeBasedParticleFilter*)particleFilter)->getVariance().theta) * 180 / M_PI << " deg."
	 << endl;
    postStateCompletion();
  }

  $setupmachine {
    rundemo: Localize =C=> ReportPositionNode =C=> Turn(deg2rad(90.0)) =C=> WalkForward(1250) =C=>
    Turn(deg2rad(-90.0)) =C=> WalkForward(1500) =C=>
    Turn(deg2rad(-90.0)) =C=> WalkForward(2500) =C=>
    Turn(deg2rad(-90.0)) =C=> WalkForward(1500) =C=>
    Turn(deg2rad(-90.0)) =C=> WalkForward(1250) =C=>
    Turn(deg2rad(-90.0)) =C=> ReportPositionNode =C=> Localize =C=> ReportPositionNode
  }
}

REGISTER_BEHAVIOR(VeeNavigate);