/************************************************************/
/*    NAME: Fadlane Maxime                                  */
/*    ORGN: MIT                                             */
/*    FILE: Localization.cpp                                */
/*    DATE:                                                 */
/************************************************************/

#include <iterator>
#include "MBUtils.h"
#include "ACTable.h"
#include "Localization.h"

using namespace std;

//---------------------------------------------------------
// Constructor

Localization::Localization()
{
}


//----------------------------------------------------------
// localization functions

void Localization::Sivia()
{   
    if (k<2) return;
    box X(E);
    //box X = box(interval(-100,100),interval(-100,100));
    box Last_E(E);
    E=EmptyBox(2);
    list<box> L;
    int h=0;
    L.push_back (X);
    while ( !L.empty() )
    {   h++; X=L.front();   L.pop_front();

        //cout << h << " -> "; // debug

        Contract(X,k); 

        if(!X.IsEmpty()){
          if (h<10){   
              box X1(2);  box X2(2);
              Bisect (X,X1,X2);
              L.push_back(X1);  L.push_back(X2);
          }
          else{
            ostringstream fileScout;
            fileScout << scout_nb << ".txt";
            log.open(fileScout.str().c_str(), ios_base::app);
            log << 0.1*k << " " <<  X[1].inf << " " << X[1].sup << " " << X[2].inf << " "<< X[2].sup << "\n";
            log.close();
              
            E=E|X;
          }
        }
    }

     if (E.IsEmpty()){
        E = Last_E + box(interval(-10,10),interval(-10,10));
    //     cout << interval2str(X0[k]) << " " << interval2str(Y0[k]) << " " << interval2str(Range[k]) << '\n';
     }
}

box Localization::phi(const box&X,int k1, int k2)
{
  if (k1==k2) return X;
  if (k2>k1) return(phi(X,k1,k2-1)+dt*(box(V[k2-1]*Cos(Psi[k2-1]),V[k2-1]*Sin(Psi[k2-1]))));
  if (k2<k1) return(phi(X,k1,k2+1)-dt*(box(V[k2]*Cos(Psi[k2]),V[k2]*Sin(Psi[k2]))));
}
//----------------------------------------------------------------
void Localization::CObsk1k2(box& X, int k1, int k2) // to be consistent with the data at time k2
    {   box Xk2(2);
        Xk2=phi(X,k1,k2);
        CinRing(Xk2[1],Xk2[2],X0[k2],Y0[k2],Range[k2]);
        //cout << box2str(phi(Xk2,k2,k1)) << '\n';

        X= X & phi(Xk2,k2,k1);
    }
//----------------------------------------------------------------
void Localization::CObsk1(box& X, int k1) // consistent with a past window of length 5
{  //vector<box> Xk;
   for (int k2=k1; (k2>0)&&(k1-k2<1);k2--)
    {  //box Xk1(X);
       CObsk1k2(X,k1,k2);
       //Xk.push_back(Xk1);
    }
    
    //S_q_in(X,Xk.size(),Xk,outer);
    
}
//----------------------------------------------------------------
void Localization::CdistBuddy(box& X)
{
  box A(Ebuddy);
  interval R=Sqrt(Sqr(Dbuddy)-Sqr(depth_buddy-depth));
  Cdist(R,X,A);
}

//----------------------------------------------------------------
void Localization::Contract(box& X, int k1)
{   
    CObsk1(X,k1);
    CdistBuddy(X);
}

//---------------------------------------------------------
// interval conversion
interval Localization::str2interval(string chaine)
{
  double binf, bsup;
  MOOSValFromString(binf,chaine, "binf");
  MOOSValFromString(bsup,chaine, "bsup");
  return interval(binf,bsup);
}

box Localization::str2box(string chaine)
{
  double bxi, bxs, byi, bys;
  MOOSValFromString(bxi,chaine, "bxi");
  MOOSValFromString(bxs,chaine, "bxs");
  MOOSValFromString(byi,chaine, "byi");
  MOOSValFromString(bys,chaine, "bys");
  return box(interval(bxi,bxs),interval(byi,bys));
}

/*
string Localization::interval2str(interval I)
{
  ostringstream strs;
  strs << "binf=" << I.inf << ",bsup=" << I.sup;
  return strs.str();
}//*/

string Localization::box2str(box B)
{
  ostringstream strs;
  strs << "bxi=" << B[1].inf << ",bxs=" << B[1].sup << "byi=" << B[2].inf << ",bys=" << B[2].sup;
  return strs.str();
}//*/



//********************************************************************
// MOOS FUNCTIONS
//********************************************************************

//---------------------------------------------------------
// Procedure: OnNewMail

