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

// #include <iterator>
// #include "MBUtils.h"
// #include "ACTable.h"
// #include "Controller.h"
// #include <math.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 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 == "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 = msg.GetDouble();
//      }
//      if(key == "IMU_HEADING"){
//        estim_heading = msg.GetDouble();
//      }
//      if(key == "IMU_PITCH"){
//        estim_pitch = msg.GetDouble();
//      }

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

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

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

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

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

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

//   AppCastingMOOSApp::PostReport();
//   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());

//   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 == "KP_VX") {
//       Kp_vx = atof(value.c_str());
//     }
//     else if(param == "KP_HEADING") {
//       Kp_heading = atof(value.c_str());
//     }
//     else if(param == "KP_PITCH") {
//       Kp_pitch = atof(value.c_str());
//     }
//     else{
//       reportUnhandledConfigWarning(orig);
//     }

//   }
  
//   registerVariables();	
//   return(true);
// }

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

// void Controller::registerVariables()
// {
//   AppCastingMOOSApp::RegisterVariables();
//   // Register("FOOBAR", 0);
//   Register("IMU_HEADING");
//   Register("IMU_PITCH");
//   Register("DVL_VX");

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


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

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




