#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <logLib.h>             /* vxWorks message logging declarations      */

#include <mbariTypes.h>
#include "trig.h"
#include "vehicleTurnsDM.h"

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

#define INITIAL_TWIST_MNEM "InitialTwist(rad)"
#define LAUNCH_OFFSET_MNEM "LaunchOffset(rad)"
#define DEFAULT_INITIAL_TWIST 0.
#define DEFAULT_LAUNCH_OFFSET M_PI

#define TWO_PI (M_PI * 2)
#define MAX_TWIST (M_PI * 20)

#define dprint if (debug) logMsg
#define dprintf if (debug) fprintf

FILE *_logFile = 0;

int openTurnsLog(const char *access)
{
  if ((_logFile = fopen(TURNS_COUNTER_LOG, access)) == NULL)
  {
    logMsg("openTurnsLog() - Couldn't open log file %s\n", TURNS_COUNTER_LOG);
    return -1;
  }
  else
  {
    return 0;
  }
}


int writeTurnsLog(Flt32 twist, Flt32 offset)
{
  MBool debug = FALSE;

  if (!_logFile)
  {
    if (openTurnsLog("w") == -1)
    {
      logMsg("writeTurnsLog() - openTurnsLog(\"w\") failed\n");
      return -1;
    }
  }

  if (twist > MAX_TWIST || twist < -MAX_TWIST)
  {
    logMsg("writeTurnsLog() - twist of %f rads is out of range\n", twist);
    return -1;
  }

  if (offset > TWO_PI || offset < -TWO_PI)
  {
    logMsg("writeTurnsLog() - launchOffset of %f rads is out of range\n", offset);
    return -1;
  }

  rewind(_logFile);
  dprint("writeTurnsLog() - Writing twist and offset to %s...\n", TURNS_COUNTER_LOG);

  fprintf(_logFile, "%s: %9.6f\n", INITIAL_TWIST_MNEM, twist);
  fprintf(_logFile, "%s: %9.6f\n", LAUNCH_OFFSET_MNEM, offset);
  fflush(_logFile);
  return 0;
}


int readTurnsLog(Flt32 *twist, Flt32 *offset)
{
  char buf[256];
  MBool error = FALSE;
  char *token;
  int nrec;
  MBool debug = FALSE;

  if (!_logFile)
  {
    if (openTurnsLog("r") == -1)
    {
      logMsg("readTurnsLog() - openTurnsLog(\"r\") failed\n");
      *twist = DEFAULT_INITIAL_TWIST;
      *offset = DEFAULT_LAUNCH_OFFSET;
      return -1;
    }
  }

  for (nrec = 0; nrec < 2 && !error; nrec++)
  {
    if (fgets(buf, sizeof(buf), _logFile))
    {
      if ((token = strchr(buf, ':')) == NULL)
      {
        logMsg("readTurnsLog() - missing ':' delimiter in line\n");
        error = TRUE;
        break;
      }

      /* Skip past delimiter */
      token++;

      dprintf(stderr, "readTurnsLog() - parse line %d;\nbuf=%s, token=%s\n",
              nrec, buf, token);

      switch(nrec)
      {
        case 0:
        *twist = (float )atof(token);
        break;

        case 1:
        *offset = (float )atof(token);
        break;
      }
    }
    else
    {
      logMsg("readTurnsLog() - missing lines in file\n");
      error = TRUE;
    }
  }

  if (*twist > MAX_TWIST || *twist < -MAX_TWIST)
  {
    logMsg("readTurnsLog() - twist of %f rads is out of range\n", *twist);
    error = TRUE;
  }

  if (*offset > TWO_PI || *offset < -TWO_PI)
  {
    logMsg("readTurnsLog() - launchOffset of %f rads is out of range\n", *offset);
    error = TRUE;
  }

  if (error)
  {
    *twist = DEFAULT_INITIAL_TWIST;
    *offset = DEFAULT_LAUNCH_OFFSET;
    return -1;
  }

  dprint("readTurnsLog() - twist=%f, offset=%f\n", *twist, *offset);

  return 0;
}


int closeTurnsLog()
{
  if (!_logFile)
    return 0;

  fclose(_logFile);
  _logFile = 0;
  return 0;
}