bool Localization::OnNewMail(MOOSMSG_LIST &NewMail)
{
  AppCastingMOOSApp::OnNewMail(NewMail);

  MOOSMSG_LIST::iterator p;
  for(p=NewMail.begin(); p!=NewMail.end(); p++) {
    CMOOSMsg &msg = *p;
    string key    = msg.GetKey();

     if(key == "DVL_VX") 
       Vx = str2interval(msg.GetString());
     else if(key == "DVL_VY")
       Vy = str2interval(msg.GetString());
     else if(key == "DVL_VZ") 
       Vz = str2interval(msg.GetString());
     else if(key == "RANGE_BUDDY"){
       Dbuddy = str2interval(msg.GetString());
       newRange = true;
     }
     else if(key == "BOX_BUDDY")
       Ebuddy = str2box(msg.GetString());
     else if(key == "BOAT_X") 
       Boatx = msg.GetDouble();
     else if(key == "BOAT_Y") 
       Boaty = msg.GetDouble();
     else if(key == "BOAT_Z") 
       Boatz = msg.GetDouble();
     else if(key == "IMU_HEADING") 
       heading = str2interval(msg.GetString());
     else if(key == "IMU_PITCH") 
       pitch = str2interval(msg.GetString());
     else if(key == "IMU_ROLL") 
       roll = str2interval(msg.GetString());
     else if(key == "RANGE_B1"){
       Dboat = str2interval(msg.GetString());
       newRange = true;
     }
     else if(key=="SCOUT_NB")
      scout_nb = (int)msg.GetDouble();
     else if(key == "PRESSURE_Z")
        depth = str2interval(msg.GetString());
     else if(key == "PRESSURE_Z_BUDDY")
        depth_buddy = str2interval(msg.GetString());

     else if(key != "APPCAST_REQ") // handle by AppCastingMOOSApp
       reportRunWarning("Unhandled Mail: " + key);
   }
	
   return(true);
}

//---------------------------------------------------------
// Procedure: OnConnectToServer

bool Localization::OnConnectToServer()
{
   registerVariables();
   return(true);
}

//---------------------------------------------------------
// Procedure: Iterate()
//            happens AppTick times per second

bool Localization::Iterate()
{
  AppCastingMOOSApp::Iterate();
  dt = MOOSTime() - time;
  time += dt;
  V.push_back(Center(Vx));//Sqrt(Sqr(Vx)+Sqr(Vy)+Sqr(Vz)));
  Range.push_back(Sqrt(Sqr(Dboat)-Sqr(depth)));
  Psi.push_back(heading);
  Theta.push_back(pitch);
  Phi.push_back(roll);
  X0.push_back(Boatx); 
  Y0.push_back(Boaty); 
  Z0.push_back(Boatz);


  // if(k>10){
  //   V.pop_back();
  //   Range.pop_back();
  //   Psi.pop_back();
  //   Theta.pop_back();
  //   Phi.pop_back();
  //   X0.pop_back();
  //   Y0.pop_back();
  //   Z0.pop_back();
  // }


  if(newRange){
    Sivia();
    newRange = false;
  }
  Notify("ESTIM_X", Center(E[1]));
  Notify("ESTIM_Y", Center(E[2]));
  Notify("ESTIM_Z", Center(depth));
  Notify("BOX_SELF", box2str(E));
  E=phi(E,k,k+1);
  k++;
  return(true);
}

//---------------------------------------------------------
// Procedure: OnStartUp()
//            happens before connection is open

bool Localization::OnStartUp()
{
  AppCastingMOOSApp::OnStartUp();

  STRING_LIST sParams;
  m_MissionReader.EnableVerbatimQuoting(false);
  if(!m_MissionReader.GetConfiguration(GetAppName(), sParams))
    reportConfigWarning("No config block found for " + GetAppName());

  time = MOOSTime();
  dt = 1.0;
  k = 0;
  newRange = false;
  scout_nb = 0;

  E=box(interval(-100,100),interval(-100,100));
  depth = 0.0;
  Vx = 0.0; Vy = 0.0; Vz = 0.0;
  heading = 0.0; pitch = 0.0; roll = 0.0;

  Ebuddy = box(interval(-100,100),interval(-100,100));
  depth_buddy=0.0; Dbuddy = interval(-10,10);

  Dboat = interval(-10,10); 
  Boatx = 0.0; Boaty = 0.0; Boatz = 0.0;

  registerVariables();	
  return(true);
}

//---------------------------------------------------------
// Procedure: registerVariables

void Localization::registerVariables()
{
  AppCastingMOOSApp::RegisterVariables();

  Register("DVL_VX", 0);
  Register("DVL_VY", 0);
  Register("DVL_VZ", 0);

  Register("BOX_BUDDY", 0);
  Register("RANGE_BUDDY", 0);
  Register("PRESSURE_Z_BUDDY", 0);

  Register("BOAT_X", 0);
  Register("BOAT_Y", 0);
  Register("BOAT_Z", 0);

  Register("IMU_HEADING",0);
  Register("IMU_PITCH",0);
  Register("IMU_ROLL",0);

  Register("RANGE_B1", 0);

  Register("PRESSURE_Z",0);

  Register("SCOUT_NB", 0);
}




