#include "Dvl.h"
#include "MathP.h"

Dvl::Dvl(const char *name, NavSensors *sensors, int maxBad)
  : NavSensor(name, sensors, maxBad)
{
  _DvlIF = 0;
  _consecGoodHits[0] = 0;
  _consecGoodHits[1] = 0;
  _consecGoodHits[2] = 0;
  _consecGoodHits[3] = 0;
}

TaskInterface *Dvl::createTaskIF(int timeout)
{
  try {
    _DvlIF = new DvlIF(name(), timeout);
  }
  catch (...) {
    _DvlIF = 0;
  }

  return _DvlIF;
}
//
// This is called once each control sample period by Navigation::readSensors().
//
DeviceIF::Status Dvl::readTaskIF(Boolean *valid, 
				 TimeIF::TimeSpec *sampleTime)
{
   DeviceIF::Status status = _DvlIF->get(&data, &m_newData);
   //DeviceIF::Status status = _DvlIF->get(&data);
   //
   // The m_newData argument above is no longer used, as of 03/11/24.  Trying
   // to remove it causes (at this point) inexplicable link errors.  This
   // routine now checks for new data.
   //
   // Determine sensor status:
   //
   *valid = True;
   if( status != OK ) *valid = False;

   sampleTime->seconds = data.sampleTime.seconds;
   sampleTime->nanoSeconds = data.sampleTime.nanoSeconds;
  //
  // Flag new data.  We use a != rather than an < in the "if" below so that a
  // Dvl clock rollover doesn't set newData to False.  The code below relys on
  // Navigation only calling this method ONCE, and then subsequently using the
  // newData() function below to check it.
  //
  m_newData = False;
  if( m_lastPingTime != data.pingTime )
  {
    m_newData = True;
    m_lastPingTime = data.pingTime;
  }

   return status;
}
//
// Return a True if there is new data:
//
Boolean Dvl::newData()
{
   if(m_newData) return True;
   return False;
}
//
// Compute the altitude.
//
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 Dvl::altitude(NavigationIF::Attitude *attitude)
{
   double ctheta, stheta, cphi, sphi;
   int numValidBeams=0;
   double beamRange[4];
   double aveAlt = 0., minRange = 1000.;
   Boolean useMinBeam = False;
   int minIndx = -1;
   
   ctheta = cos(attitude->pitch);
   stheta = sin(attitude->pitch);
   cphi   = cos(attitude->roll);
   sphi   = sin(attitude->roll);
   //
   // Unfortunately the beams are not declared as an array in the DvlIF.idl.

   // keep running count of number of consecutive good hits for filtering
   (data.beam1 != BAD_BEAM_RANGE) ? _consecGoodHits[0]++:_consecGoodHits[0]=0;
   (data.beam2 != BAD_BEAM_RANGE) ? _consecGoodHits[1]++:_consecGoodHits[1]=0;
   (data.beam3 != BAD_BEAM_RANGE) ? _consecGoodHits[2]++:_consecGoodHits[2]=0;
   (data.beam4 != BAD_BEAM_RANGE) ? _consecGoodHits[3]++:_consecGoodHits[3]=0;

   // clip to prevent rollover
   if (_consecGoodHits[0] >= MIN_GOOD_HITS) _consecGoodHits[0] = MIN_GOOD_HITS;
   if (_consecGoodHits[1] >= MIN_GOOD_HITS) _consecGoodHits[1] = MIN_GOOD_HITS;
   if (_consecGoodHits[2] >= MIN_GOOD_HITS) _consecGoodHits[2] = MIN_GOOD_HITS;
   if (_consecGoodHits[3] >= MIN_GOOD_HITS) _consecGoodHits[3] = MIN_GOOD_HITS;
 
   beamRange[0] = data.beam1;
   beamRange[1] = data.beam2;
   beamRange[2] = data.beam3;
   beamRange[3] = data.beam4;

   //mark beam range as bad if it hasn't tracked the bottom for more than
   //MIN_GOOD_HITS number of bottom detects
   if (_consecGoodHits[0] < MIN_GOOD_HITS)  
	beamRange[0] = BAD_BEAM_RANGE;
   if (_consecGoodHits[1] < MIN_GOOD_HITS)
	beamRange[1] = BAD_BEAM_RANGE;
   if (_consecGoodHits[2] < MIN_GOOD_HITS)
  	beamRange[2] = BAD_BEAM_RANGE;
   if (_consecGoodHits[3] < MIN_GOOD_HITS)
	beamRange[3] = BAD_BEAM_RANGE;

   //
   // Beam numbering convention, looking down on Dvl from above.  x and y are
   // the vehicle axes.
   //
   //                x
   //                ^ 
   //                |
   //              1 | 3
   //                |------> y
   //              4   2
   //
   // h[i] is the the z component of each beam's unit vector (in N).
   // Multiply by the measured range to get the altitude of the i^th beam
   // assuming a planar level bottom.
   //
   h[0] = ( -stheta*c3s2  -  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*data.beam1;
   h[1] = (  stheta*c3s2  +  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*data.beam2;
   h[2] = ( -stheta*c3s2  +  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*data.beam3;
   h[3] = (  stheta*c3s2  -  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*data.beam4;

   for( int i=0; i<NUM_BEAMS; i++ ) 
   {
      /*
      ** Get range from beam i
      */
      if (beamRange[i] != (double) BAD_BEAM_RANGE) 
      {
	 numValidBeams++;
	 /*
	 ** Sum up all ranges, or take the minimum range of the four beams
	 */
	 if( useMinBeam ) 
	 {
	    if( beamRange[i] < minRange ) 
	    {
	       minIndx = i;
	       minRange = beamRange[i];
	    }
	 }
	 else 
	 {
	    aveAlt += h[i];
	 }  // if( useMinBeam ) 
      }  // if (beamRange[i] != (double) BAD_BEAM_RANGE) 
   }  // for( i=0; i<NUM_BEAMS; i++ ) 
   //
   // Select either the minimum altitude or the average of the altitudes
   // computed from the good (nonzero) beam ranges.  If none of the beams
   // are getting an echo, return 1000 meters.
   //
   if(numValidBeams)
   {
      if( useMinBeam )  return( h[minIndx] );
      else              return( aveAlt / ( (double) numValidBeams ) );
   }
   else
   {
      return ( 1000.0 );
   }
   //
   // This was the old way:
   //return( data.range * cos(attitude->roll) * cos(attitude->pitch) );
}