/****************************************************************************/
/* Copyright (c) 2007 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : StatePublisher.h                                              */
/* Author   :                                                               */
/* Project  : Onboard Deliberative Autonomy                                 */
/* Version  : 1.0                                                           */
/* Created  : 02/05/2007                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <math.h>
#include <unix.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/ioctl.h> // IO Control of socket.
#include <time.h>
#include "System.h"
#include "StatePublisher.h"
#include "StatePacket.h"
#include "LayeredControl/VcsMessage.h"
#include "Syslog.h"
#include <StringAttribute.h>
#include <IntegerAttribute.h>
#include <AttributeParser.h>

#define NGULPERS 10

double StatePublisher::getTime() {
   struct timespec now;
   double value; 
   clock_gettime(CLOCK_REALTIME, &now);
   value = now.tv_sec;
   value += now.tv_nsec/1e9;
   return value;  
}

StatePublisher::StatePublisher(char* config)
   : PeriodicTask("statePublisher"), _config(config), _attributes(config),
     _samplePeriod(500), _amcIP(NULL), _amcPort(0), _amcSocket(-1), 
     _clusterName(NULL), _msgs(NULL) {

  _statePacketSender = new StatePacketSender();
  _startTime = getTime();

  _msgs = new VcsMessage(MessageQueue::ReadWrite, VcsMessageQueueName);
  //
  // Initialize:
  //
  addPeriodicCallback(_samplePeriod, (CallbackMethod)StatePublisher::callback);
}


StatePublisher::~StatePublisher()
{
  
  delete _statePacketSender;

  if( _amcSocket>0 ) {
    close(_amcSocket);
    _amcSocket = -1;
  }
  
  if(_tailConeIF)    delete _tailConeIF;
  if(_navigationIF)  delete _navigationIF;
  if(_batteryIF)     delete _batteryIF;
  if(_hydroscatIF)   delete _hydroscatIF;
  if(_ctdIF)         delete _ctdIF;
  if(_isusIF)        delete _isusIF;
  if(_clusterIF)     delete _clusterIF;
  if( _msgs )
    delete _msgs; 
}

int StatePublisher::init()
{
  Boolean debug = _verbose;

  char configFile[100];
  strcpy(configFile, System::configurationFile(_config));

  createCfgAttributes();
  loadConfigFile(configFile);
  System::copyToLogDir(configFile);
  reportCfgAttributes();

  // Create socket
  //
  setupSocket();
  if (_amcSocket < 0)
  {
    Syslog::write("StatePublisher: Unable to initialize interface to AMC");
    return -1;
  }

  // Create interfaces to vehicle servers
  //
  Syslog::write("StatePublisher: Acquiring interfaces ctd, nav, and tailcone servers");
  int timeout = 10;
#if 0
  try 
  {
    _tailConeIF = new TailConeIF(TailConeIFServerName, timeout);
  }
  catch (...) 
  {
    Syslog::write("StatePublisher: Unable to initialize tailcone interface");
    _tailConeIF = 0;
  }
#else 
  _tailConeIF = 0;
#endif 

  try 
  {
    _navigationIF = new NavigationIF(NavigationIFServerName, timeout);
  }
  catch (...) 
  {
    Syslog::write("StatePublisher: Unable to initialize nav interface");
    _navigationIF = 0;
  }

#if 0
  try 
  {
    _batteryIF = new BluefinBatteryIF(BluefinBatteryIFServerName, timeout);
  }
  catch (...) 
  {
    Syslog::write("StatePublisher: Unable to initialize battery interface");
    _batteryIF = 0;
  }
#else 
  _batteryIF = 0;
#endif 
  
  try 
  {
    _hydroscatIF = new HydroscatIF(HydroscatIFServerName, timeout);
  }
  catch (...) 
  {
    Syslog::write("StatePublisher: Unable to initialize hydroscat interface");
    _hydroscatIF = 0;
  }

  try 
  {
    Syslog::write("StatePublisher: Connecting to CTD server \"%s\"", _ctdServerName);
    _ctdIF = new CtdIF("ctd", _ctdServerName, timeout);
  }
  catch (...) 
  {
    Syslog::write("StatePublisher: Unable to initialize ctd interface");
    _ctdIF = 0;
  }

  try {
    _isusIF = new IsusIF(IsusIFServerName, timeout);
  } catch(...) {
    Syslog::write("StatePublisher: Unable to initialize isus interface");
    _isusIF = 0;
  }
    

  try 
  {
    _clusterIF = new ClusterIF(ClusterIFServerName, timeout);
  }
  catch (...) 
  {
    Syslog::write("StatePublisher: Unable to initialize cluster interface");
    _clusterIF = 0;
  }

  try {
    _smartSampler = new SmartSamplerIF(SmartSamplerIFServerName, timeout);
  } catch(...) {
    Syslog::write("StatePublisher: Unable to initialize smart sampler interface");
    _smartSampler = 0;
  }
  
  return 0;
}

int StatePublisher::setupSocket()
{
  Boolean debug = _verbose;
  
  int  sockfd;

  // Initialize our socket to an unuseable value
  //
  _amcSocket = -1;
  
  // Attempt to setup a UDP client socket on the AMC port
  // We will send state packets to the AMC server
  //
  dprintf("StatePublisher: Socket params - IP:%s, Port:%d", _amcIP, _amcPort);
  
  bzero((char*) &_servaddr, sizeof(_servaddr));
  _servaddr.sin_family      = AF_INET;
  inet_aton(_amcIP, &_servaddr.sin_addr);
  _servaddr.sin_port        = htons((short)_amcPort);

  if (0 == _servaddr.sin_addr.s_addr)
  {
    dprintf("StatePublisher: Host %s translated to addr: %d", _amcIP,
            _servaddr.sin_addr.s_addr);
    Syslog::write("StatePublisher: Could not resolve ip address");
    return -1;
  }

  // Get a socket resource
  //
  if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
  {
    dprintf("StatePublisher: socket init failed (socket() call = %d)", sockfd);
    Syslog::write("StatePublisher: Failed to initialize socket");
    return sockfd;
  }

  // No buffering
//   size_t packetSize = 0;
//   if( setsockopt(sockfd, SOL_SOCKET, SO_SNDBUF, &packetSize, sizeof(packetSize))<0 ) {
//     close(sockfd);
//     dprintf("StatePublisher: change socket buffer size failed (ERRNO = %d)", errno);
//     Syslog::write("StatePublisher: failed to change socket buffer size.");
//     return -1;
//   }

//   if( connect(sockfd, (struct sockaddr const *)&_servaddr, sizeof(struct sockaddr_in))<0 ) {
//     close(sockfd);
//     dprintf("StatePublisher: socket connection to server failed (ERRNO = %d)", errno);
//     Syslog::write("StatePublisher: socket connection failed.");
//     return -1;
//   }

  // Socket is working
  //
  Syslog::write("StatePublisher: Socket created on fd %d", sockfd);
  _amcSocket = sockfd;  
  
  return 0;
}

void StatePublisher::callback()
{
  static double start = 0.0, pred = 0.0;
  static unsigned long nsent = 0;
  double cur;
  struct timespec mtime;
  
  clock_gettime(CLOCK_REALTIME, &mtime);
  cur = mtime.tv_sec + (mtime.tv_nsec / 1e9);
  if( 0.0==start )
    start = cur;
  cur = cur - start;
  
  //Syslog::write("StatePublisher [t=%g, delta=%g]: Updating state vector", cur, cur-pred);
  pred = cur;
  Boolean debug = True;

  if (_amcSocket < 0) return;
  
  if( handleLCMessages()==False ) {
    Syslog::write("StatePublisher - mission aborted");
    return;
  }
  // Populate the state vector with fresh vehicle data
  //
  
  NavigationIF::Position p;
  NavigationIF::Attitude a;

  if (_navigationIF) {
    _navigationIF->state(&p, &a);
  } else 
	Syslog::write("StatePublisher: No navigation!!!!!\n");


  //
  // Position struct in StatePacket.h:
  _statePacket.position.latitude = p.latitude;
  _statePacket.position.longitude = p.longitude;
  _statePacket.position.x = p.x;
  _statePacket.position.y = p.y;
  _statePacket.position.z = p.z;
  //Syslog::write("SatePublisher: position (%lf, %lf, %lf)\n", p.x, p.y, p.z);
//   _statePacket.position.altitude = p.altitude;
//   _statePacket.position.xRate = p.xRate;
//   _statePacket.position.yRate = p.yRate;
//   _statePacket.position.zRate = p.zRate;
//   _statePacket.position.depthRate = p.depthRate;
//   _statePacket.position.altitudeRate = p.altitudeRate;
//   _statePacket.position.updateTimeSec = p.updateTime.seconds;
//   _statePacket.position.updateTimeNanosec = p.updateTime.nanoSeconds;
  //
  // Attitude struct in StatePacket.h:
//   _statePacket.attitude.roll    = a.roll;
//   _statePacket.attitude.pitch   = a.pitch;
//   _statePacket.attitude.yaw     = a.yaw;
//   _statePacket.attitude.omega_B_x = a.omega_B_x;
//   _statePacket.attitude.omega_B_y = a.omega_B_y;
//   _statePacket.attitude.omega_B_z = a.omega_B_z;
//   _statePacket.attitude.updateTimeSec = p.updateTime.seconds;
//   _statePacket.attitude.updateTimeNanosec = p.updateTime.nanoSeconds;

#if 0
  double omega;
  double elevator;
  double rudder;
  TimeIF::TimeSpec time;
  if (_tailConeIF) {
    _tailConeIF->actual(&omega, &elevator, &rudder, &time);
    
    _statePacket.tailCone.omega     = omega;
    _statePacket.tailCone.elevator  = elevator;
    _statePacket.tailCone.rudder    = rudder;
    _statePacket.tailCone.updateTimeSec  = time.seconds;
    _statePacket.tailCone.updateTimeNanosec = time.nanoSeconds;
  } else {
    // No connection ot tailcone
  }

  short batNum = 1;
  BluefinBatteryIF::BfBattData batteryData;
  
  if(_batteryIF) {
    _batteryIF->getBatteryData( batNum, &batteryData );

    // Map battery state from the enum defined in IDL to the enum defined
    // in StatePacket
    switch (batteryData.state) {
    case BluefinBatteryIF::Unknown:
      _statePacket.batteryData.state = BattUnknown;
      break;

    case BluefinBatteryIF::Shutdown:
      _statePacket.batteryData.state = BattShutdown;
      break;

    case BluefinBatteryIF::Discharging:
      _statePacket.batteryData.state = BattDischarging;
      break;

    case BluefinBatteryIF::Charging:
      _statePacket.batteryData.state = BattCharging;
      break;

    case BluefinBatteryIF::Balancing:
      _statePacket.batteryData.state = BattBalancing;
      break;

    default:
      _statePacket.batteryData.state = BattUnknown;
    }

    // Map battery fault code from the enum defined in IDL to the enum defined
    // in StatePacket
//      switch (batteryData.fault) {

//      case BluefinBatteryIF::Clear:
//        _statePacket.batteryData.fault = BattClear;
//        break;

//     case BluefinBatteryIF::OverVolt:
//        _statePacket.batteryData.fault = BattOverVolt;
//        break;

//      case BluefinBatteryIF::UnderVolt:
//        _statePacket.batteryData.fault = BattUnderVolt;
//        break;

//      case BluefinBatteryIF::OverCurrent:
//        _statePacket.batteryData.fault = BattOverCurrent;
//        break;

//      case BluefinBatteryIF::MaxCellOverVolt:
//        _statePacket.batteryData.fault = BattMaxCellOverVolt;
//        break;

//      case BluefinBatteryIF::MinCellUnderVolt:
//        _statePacket.batteryData.fault = BattMinCellUnderVolt;
//        break;

//      case BluefinBatteryIF::OverTemp:
//        _statePacket.batteryData.fault = BattOverTemp;
//        break;

//     case BluefinBatteryIF::WaterLeak:
//        _statePacket.batteryData.fault = BattWaterLeak;
//        break;

//      case BluefinBatteryIF::HardwareProt:
//        _statePacket.batteryData.fault = BattHardwareProt;
//        break;

//      case BluefinBatteryIF::HardwareTransient:
//        _statePacket.batteryData.fault = BattHardwareTransient;
//        break;

//      case BluefinBatteryIF::WatchdogTimeout:
//        _statePacket.batteryData.fault = BattWatchdogTimeout;
//        break;


//      default:
//        _statePacket.batteryData.fault = BattUnknownFault;
//      }

//      _statePacket.batteryData.battSN = batteryData.battSN;
     _statePacket.batteryData.voltage = batteryData.voltage;
//      _statePacket.batteryData.current = batteryData.current;
//      _statePacket.batteryData.temp = batteryData.temp;
     _statePacket.batteryData.minvoltage = batteryData.minvoltage;
//      _statePacket.batteryData.maxvoltage = batteryData.maxvoltage;
//      _statePacket.batteryData.waterleak = batteryData.waterleak;
//      _statePacket.batteryData.capacity = batteryData.capacity;
  }
  else {
    // No connection to battery service?
//    Syslog::write("StatePublisher: no connection to battery service");
  }
#endif 

   HydroscatIF::Data hydroscatData;
   if(_hydroscatIF) {
     _hydroscatIF->getData( &hydroscatData );

     _statePacket.hydroscat.valid = hydroscatData.deviceReady 
       /* && hydroscatData.calculated.ready */ ;
     if( _statePacket.hydroscat.valid ) {
       _statePacket.hydroscat.bb470 = hydroscatData.calculated.bb470;
       _statePacket.hydroscat.bb676 = hydroscatData.calculated.bb676;
       _statePacket.hydroscat.fl = hydroscatData.calculated.fl676_uncorr;
     }
   } else {
     _statePacket.hydroscat.valid = False;
   }

  unsigned short clusterData = 0;
  double clusterDist;

  // A value of -1 implies that there is a problem ...
  _statePacket.cluster.id = -1;

  if(_clusterIF ) {
#if 0
    dprintf("StatePublisher: getting cluster %s", _clusterName);
#endif
    if( DeviceIF::Ok==_clusterIF->getCluster(_clusterName, &clusterData, &clusterDist) ) {
#if 0
      dprintf("StatePublisher: [%s]=%d", _clusterName, clusterData);
#endif
      _statePacket.cluster.id = clusterData;
    }
  }

  if( _smartSampler ) {
    SmartSamplerIF::GulperInfoArrayType state;
    _smartSampler->gulperState(state);
    for(int i=0; i<NGULPERS; ++i) {
      switch( state[i].curstate ) {
      case SmartSamplerIF::Free:
	Syslog::write("StatePublisher: gulper %d is free.", i);
	_statePacket.gulper[i].state = g_Free;
	break;
      case SmartSamplerIF::Allocated:
	Syslog::write("StatePublisher: gulper %d is allocated.", i);
	_statePacket.gulper[i].state = g_Allocated;
	break;
      case SmartSamplerIF::Fired:
	Syslog::write("StatePublisher: gulper %d is fired.", i);
	_statePacket.gulper[i].state = g_Fired;
	break;
      default:
	Syslog::write("StatePublisher: gulper %d is unknown.", i);
	_statePacket.gulper[i].state = g_Unknown;
      }
    }
  } else {
    for(int i=0; i<NGULPERS; ++i) {
      _statePacket.gulper[i].state = g_Unknown;
    }
  }

  // CTD data
  //
   TimeIF::TimeSpec tsc, tst;
   float c, t, d, cf, tf, v1, v2, v3, v4, v5, v6, salinity;
   c = t = d = 0.;

   _statePacket.ctd.valid = False;
   if (_ctdIF) {
     Boolean ret = (DeviceIF::Ok==_ctdIF->get_CTD_data(&c, &t, &d, 
						       &cf, &tf, &tsc, &tst,
						       &v1, &v2, &v3, &v4, &v5, &v6));
     if( ret ) {
       _ctdIF->salinity(&salinity);
       _statePacket.ctd.valid = True;
       _statePacket.ctd.c = c;
       _statePacket.ctd.t = t;
       _statePacket.ctd.d = d;
       _statePacket.ctd.salinity = salinity;
     }
   }

   _statePacket.isus.valid = False;
   if (_isusIF ) {
     IsusIF::Data isus_data;
     Boolean ret = (DeviceIF::Ok==_isusIF->getData(&isus_data));
     
     if( ret ) {
       _statePacket.isus.valid = isus_data.deviceReady && !isus_data.badComms;
       _statePacket.isus.nitrate = isus_data.nitrate;
     }
   }
  
