//-*-c++-*-
#ifndef _OdometryDemo_h_
#define _OdometryDemo_h_

#include "DualCoding/DualCoding.h"
#include "Behaviors/Services/DeadReckoningBehavior.h"
#ifdef TGT_CREATE
#include "Localization/CreateMotionModel.h"
#else
#include "Behaviors/Services/DeadReckoningBehavior.h"
#endif
#include "Motion/MotionPtr.h"

using namespace DualCoding;
using namespace std;

class OdometryDemo : public VisualRoutinesStateNode {
private:
  ParticleFilter<LocalizationParticle>::MotionModel * motion;
  ParticleFilter<LocalizationParticle>::MotionModel::particle_collection theparticle;

public:
    OdometryDemo() : VisualRoutinesStateNode("OdometryDemo"),
#ifdef TGT_CREATE
		     motion(new CreateMotionModel<LocalizationParticle>(0.1, 0.1, 0, 0, 0, 0)),
#else
		     motion(new DeadReckoningBehavior<LocalizationParticle>()),
#endif
		     theparticle()
  {
    theparticle.push_back(LocalizationParticle());
  }

  virtual void setup() {
    MotionPtr<XWalkMC> walker;
    MotionManager::MC_ID walkID = addMotion(walker);
#statemachine
  startnode:  XWalkNode($, 0, 400, 0, 0, 0, 0)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, 0, 0.15, -M_PI/2)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, -400, 0, 0)[setMC(walkID)] =C=>
      XWalkNode($, 0, 0, 0, 0, 0.15, M_PI/2)[setMC(walkID)] =C=>

      SpeechNode($,"Done")
#endstatemachine
  }

  virtual void DoStart() {
    cout << "Starting motion model demo." << endl;

    if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion))
      motBeh->start();

    erouter->addTimer(this, 0, 1000);
  }

  virtual void processEvent(const EventBase& event)
  {
    if (event.getGeneratorID() == EventBase::timerEGID) {
      // update the dead reckoning motion
      LocalizationParticle old = theparticle[0];
      motion->updateMotion(theparticle);
      
      NEW_SHAPE(odomPath, LineData, new LineData(worldShS, Point(old.x, old.y), Point(theparticle[0].x, theparticle[0].y)));
    }
  }

  static std::string getClassDescription() {
    return "Simple odometry test.";
  }

};

#endif