/************************************************************/
/*    NAME: Thomas                                              */
/*    ORGN: MIT                                             */
/*    FILE: Controller.cpp                                        */
/*    DATE:                                                 */
/************************************************************/

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

using namespace std;

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

Controller::Controller()
{
}

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

bool Controller::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 == "DESIRED_VX"){
       desired_vx = msg.GetDouble();
     }
     if(key == "DESIRED_HEADING"){
       desired_heading = msg.GetDouble();
     }
     if(key == "DESIRED_PITCH"){
       desired_pitch = msg.GetDouble();
     }
     if(key == "DVL_VX"){
       estim_vx = Center(str2interval(msg.GetString()));
     }
     if(key == "IMU_HEADING"){
       estim_heading = Center(str2interval(msg.GetString()));
     }
     if(key == "IMU_PITCH"){
       estim_pitch = Center(str2interval(msg.GetString()));
     }

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

interval Controller::str2interval(string chaine)
{
  double binf, bsup;
  MOOSValFromString(binf,chaine, "binf");
  MOOSValFromString(bsup,chaine, "bsup");
  return interval(binf,bsup);
}

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

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

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

bool Controller::Iterate()
{
  AppCastingMOOSApp::Iterate();
  
  desired_thrust = atan((desired_vx - estim_vx))*2/PI;
  desired_vrudder = - atan( tan( (desired_heading - estim_heading)/2 ) )*2/PI;
  desired_hrudder = - atan(tan( (desired_pitch - estim_pitch)/2) )*2/PI;

  Notify("DESIRED_THRUST", desired_thrust);
  Notify("DESIRED_HRUDDER", desired_hrudder);
  Notify("DESIRED_VRUDDER", desired_vrudder);

  return(true);
}

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

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

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

  registerVariables();	
  return(true);
}

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

void Controller::registerVariables()
{
  AppCastingMOOSApp::RegisterVariables();
  
  Register("IMU_HEADING");
  Register("IMU_PITCH");
  Register("DVL_VX");

  Register("DESIRED_HEADING");
  Register("DESIRED_PITCH");
  Register("DESIRED_VX");
}




