/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Simulate the velocity measurement from the RDI DVL            */
/* Filename : SimulatedDvl.h                                                */
/* Author   : McEwen, following O'Reilly's template.                        */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 04/03/2001                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <time.h>
#include "SimulatedDvlSide.h"
#include "Syslog.h"
#include "MathP.h"
#include "VehicleConfigurationIF.h"

SimulatedDvlSide::SimulatedDvlSide()
  : DvlSideIF_SK()
{
  //
  // Make the vehicle parameters available in the constructor. This is a
  // local variable.
  //
  VehicleConfigurationIF vehicleConfig("vehicleConfig");

  _simulator = new SimulatorIF("simulator");
  m_log = new DvlSideLog(this, DataLog::BinaryFormat);

  //SerialDevice *serialDevice = new SerialDevice("");
  //_dvl = new Dvl( serialDevice, False );

  triggerEvent(DeviceIF::Ok);

  for( int i=0; i<NXE; i++ )
  {
    bottomTrackVelocity[i] = 0.;  
    waterMassVelocity[i]   = 0.;    
  }
//  double c3s2 = 1./sqrt(2.)/2.;   //cos(pi/4)*sin(pi/6),   pi/6 = 30 degrees.
//  double s3s2 = 1./sqrt(2.)/2.;   //sin(pi/4)*sin(pi/6)
//  double c2   = sqrt(3.)/2.;      //cos(pi/6)

  double c3s2 = 1./sqrt(2.)*sin(PI/9.);   //cos(pi/4)*sin(pi/9),   pi/9 = 20 degrees.
  double s3s2 = 1./sqrt(2.)*sin(PI/9.);   //sin(pi/4)*sin(pi/9)
  double c2   = cos(PI/9.);               //cos(pi/9)
  //
  // Initialize the line-of-sight unit vectors for each Dvl beam.  See Figure
  // 2 under the EX command in the Dvl manual.  On my shelf, the "New Dvl
  // Manual," page 46, has my derivation.
  //
  //
  // Beam numbering convention, looking through Dvl from behind.  x and z are
  // the vehicle axes.  The Dvl is mounted looking out the port side along
  // vehicle -y.
  //
  //                x
  //                ^ 
  //                |
  //              1 | 3
  //                |------> z
  //              4   2
  //
  //
  // Another somewhat tricky thing here is that, for example, passing
  // los_B[0] to a function that is expecting a 3 element vector will take
  // row 0 and all columns of the array below.
  los_B[0][0] =  c3s2; los_B[0][1] = -c2; los_B[0][2] =  -s3s2; 
  los_B[1][0] = -c3s2; los_B[1][1] = -c2; los_B[1][2] =   s3s2;
  los_B[2][0] =  c3s2; los_B[2][1] = -c2; los_B[2][2] =   s3s2;
  los_B[3][0] = -c3s2; los_B[3][1] = -c2; los_B[3][2] =  -s3s2;

  //los_B[0][0] =  c3s2; los_B[0][1] = -s3s2; los_B[0][2] =  c2; 
  //los_B[1][0] = -c3s2; los_B[1][1] =  s3s2; los_B[1][2] =  c2; 
  //los_B[2][0] =  c3s2; los_B[2][1] =  s3s2; los_B[2][2] =  c2; 
  //los_B[3][0] = -c3s2; los_B[3][1] = -s3s2; los_B[3][2] =  c2; 
  //
  // Dvl offset from the origin Bo of the body-fixed axes, r_Bo_Dvlo_B.
  // LATER: Read in the distance of the Dvl from the nose in dvl.cfg, then
  // compute this using the length "len" from vehicle.cfg
  offset_B[0] = 2.62 - 3.38;  //These numbers are for the Multibeam 8Oct04.
  offset_B[1] = 0.;
  offset_B[2] = 0.;
  
  //for( i=0; i<3; i++ ) offset_B[i] = 0.;
}

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


void SimulatedDvlSide::name(DeviceIF::Name name)
{
  strcpy(name, "Simulated Dvl");
}


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


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


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


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


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


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


DeviceIF::Status SimulatedDvlSide::status()
{
  return DeviceIF::Ok;
}

const double maxDvlRange = 200.;       //Meters
const double minDvlRange = 1.;         //Meters
const double rangeTol    = 0.2;        //Range accuracy to 20 cm.
const double maxAngle    = 9.*PI/20.;  //81 Degrees
//double offset_B[3] = {0., 0., 0.}; //r_Bo_Dvlo_B
//double los_B[3]    = {0., 0., 1.}; //r_Dvlo_p_B (los unit vector)

