static char simdive_id[] = "$Header: simdive.cc,v 1.2 97/03/20 12:32:09 oreilly Exp $";

/*
$Log:	simdive.cc,v $
Revision 1.2  97/03/20  12:32:09  12:32:09  oreilly (Thomas C. O'Reilly)
..

Revision 1.1  96/10/28  09:13:03  09:13:03  oreilly (Thomas C. O'Reilly)
Initial revision

*/
#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include <math.h>
#include "DataManager.h"
#include "sensorsDm.h"
#include "kalmanDM.h"
#include "powerDM.h"
#include "Math.h"
#include "State.h"
#define MAIN
#include "DmErrno.h"

void main(int argc, char **argv)
{
  Errno err;

  if ((err = dm_init(NULL)) != OK)
  {
    if (err == EDM_NO_DAEMON)
    {
      fprintf(stderr, "Data Manager daemon isn't running\n");
    }
    else
    {
      fprintf(stderr, "Data Manager daemon initialization error %d\n", err);
    }

    exit(1);
  }

  if (argc != 3)
  {
    fprintf(stderr, "usage: %s freq maxDepth\n", argv[0]);
    exit(1);
  }

  float omega = atof(argv[1]);
  Flt64 maxDepth = atof(argv[2]);
  
  char errorBuf[errorMsgSize];
  char itemName[MaxDmNameLen];

  strcpy(itemName, VEHICLE_DEPTH_DM);

  DmFlt64Object *dmDepth = new DmFlt64Object(itemName);
  if (dmDepth->error())
  {
    dmDepth->prtErrorMsg();
    exit(1);
  }
  if ((err = dmDepth->startProvide(DM_ASYNC)) != SUCCESS)
  {
    sprintDmError(err, "dmDepth->startProvide() failed", errorBuf);
    fprintf(stderr, "%s\n", errorBuf);
    exit(1);
  }
  
  strcpy(itemName, VEHICLE_ALTITUDE_DM);

  DmFlt32Object *dmAltitude = new DmFlt32Object(itemName);
  if (dmAltitude->error())
  {
    dmAltitude->prtErrorMsg();
    exit(1);
  }

  if ((err = dmAltitude->startProvide(DM_ASYNC)) != SUCCESS)
  {
    sprintDmError(err, "dmAltitude->startProvide() failed", errorBuf);
    fprintf(stderr, "%s\n", errorBuf);
    exit(1);
  }

  MBool readRaw = FALSE;
  DmAttitudeObject *dmAttitude = new DmAttitudeObject(readRaw);
  if (dmAttitude->error())
  {
    dmAttitude->prtErrorMsg();
    exit(1);
  }  
  if ((err = dmAttitude->startProvide(DM_ASYNC)) != SUCCESS)
  {
    sprintDmError(err, "dmAtitude->startProvide() failed", errorBuf);
    fprintf(stderr, "%s\n", errorBuf);
    exit(1);
  }  

  strcpy(itemName, POWER_DM_PREFIX THRUSTER_STBD_HORIZ_DM ".CURRENT");
  DmInt16Object *dmCurrent = new DmInt16Object(itemName);
  if (dmCurrent->error())
  {
    dmCurrent->prtErrorMsg();
    exit(1);
  }
  if ((err = dmCurrent->startProvide(DM_ASYNC)) != SUCCESS)
  {
    sprintDmError(err, "dmCurrent->startProvide() failed", errorBuf);
    fprintf(stderr, "%s\n", errorBuf);
    exit(1);  
  }
  
  time_t start = time(NULL);
  float bottomAmp = 5.;
  float bottomOmega = .2;
  Flt32 roll = 0.;
  Flt32 rollInc = 0.05;
  Flt32 maxRoll;
  Flt32 minRoll;
  maxRoll = 720. * Math::RadsPerDeg;
  minRoll = -maxRoll;
  Flt32 heading = 0.;
  Int16 current = 1000;
  
  while(1)
  {
    time_t t = time(NULL);

    // Put in sinusoidal bottom
    Flt64 bottomDepth = bottomAmp * cos(bottomOmega * (t - start)) + maxDepth;

    Flt64 depth = maxDepth/2. * cos(omega * (t - start)) + maxDepth/2.;
    if (depth > bottomDepth) depth = bottomDepth;
 
    float altitude = bottomDepth - depth; 
    
    DM_Time dmTime;
    dmTime.tv_sec = t;
    dmTime.tv_usec = 0;

    if ((err = dmDepth->write(depth, &dmTime)) != SUCCESS)
    { 
      sprintDmError(err, "Error writing depth", errorBuf);
      fprintf(stderr, "%s\n", errorBuf);
    }
    
    if ((err = dmAltitude->write(altitude, &dmTime)) != SUCCESS)
    {
      sprintDmError(err, "Error writing altitude", errorBuf);
      fprintf(stderr, "%s\n", errorBuf);
    }

    fprintf(stderr, "roll=%.2f, max=%.2f inc=%.2f\n", 
	    roll, maxRoll, rollInc);
      
    roll += rollInc;

    if (roll > maxRoll)
      rollInc = -rollInc;
    else if (roll < minRoll)
      rollInc = -rollInc;

    StateValue state[6];
    state[RollIndex] = roll;
    state[HeadingIndex] = heading;
    
    if ((err = dmAttitude->write(state, &dmTime)) != SUCCESS)
    {
      sprintDmError(err, "Error writing attitude", errorBuf);
      fprintf(stderr, "%s\n", errorBuf);
    }
    
    heading += (5. * Math::RadsPerDeg);
    
    if (heading > Math::TwoPi)
      heading = 0.;
    
    fprintf(stderr, "depth=%.1f, alt=%.1f roll=%.2f heading=%.1f\n", 
	    depth, altitude, roll, heading);


    if (current == 1000)
      current = 2000;
    else
      current = 1000;
    
    if ((err = dmCurrent->write(current, &dmTime)) != SUCCESS)
    {
      sprintDmError(err, "Error writing current", errorBuf);
      fprintf(stderr, "%s\n", errorBuf);
    }
    
    sleep(1);
  }
}