#include <process.h>
#include <MathP.h>
#include <time.h>
#include "SimulatedHydroscat.h"
#include "Syslog.h"

SimulatedHydroscat::SimulatedHydroscat(const char *name) : HydroscatIF_SK()
{
  _simulator = new SimulatorIF("simulator");
  _first = True;
}

SimulatedHydroscat::~SimulatedHydroscat()
{
   delete _simulator;
}



void SimulatedHydroscat::name(DeviceIF::Name name)
{
  strcpy(name, "Hydroscat");
}


void SimulatedHydroscat::serialNumber(DeviceIF::Name number)
{
  strcpy(number, "XXX");
}


DeviceIF::Status SimulatedHydroscat::initialize()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedHydroscat::powerOn()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedHydroscat::powerOff()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedHydroscat::status()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedHydroscat::dataLoggingOn()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedHydroscat::dataLoggingOff()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}

Boolean SimulatedHydroscat::ready()
{
   return True;
}

DeviceIF::Status SimulatedHydroscat::getData(HydroscatIF::Data *data)
{
  Boolean debug = False;
  double dx, dy, distxy;

  _simulator->state( _position, _velocity, 
		     _eulerAngles, _angularVelocity );

  if(  (_simulator->simTime() > 1.0)  && _first )
  {
     Syslog::write("SimulatedHydroscat - setting pos0 at simTime = %.2f", 
		   _simulator->simTime());
     _first = False;
     _pos0[0]=_position[0];
     _pos0[1]=_position[1];
     _pos0[2]=_position[2];
  }

  data->snorm1 = (short) 2*_position[2];
  data->snorm2 = (short) 5*_position[2];
  data->snorm3 = (short) 10*_position[2];

  dx = _position[0]-_pos0[0];
  dy = _position[1]-_pos0[1];
  distxy = sqrt( dx*dx + dy*dy );

  if( debug && !(_simulator->nTs() % 5) )
  {
     Syslog::write("SimulatedHydroscat - simTime = %.2f", 
		   _simulator->simTime());
     Syslog::write("SimulatedHydroscat - pos0[0] = %6.2e", _pos0[0]);
     Syslog::write("SimulatedHydroscat - pos0[1] = %6.2e", _pos0[1]);

     Syslog::write("SimulatedHydroscat - dx = %6.2e", dx);
     Syslog::write("SimulatedHydroscat - dy = %6.2e", dy);
     Syslog::write("SimulatedHydroscat - distxy = %6.2e", distxy);
  }

  data->calculated.fl676_uncorr = 1.5e-3;
  if( distxy > 100.) data->calculated.fl676_uncorr = 2.5e-3;

  data->calculated.bb470 = 0.;
  if( distxy > 175.) data->calculated.bb470 = 25.e-3;

  if( distxy > 200.) data->calculated.fl676_uncorr = 1.0e-3;



  data->seconds   = _simulator->simTime();
  data->hseconds  = 0.;

  return DeviceIF::Ok;
}

 
int SimulatedHydroscat::spawnAuxTasks()
{
  return 0;
}