DeviceIF::Status SimulatedDvlSide::get(Data *data, Boolean *newData)
{
   double position[3], velocity[3], backDiff[3], simTime;
   double EulerAngles[3], angularVelocity[3], roll, pitch, yaw;
   long k;                         //Sample period counter
   long simCntr[4];                //For counting bisect iterations per beam.
   for( int i=0; i<NUM_BEAMS; i++ ) simCntr[i] = 0;
  DeviceIF::Status status;
  Boolean debug = False;
  //
  // Copy the state.  waterMassVelocity = vel_Bo_W_B.  Beware that position
  // and EulerAngles are member variables and so are passed to
  // ComputeDvlAltitude() further below.
  //
  _simulator->state( position, data->waterMassVelocity, 
		     EulerAngles, angularVelocity );
  //
  // This returns vel_Bo_N_W from the sim.
  //
  _simulator->translationalBottomVelocity( data->bottomTrackVelocity );
  //
  // Rob:  Change pitch and roll to space-fixed, limited.
  //
  data->temp       = 15.;
  data->roll       = EulerAngles[0];
  data->pitch      = EulerAngles[1];
  data->heading    = EulerAngles[2];

  roll  = Math::limit( EulerAngles[0], maxAngle, -maxAngle );
  pitch = Math::limit( EulerAngles[1], maxAngle, -maxAngle );

  //
  // Copy simTime
  //
  k = _simulator->nTs();
  //if( (k % 20) == 0 ) Syslog::write("SimulatedDvlSide - Got here.");
  //
  // Post a DVL reading at 1 Hz.  
  //
  *newData = False;
  if( (k % 5) == 0 ) *newData = True;
  //
  // Fill in the "data" array:
  //
  data->pingTime = _simulator->simTime();
  dprintf( "SimulatedDvlSide:: data->pingTime = %.1f\n", data->pingTime);
  //
  // This is the original range calculation.  rsm 04/4/14.
  // data->range    = ( _simulator->waterDepth() - position[2] )/
  //                  ( cos(pitch) * cos(roll) );
  //
  // Compute the Dvl beam range.  First, we'll just use a single beam along
  // the Dvl LOS.  Later we'll expand this to four beams.

  //data->range = _simulator->beamRange( offset_B, los_B, 0., maxDvlRange, 
  //		rangeTol);
  double grazing[4];
  for( i=0; i<NUM_BEAMS; i++ )
  {
     Boolean surfaceDetect;
     long *simCntrp = simCntr + i;
     beamRange[i] = _simulator->beamRange( offset_B, los_B[i], 0., maxDvlRange, 
					   rangeTol, simCntrp, &(grazing[i]),
	                                   &surfaceDetect);
  //					   rangeTol, &(simCntr[i]) );
  }
  //
  // The code below repeats some of the code in Dvl::cvtRange().  The best
  // way to handle this would have been to create a Dvl object _dvl here, and
  // just call _dvl->cvtRange() in dvl/Dvl.cc. But, I just can't get it to
  // link due to the excessive complexity of this architecture. The problem
  // seems to be that DvlLog exists both here and in onboard/Dvl, the only
  // difference being that dvl/DvlLog.cc creates a "Dvl" object, while 
  // Simulator/DvlLog creates a "SimulatedDvlSide" object. So, I'm unfortunately
  // going have to repeat some of the logic here.
  //
  // Compute the range using the real code.  First scale into an int:
  //   for( i=0; i<NUM_BEAMS; i++ )
  //   {
  //      short rangeCM;
  //      rangeCM = (int)beamRange[i]*100.;
  //      rawRange[i][DOPPLER_LSB] = rangeCM | 0x00FF;
  //      rawRange[i][DOPPLER_MSB] = ( ( (rangeCM | 0xFF00) >> 8 ) | 0x00FF );
  //   }
  //Boolean useMinBeam = False;
  //double beamRangeChk[NUM_BEAMS];
  //_dvl->cvtRange( rawRange, beamRangeChk, &(data->range), useMinBeam );
  //for( i=0; i<NUM_BEAMS; i++ ) 
  //   if( beamRange[i] != beamRangeChk[i] ) 
  //	printf("SimulatedDvlSide::get.  Error in cvtRange, beam[%d].\n", i);
#define BAD_BEAM_RANGE 0
  int numValidBeams=0;
  double range = 0., minRange = 200.;
  double maxGrazing = PI/8.;
  Boolean useMinBeam = False;
  const double cos30 = cos(PI/6.);
  data->bottomStatus = 0;                //Start with all beams good.
  for( i=0; i<NUM_BEAMS; i++ ) 
  {
     /*
     ** Get range from beam i.  
     */
     if( ( beamRange[i] != (double) BAD_BEAM_RANGE ) &&
	 ( beamRange[i] < maxDvlRange )              &&
	 ( beamRange[i] > minDvlRange )                 )
//	 ( grazing[i]   <= maxGrazing )                 )  Backwards?
     {
	numValidBeams++;
	/*
	** Sum up all ranges, or take the minimum range of the four beams
	*/
	if( useMinBeam ) 
	{
	   if( beamRange[i] < minRange ) minRange = beamRange[i];
	}
	else 
	{
	   range += beamRange[i];
	}
     }
     else //if (beamRange[i] == BAD_BEAM_RANGE) 
     {
	// Set both low echo amplitude and low correlation flags for this beam:
	data->bottomStatus = ( data->bottomStatus | ( 0x3 << 2*i ) );
	//
	// 1 Oct 07 rsm.  _simulator->beamRange has been changed to return
	// maxRange if the beam doesn't intersect the bottom, which is
	// intuitive and what the echo sounder and range finder presumably
	// do.  The RDI Dvl returns 0 in this case, so here we set it to zero
	// rather than maxRange.
	beamRange[i] = (double) BAD_BEAM_RANGE;
     }
  }
  /*
  ** If we're computing average altitude, divide by NUM_BEAMS to get the 
  ** average beam range, then multiply by cos(pi/6) to get LOS range.
  ** Otherwise, just convert the minimum range to LOS range.
  **
  ** 2005/1/12:  This altitude calcuation is now superceded by the one in 
  ** onboard/Navigation/Dvl.cc.  However, Navigation still relys upon 
  ** bottomDetectStatus.
  **
  */
  if( useMinBeam )
    data->range = cos30 * minRange;
  else if(numValidBeams)
    data->range = cos30 * range / ( (double) numValidBeams );
  else data->range = 0.;
  //
  // Set the bottomDetectStatus flag:
  if( numValidBeams )
  {
     data->bottomDetectStatus = True;
     status = DeviceIF::Ok;
  }
  else 
  {
     data->bottomDetectStatus = False;
     status = DeviceIF::Ok; //status is OK as long as the device didn't fail
  }
  data->dataStatus = data->bottomStatus;
  //
  // Water tracking is always good:
  //
  data->waterStatus = 0;
  //
  data->beam1 = beamRange[0];
  data->beam2 = beamRange[1];
  data->beam3 = beamRange[2];
  data->beam4 = beamRange[3];
  //
  // Fill in the local data for the log->write.
  //
  for( i=0; i<3; i++ )
  {
    bottomTrackVelocity[i] = data->bottomTrackVelocity[i];
    waterMassVelocity[i]   = data->waterMassVelocity[i];
  }
  *dvlTemp    = data->temp;
  *dvlRoll    = data->roll;
  *dvlPitch   = data->pitch;
  *dvlHeading = data->heading;
  *dvlRange   = data->range;

  *bottomStatus = data->bottomStatus;
  *waterStatus  = data->waterStatus;

  m_log->write();
  //
  // Print out debug info (shut off with && 0).
  //
  if( (k % 20) == 0 && (k != 0) && 0 )
     Syslog::write(" SimulatedDvlSide::get() \n"
	  "               waterDepth(0.,0.) = %.1f \n"
	  "               Dvl range = %.1f,\n"
	  "               range = %.1f\n"
	  "               numValidBeams = %d\n"
	  "               beam1 range = %.1f,  iterations = %d, grazing = %.1f \n"
	  "               beam2 range = %.1f,  iterations = %d, grazing = %.1f \n"
	  "               beam3 range = %.1f,  iterations = %d, grazing = %.1f \n"
	  "               beam4 range = %.1f,  iterations = %d, grazing = %.1f \n"
	  "               bottomStatus = %x ", _simulator->waterDepth(0.,0.),
	                  data->range,
	                  range,
     	                  numValidBeams,
	                  beamRange[0], simCntr[0], grazing[0]*PI/180.,
	                  beamRange[1], simCntr[1], grazing[1]*PI/180.,
	                  beamRange[2], simCntr[2], grazing[2]*PI/180.,
	                  beamRange[3], simCntr[3], grazing[3]*PI/180.,
	                  data->bottomStatus);


  return status;
}

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