/************************************************************/
/*    NAME: BELLAICHE Adrien                                */
/*    ORGN: MIT                                             */
/*    FILE: Behaviour.cpp                                   */
/*    DATE:                                                 */
/************************************************************/

#include <iterator>
#include "MBUtils.h"
#include "ACTable.h"
#include "Behaviour.h"
#include "math.h"

using namespace std;

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

Behaviour::Behaviour()
{
  des_range = 0, des_angle = 0, des_depth = 0;
  px = 0, py = 0, pz = 0;
  boatx = 0, boaty = 0, boatz = 0, boath = 0, boatw = 0, boats = 0;
}

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

bool Behaviour::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 == "ESTIM_X") {
      px = msg.GetDouble();
    } else if(key == "ESTIM_Y") {
      py = msg.GetDouble();
    } else if(key == "ESTIM_Z") {
      pz = msg.GetDouble();
    } 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 == "BOAT_HEADING") {
      boath = msg.GetDouble();
    } else if(key == "BOAT_HEADING_DOT") {
      boatw = msg.GetDouble();
    } else if(key == "BOAT_SPEED") {
      boats = msg.GetDouble();
    } else if(key != "APPCAST_REQ") // handle by AppCastingMOOSApp
      reportRunWarning("Unhandled Mail: " + key);
    }

   return(true);
}

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

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

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

bool Behaviour::Iterate()
{
  AppCastingMOOSApp::Iterate();
  double qx = boatx + des_range*(cos(boath)*cos(des_angle) - sin(boath)*sin(des_angle));
  double qy = boaty + des_range*(sin(boath)*cos(des_angle) + cos(boath)*sin(des_angle));
  double qz = des_depth;

  double qdotx = boats*cos(boath) - (qy-boaty)*boatw;
  double qdoty = boats*sin(boath) + (qx-boatx)*boatw;

  double wx = qdotx + 0.2*(qx-px);
  double wy = qdoty + 0.2*(qy-py);
  double wz = 0.2*(qz-pz);

  double des_speed = sqrt(wx*wx+wy*wy+wz*wz);
  double des_heading = atan2(wy,wx);
  double des_pitch = 0.0;//atan(des_depth-pz)*2/PI;
  
  /*
  if(des_speed > 1.0){
    des_speed=1.0;
  }
  if(fabs(des_pitch) > 1.0){
    des_pitch=des_pitch/fabs(des_pitch);
  }//*/

  Notify("DESIRED_VX", des_speed);
  Notify("DESIRED_HEADING", des_heading);
  Notify("DESIRED_PITCH", des_pitch);

  log.open("log.txt", ios_base::app);
  log << t << " " << boatx << " " << boaty << " " << px << " " << py << " " << des_speed << " " << des_heading <<"\n";
  log.close();
  t+=0.1; // arbitrary time (10 times per seconds)
  //AppCastingMOOSApp::PostReport();
  return(true);
}

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

bool Behaviour::OnStartUp()
{
  AppCastingMOOSApp::OnStartUp();
  STRING_LIST sParams;
  m_MissionReader.EnableVerbatimQuoting(false);
  if(!m_MissionReader.GetConfiguration(GetAppName(), sParams))
    reportConfigWarning("No config block found for " + GetAppName());

  STRING_LIST::iterator p;
  for(p=sParams.begin(); p!=sParams.end(); p++) {
    string orig  = *p;
    string line  = *p;
    string param = toupper(biteStringX(line, '='));
    string value = line;
    if(param == "DES_ANGLE") {
      des_angle = atof(value.c_str());
    } else if(param == "DES_DEPTH") {
      des_depth = atof(value.c_str());
    } else if(param == "DES_RANGE") {
      des_range = atof(value.c_str());
    } else {
      reportUnhandledConfigWarning(orig);
    }
  }
  t=0.0;
  registerVariables();
  return(true);
}

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

void Behaviour::registerVariables()
{
  AppCastingMOOSApp::RegisterVariables();
  Register("ESTIM_X",0);
  Register("ESTIM_Y",0);
  Register("ESTIM_Z",0);
  Register("BOAT_HEADING",0);
  Register("BOAT_HEADING_DOT",0);
  Register("BOAT_X",0);
  Register("BOAT_Y",0);
  Register("BOAT_Z",0);
  Register("BOAT_SPEED",0); 
}


//------------------------------------------------------------
// Procedure: buildReport()
/*
bool Behaviour::buildReport() 
{
  m_msgs << "============================================ \n";
  m_msgs << "File:                                        \n";
  m_msgs << "============================================ \n";

  ACTable actab(4);
  actab << "Alpha | Bravo | Charlie | Delta";
  actab.addHeaderLines();
  actab << "one" << "two" << "three" << "four";
  m_msgs << actab.getFormattedString();

  return(true);
}//*/