#if 0
   dprintf("StatePublisher: CtdIF says... Temp = %f, depth = %f", t, d);
 
  if (debug) {
    dprintf("printPacket()");
    printPacket(_statePacket);
  }
#endif
  //printPacket(_statePacket); 
  // Encode state packet and send it over socket
  if (_statePacketSender->send(&_statePacket, _amcSocket, _servaddr) == -1) {
    clock_gettime(CLOCK_REALTIME, &mtime);
    cur = mtime.tv_sec + (mtime.tv_nsec / 1e9)-start;
    Syslog::write("At %g StatePublisher ERROR: packet sender failed", cur);
  }
  else {
    ++nsent;
    clock_gettime(CLOCK_REALTIME, &mtime);
    cur = mtime.tv_sec + (mtime.tv_nsec / 1e9)-start;
    if( 0==nsent%10 ) 
      dprintf("At %g StatePublisher: sent update to AMC (%d)", cur, nsent);
  }
}


Boolean StatePublisher::handleLCMessages() {
  VcsMessage::Message msg;

  while( _msgs->read(&msg)>0 ) {
    switch( msg._msg ) {
    case VcsMessage::MissionAborted:
      Syslog::write("StatePublisher::handleLCMessages() - Mission Aborted");
      ackAmc(-1, 'A'); // A for aborted
      return False;
    case VcsMessage::BehaviorStarted:
      if( msg._behaviorId >= 0 ) {
	ackAmc(msg._behaviorId, 'S'); // S for started
	struct timespec mtime; double cur;
        clock_gettime(CLOCK_REALTIME, &mtime);
        cur = mtime.tv_sec + (mtime.tv_nsec/1e9);
	Syslog::write("%f: StatePublisher : started %ld\n", cur, msg._behaviorId);
      }
      break;
    case VcsMessage::BehaviorFinished:
      if( msg._behaviorId >= 0 ) { 
	ackAmc(msg._behaviorId, 'F'); // F for finished
	Syslog::write("StatePublisher : finished %ld\n", msg._behaviorId);
      }
      break;
    default:
      Syslog::write("StatePublisher::handleLCMessages - unknown command %d", msg._msg);
    }
  }
  return True;
}

