/************************************************************/
/*    NAME: ENSTA                                              */
/*    ORGN: MIT                                             */
/*    FILE: Localization.h                                          */
/*    DATE: December 29th, 1963                             */
/************************************************************/

#ifndef Localization_HEADER
#define Localization_HEADER

#include <fstream>
#include <iostream>
#include "interval.h"
#include "box.h"  
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include <vector>
#include <string>
#include <sstream>

class Localization : public AppCastingMOOSApp
{
 public:
   Localization();
   ~Localization() {};

   ofstream log;
   interval str2interval(string chaine);
   box str2box(string chaine);
   //string interval2str(interval I);
   string box2str(box B);
   void CObsk1(box& X, int k1);
   void CObsk1k2(box& X, int k1, int k2);
   box phi(const box&X,int k1, int k2);
   void Sivia();
   void Contract(box& X, int k1);
   void CdistBuddy(box& X);

 protected: // Standard MOOSApp functions to overload  
   bool OnNewMail(MOOSMSG_LIST &NewMail);
   bool Iterate();
   bool OnConnectToServer();
   bool OnStartUp();

 protected: // Standard AppCastingMOOSApp function to overload 

 protected:
   void registerVariables();


 private: // Configuration variables

 private: // State variables
  bool                newRange;
 	int                 k;
 	double 				      dt, time;
 	interval 			      Dboat,Dbuddy;
 	double				      Boatx,Boaty,Boatz;
 	vector<double> 		  X0,Y0,Z0; 				// la position du bateau
  box					        Ebuddy, E; 				// estimées de la position des scouts
  vector<interval>	  Range;					// distance au bateau
  interval  				  heading,pitch,roll; 	// attitude du scout
  vector<double> 	    V;
  vector<interval>		Psi,Theta,Phi;
  interval			      Vx,Vy,Vz;				// vitesse du scout
  interval            depth, depth_buddy;
  int                 scout_nb;
};

#endif 
