/************************************************************/
/*    NAME: ENSTA                                              */
/*    ORGN: MIT                                             */
/*    FILE: SimDVL.cpp                                        */
/*    DATE:                                                 */
/************************************************************/

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

using namespace std;

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

SimDVL::SimDVL()
{
   nav_VX=0;
   nav_VY=0;
   nav_VZ=0;
   dvl_accuracy_X=0;
   dvl_accuracy_Y=0;
   dvl_accuracy_Z=0;
   dvl_bias_X=0;
   dvl_bias_Y=0;
   dvl_bias_Z=0;
}

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

bool SimDVL::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 0 // Keep these around just for template
    string comm  = msg.GetCommunity();
    double dval  = msg.GetDouble();
    string sval  = msg.GetString(); 
    string msrc  = msg.GetSource();
    double mtime = msg.GetTime();
    bool   mdbl  = msg.IsDouble();
    bool   mstr  = msg.IsString();
#endif

     if(key == "FOO") 
       cout << "great!";
     else if(key == "NAV_VX")
       nav_VX=msg.GetDouble();
     else if(key == "NAV_VY")
       nav_VY=msg.GetDouble();
     else if(key == "NAV_VZ")
       nav_VZ=msg.GetDouble();

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

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

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

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

bool SimDVL::Iterate()
{
  AppCastingMOOSApp::Iterate();
  // Do your thing here!
  Notify("DVL_VX",nav_VX+MOOSWhiteNoise(dvl_accuracy_X*nav_VX)+dvl_bias_X);
  Notify("DVL_VY",nav_VY+MOOSWhiteNoise(dvl_accuracy_Y*nav_VY)+dvl_bias_Y);
  Notify("DVL_VZ",nav_VZ+MOOSWhiteNoise(dvl_accuracy_Z*nav_VZ)+dvl_bias_Z);
  AppCastingMOOSApp::PostReport();
  return(true);
}

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

bool SimDVL::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;

    bool handled = false;
    if(param == "FOO") {
      handled = true;
    }
    else if (param == "DVL_ACCURACY_X") 
      dvl_accuracy_X = atof(value.c_str());
    else if (param == "DVL_ACCURACY_Y") 
      dvl_accuracy_Y = atof(value.c_str());
    else if (param == "DVL_ACCURACY_Z") 
      dvl_accuracy_Z = atof(value.c_str());
    else if (param == "DVL_BIAS_X") 
      dvl_bias_X = atof(value.c_str());
    else if (param == "DVL_BIAS_Y") 
      dvl_bias_Y = atof(value.c_str());
    else if (param == "DVL_BIAS_Z") 
      dvl_bias_Z = atof(value.c_str());
    else if(param == "BAR") {
      handled = true;
    }

    if(!handled)
      reportUnhandledConfigWarning(orig);

  }
  
  registerVariables();	
  return(true);
}

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

void SimDVL::registerVariables()
{
  AppCastingMOOSApp::RegisterVariables();
  Register("NAV_VX");
  Register("NAV_VY");
  Register("NAV_VZ");
  // Register("FOOBAR", 0);
}


//------------------------------------------------------------
// Procedure: buildReport()

bool SimDVL::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);
}




