#include "RemoteAgentDriver.h"
#include "RemoteAgentTypes.h"
#include "MyAssert.h"
#include "ModemMessage.h"
#include "System.h"
#include "TimeIF.h"
#include "Attributes.h"
#include "StringAttribute.h"
#include "IntegerAttribute.h"
#include "AttributeParser.h"

#include "MMAhrs.h"
#include "MMSeacat.h"
#include "MMSeabirdSBE25.h"
#include "MMAgentRequest.h"
#include "MMActiveBehavior.h"
#include "MMBehavior.h"
#include "MMDepthSensor.h"
#include "MMDvl.h"
#include "MMDriver.h"
#include "MMGps.h"
#include "MMNavigation.h"
#include "MMTailcone.h"
#include "MMOas.h"
#include "MMSvs.h"
#include "MMPower.h"
#include "MMAlarm.h"
#include "MMHoming.h"
#include "MMBluefinBattery.h"
#include "MMString.h"
#include "MMUSBL.h"
#include "MMMissionInfo.h"
#include "MMMissionName.h"
#include "MMEnvironmental.h"
#include "MMFluorometer.h"

#include "ModemSerial.h"
#include "ModemSocket.h"
#include "ModemBroker.h"

#include <signal.h>
#include <time.h>
#include <unistd.h>
#include <sys/name.h>
#include <sys/kernel.h>
#include <sys/types.h>
#include <sys/disk.h>
#include <vector>
#include <string>

#define SLEEP_INTERVAL 		1 	//seconds
#define DEFAULT_PERIOD 		60	//seconds
#define PING_SERVER_PERIOD 	10	//seconds
#define BATTERY_FLAGS 		8 	//number of flags

#define DEFAULT_DEBUG_LEVEL 3

RemoteAgentDriver* RemoteAgentDriver::_gpRemoteAgentDriver = 0;

//////////////////////////////////////
// RemoteAgent implementation
RemoteAgentDriver::RemoteAgentDriver()
{
  _serialPort1 = "/dev/ser5";
  _baudRate1 = 2400;

  _gpRemoteAgentDriver = this;

  m_requestFunctionList[ModemMessage::tMMActiveBehavior] = sendActiveBehavior;
  m_handleFunctionList[ModemMessage::tMMAgentRequest] = handleRequest;
}

RemoteAgentDriver::~RemoteAgentDriver()
{
    clean(_modemBroker);
}

int RemoteAgentDriver::init()
{
    _modemBroker = new ModemBroker();

    printf("RemoteAgentDriver init() - addModems\n");
    if (strlen(_serialPort1) > 0) {
	_modemBroker->addModem(new ModemSerial(_serialPort1,(int)_baudRate1) );
    }
    printf("RemoteAgentDriver init() - done\n");
    return true;
}


void RemoteAgentDriver::run()
{

    init();

    while (true) {
	System::milliSleep(1000);

	Assert(_modemBroker);
	_modemBroker->runCycle();

	if (m_count != time(NULL)) {
	    m_count = time(NULL);

	    try {
		// Check up on connection speed
		long oldSpeed = _connectionSpeed;
		_connectionSpeed = _modemBroker->getBytesPerSecond();
		if (_connectionSpeed < 2000) {
		    m_speed = 0;
		} else {
		    m_speed = 1;
		}


	    } catch (Exception e) {
		printf("exception: %s",e.msg);
	    }
	}

    }
}

void RemoteAgentDriver::send(ModemMessage* pMessage)
{
    Assert(_gpRemoteAgentDriver->_modemBroker);
    /*
    if (_gpRemoteAgentDriver->_modemBroker->getBytesPerSecond() < 2000) {
	_gpRemoteAgentDriver->m_speed = 0;
    } else {
	_gpRemoteAgentDriver->m_speed = 1;
    }
    */
    if (_gpRemoteAgentDriver->_modemBroker->hasConnection()) {
	_gpRemoteAgentDriver->_modemBroker->addMessage(pMessage);
	return;
    }
    delete pMessage;
}

 ////////////////////////////////////////////////////////////////////
void RemoteAgentDriver::receive(const ModemMessage* pMessage)
{
  ModemMessage * mm = new ModemMessage(pMessage);

  // To Do: dispatch table for MBARI messages
  (RemoteAgentDriver::_gpRemoteAgentDriver->*m_handleFunctionList[pMessage->getType()])(mm);

  delete mm;
}

RemoteAgentDriver* RemoteAgentDriver::getRemoteAgentDriver()
{
  return _gpRemoteAgentDriver;
}

//////////////////////////////////////////////////////
void RemoteAgentDriver::sendActiveBehavior()
{
  //    if(!_allowComs) return;
  //  if (checkServer(LayeredControlIFServerName) < 1) return;

    short curState = 1;
    short curBehavior = 2;
    short misState = 3;

    //misState = m_layeredControl->getMissionState();
    //curState = m_layeredControl->currentState();
    //curBehavior = m_layeredControl->currentBehavior();

    MMActiveBehavior act(curState, curBehavior, misState);
    //fprintf(stdout, "Sending active behavior: %d %d\n", act.getState(), act.getBehavior());
    //act.sendRemote();

    printf("RemoteAgentDriver - sending MMActiveBehavior message into the water\n");
    send(new ModemMessage(&act));

    //dwrite(4)("send active behavior message. Type: %d",ModemMessage::tMMActiveBehavior);
}

void RemoteAgentDriver::handleRequest(ModemMessage* mm)
{
  MMAgentRequest arm(mm); // Instantiate agent request msg using ModemMessage

  int req     = arm.getRequest();
  int reqType = arm.getRequestType();

  printf("RemoteAgentDriver - About to handle request type of %d\n", reqType);
  switch (req)
  {
  case MMAgentRequest::Single:
    (RemoteAgentDriver::_gpRemoteAgentDriver->*m_requestFunctionList[reqType])();
    break;

  default:
    printf("Got an unrecognized agent request %d", reqType);
    break;

  }
}