#include "VehicleTurnsLogger.h"
#include "tiburon.h"
#include "vehicleTurnsDM.h"
#include "DmErrno.h"
#include "vxUtils.h"
#include <logLib.h>             /* vxWorks message logging declarations      */

extern "C"
{
# include "turnsUtils.h"
};


#define dprint if (debug) logMsg


VehicleTurnsLogger::VehicleTurnsLogger()
{
  char errorBuf[errorMsgSize];

  _dmInitialTwist = new DmFlt32Object(INITIAL_VEHICLE_TURNS_SUM_DM);
  _dmTwist = new DmFlt32Object(VEHICLE_TURNS_SUM_DM);
  _dmLaunchAngle = new DmFlt32Object(ROV_SHIP_LAUNCH_ANGLE_DM);
  _dmVehicleAlive = new DmBooleanObject(TIBURON_DM_PREFIX TIBURON_ALIVE_DM);
  _dmResetTurns = new DmBooleanObject(RESET_VEHICLE_TURNS_COUNT_DM);
  _dmStopLog = new DmBooleanObject(VEHICLE_TURNS_LOG_STOP_DM);

  if ((_semaphore = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL)
  {
    setError("Couldn't create semaphore");
    return;
  }

  _dmGroup = new DmGroup();

  const int logPeriod = DM_STATIC;

  Errno err;

  if ((err = _dmInitialTwist->startConsume(logPeriod)) != SUCCESS)
  {
    sprintDmError(err, "_dmInitialTwist->startConsume()", errorBuf);
    setError(errorBuf);
    return;
  }

  if ((err = _dmLaunchAngle->startConsume(logPeriod)) != SUCCESS)
  {
    sprintDmError(err, "_dmLaunchAngle->startConsume()", errorBuf);
    setError(errorBuf);
    return;
  }

  if ((err = _dmTwist->startConsume(logPeriod, _semaphore)) != SUCCESS)
  {
    sprintDmError(err, "_dmTwist->startConsume()", errorBuf);
    setError(errorBuf);
    return;
  }

  _dmGroup->addItem(_dmTwist);

  if ((err = _dmVehicleAlive->startConsume(logPeriod, _semaphore)) != SUCCESS)
  {
    sprintDmError(err, "_dmVehicleAlive->startConsume()", errorBuf);
    setError(errorBuf);
    return;
  }

  _dmGroup->addItem(_dmVehicleAlive);

  if ((err = _dmResetTurns->startConsume(logPeriod, _semaphore)) != SUCCESS)
  {
    sprintDmError(err, "_dmResetTurns->startConsume()", errorBuf);
    setError(errorBuf);
    return;
  }

  _dmGroup->addItem(_dmResetTurns);

  if ((err = _dmStopLog->startConsume(logPeriod, _semaphore)) != SUCCESS)
  {
    sprintDmError(err, "_dmStopLog->startConsume()", errorBuf);
    setError(errorBuf);
    return;
  }

  _dmGroup->addItem(_dmStopLog);

  DM_Time t;

  // Determine if vehicle is alive
  _dmVehicleAlive->read(&_vehicleAlive, &t);
}


VehicleTurnsLogger::~VehicleTurnsLogger()
{
  closeTurnsLog();
}


int VehicleTurnsLogger::start()
{
  DM_Time t;
  MBool loggingStopped = FALSE;
  MBool debug = FALSE;

  // Vehicle turns sum updates at approximately 10 Hz, log every 10 secs
  const int SamplingRate = 100;

  int samples = 0;
  Flt32 twist, offset;

  // Read twist and offset
  dprint("VehicleTurnsLogger; initialize twist and offset\n");
  _dmTwist->read(&twist, &t);
  _dmLaunchAngle->read(&offset, &t);

  for (;;)
  {
    semTake(_semaphore, WAIT_FOREVER);
//  dprint("VehicleTurnsLogger got semaphore\n");

    if (_dmGroup->itemChanged(_dmResetTurns))
    {
      // Read twist and offset, log to file
      _dmInitialTwist->read(&twist, &t);
      _dmLaunchAngle->read(&offset, &t);
//    dprint("VehicleTurnsLogger; reset - write initialTwist=%f, offset=%f\n",
//           twist, offset);

      if (openTurnsLog("w") == -1)
      {
        logMsg("VehicleTurnsLogger; Couldn't open log file\n");
      }
      else
      {
        if (writeFile(twist, offset) == -1)
          logMsg("VehicleTurnsLogger; writeFile() failed\n");

        closeTurnsLog();
      }
      continue;
    }

    if (_dmGroup->itemChanged(_dmStopLog))
    {
      _dmStopLog->read(&loggingStopped, &t);
      dprint("VehicleTurnsLogger; loggingStopped=%d\n", loggingStopped);

      if (!loggingStopped)
        samples = 0;
    }

    if (loggingStopped)
      continue;

    if (_dmGroup->itemChanged(_dmVehicleAlive))
    {
      _dmVehicleAlive->read(&_vehicleAlive, &t);
//    dprint("VehicleTurnsLogger; _vehicleAlive=%d\n", _vehicleAlive);

      if (!_vehicleAlive)
      {
        // Vehicle just died
        writeFile(twist, offset);
      }
      else
        // Vehicle just came alive
        samples = 0;
    }

//  dprint("VehicleTurnsLogger; check alive: _vehicleAlive=%d\n",
//         _vehicleAlive);

    if (!_vehicleAlive)
      continue;

//  dprint("VehicleTurnsLogger; samples=%d, SamplingRate=%d",
//         samples, SamplingRate);

    // Read twist and offset at specified rate, and write to log file
    if (!(samples % SamplingRate))
    {
      dprint("VehicleTurnsLogger; **** SAMPLE TURNS DATA ****\n");
      // Read twist and offset, log to file
      _dmTwist->read(&twist, &t);
      _dmLaunchAngle->read(&offset, &t);
      dprint("VehicleTurnsLogger; write twist=%f, offset=%f\n", twist, offset);

      if (openTurnsLog("w") == -1)
      {
        logMsg("VehicleTurnsLogger; Couldn't open log file\n");
      }
      else
      {
        if (writeFile(twist, offset) == -1)
          logMsg("VehicleTurnsLogger; writeFile() failed\n");

        closeTurnsLog();
      }
      samples = 1;
    }

    samples++;
  }
}


int VehicleTurnsLogger::writeFile(Flt32 twist, Flt32 offset)
{
  return writeTurnsLog(twist, offset);
}