void StatePublisher::ackAmc(int32_t id, char type) {
  if( _statePacketSender->send(id, type, _amcSocket, _servaddr) == -1 ) {
    Syslog::write("StatePublisher: failed to send state update %c:%l", type, id);  
  }
}

// ########### Configuration file attributes section ###########

// Create attributes in our attributes member
//
void StatePublisher::createCfgAttributes()
{
  _attributes.add( new IntegerAttribute("PublishPeriod",
                   "StatePublisher Period", &_samplePeriod, DEFAULT_PERIOD ) );
  _attributes.add( new StringAttribute  ("AMC_IP",
                   "IP of the AMC", &_amcIP, DEFAULT_IP ) );
  _attributes.add( new IntegerAttribute  ("AMC_PORT",
                   "Receiving port on the AMC", &_amcPort, DEFAULT_PORT ) );

  _attributes.add( new StringAttribute ("ClusterName",
		   "Name of the classifier used", &_clusterName, DEFAULT_CLUSTER_NAME));
  // Gives the name of the CT server we want to connect to
  //    By defualt it would be the default name but in operation it should
  //    be either ctdDriver or ctdDriver2
  _attributes.add( new StringAttribute("CtdName", "Name of the CTD server to connect to", 
				       &_ctdServerName, CtdIFServerName));

}

// Load the config attributes and do some sanity checking
//
void StatePublisher::loadConfigFile(const char* configFile)
{
  AttributeParser::parse(configFile, &_attributes); 

  // Check attribute values against limits
  //
  if (_samplePeriod < 100)
  {
    Syslog::write("StatePublisher: Period attr=%d is out of bounds, ",
      "defaulting to %d milliseconds", _samplePeriod, DEFAULT_PERIOD);
    _samplePeriod = DEFAULT_PERIOD;
  }

  if (_amcPort < 200)
  {
    Syslog::write("StatePublisher: Port attr=%d is out of bounds, ",
      "defaulting to %d", _amcPort, DEFAULT_PORT);
    _amcPort = DEFAULT_PORT;
  }
}

// Record the attributes in Syslog
//
void StatePublisher::reportCfgAttributes()
{
     Syslog::write("StatePublisher -- configuration:\n"
       "\tPublishPeriod = %d ms\n"
		   "\tAMC_IP        = %s\n"
		   "\tAMC_PORT      = %d\n"
		   "\tClusterName   = %s\n",
		   _samplePeriod, _amcIP, _amcPort, _clusterName);
}



void StatePublisher::printPacket(StatePacket &statePacket) {
#if 0
  printf("\n\n");
  printf("Navigation update time:  %15.2f\n", 
	 (double)statePacket.position.updateTimeSec +
	 ((double)statePacket.position.updateTimeNanosec)/1.e9);

  printf("  Attitude in radians and r/s:\n");
  printf("     roll:  %5.2f\n", statePacket.attitude.roll);
  printf("    pitch:  %5.2f\n", statePacket.attitude.pitch);
  printf("      yaw:  %5.2f\n", statePacket.attitude.yaw);
  printf("omega_B_x:  %5.2f\n", statePacket.attitude.omega_B_x);
  printf("omega_B_y:  %5.2f\n", statePacket.attitude.omega_B_y);
  printf("omega_B_z:  %5.2f\n", statePacket.attitude.omega_B_z);
  #endif
  #if 0
  printf("  Position:\n");
  printf("     latitude:  %9.4f\n", statePacket.position.x);
  printf("    longitude:  %9.4f\n", statePacket.position.y);
  printf("        depth:  %9.4f\n", statePacket.position.z);
  #endif
  #if 0

  printf("Battery serial#: %d state: %d, fault: %d\n", 
	 statePacket.batteryData.battSN, 
	 statePacket.batteryData.state, 
	 statePacket.batteryData.fault); 

  printf("Battery voltage: %.3f, current: %.3f, temp: %.1f\n",
	 statePacket.batteryData.voltage,
	 statePacket.batteryData.current,
	 statePacket.batteryData.temp); 

  printf("Battery minVoltage: %.3f, maxVoltage: %.3f, capacity: %.3f\n",
	 statePacket.batteryData.minvoltage,
	 statePacket.batteryData.maxvoltage,
	 statePacket.batteryData.capacity); 

  printf("Hydroscat snorm1: %d, snorm2: %d, snorm3: %d\n",
	 statePacket.hydroscat.snorm1,
	 statePacket.hydroscat.snorm2,
	 statePacket.hydroscat.snorm3);

  printf("CTD valid: %d, c: %.5f, t: %.2f, d: %.2f\n",
	 statePacket.ctd.valid,
	 statePacket.ctd.c,
	 statePacket.ctd.t,
	 statePacket.ctd.d);

  printf("Cluster valid: %d, id: %d\n", 
	 statePacket.cluster.valid,
	 statePacket.cluster.id);

#endif
}