/****************************************************************************/
/* Copyright (c) 2008 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : BIAUV Camera Driver                                           */
/* Filename : CameraDriver.cc                                               */
/* Author   : rsm, K Headley                                                */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 05/02/2008                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "CameraDriver.h"
#define READ_TIMEOUT 3000           //Milliseconds
#define MAXRECORDBYTES 512
#include "PeriodicTask.h"
#include "SerialDeviceExt.h"
#include "Syslog.h"
#include "MathP.h"
#include "Bicam.h"

Boolean debug = False;

//////////////////////////////////////////////////////////
// SerialDeviceExt has a single IP port associated with it
// but the XPort has two (one for GPIO, the other for
// serial traffic); Could redesign SerialDeviceExt or
// eliminate it.
/**
   Benthic Imaging AUV Camera
*/
Camera::Camera(SerialDeviceExt *device, Boolean test, Boolean sim)
  : PeriodicTask("cameraDriver"), _test(test), _sim(sim)
{

  printf("Camera:: Got here!\n");

  _device = device;
  _log = new CameraLog(this, DataLog::BinaryFormat);
  _output= new CameraOutput();
  _output->write();

  addPeriodicCallback(200, (CallbackMethod)Camera::readData);

  _samplePeriodCntr = 0;
  _first = False;
  _running = False;

  if(_test)
    {
      Syslog::write("CameraDriver:: _test = %d", _test);
    }


  if( !_test )
    {
      Syslog::write("CameraDriver:: Before navigation IF.");
      try
	{
	  _navigationIF = new NavigationIF("navigation");
	}
      catch( SharedObject::Error e ) 
	{
	  Syslog::write("CameraDriver -- Caught exception: %s\n", e.msg );
	}
      catch( ... ) 
	{
	  Syslog::write("CameraDriver -- Caught exception!\n");
	}

      // don't open the device:
      // we'll be working through the
      // XPort interface
      //_device->openSD();

    }   

  _photoCntr = 0;

  _position.x = 0.;
  _position.y = 0.;

  _position.latitude  = Math::degToRad(36.00); 
  _position.longitude = Math::degToRad(-122.00); 
  _position.altitude  = 3.0; 
   
  //
  // In simulation there is no hardware, so don't try to talk to it!
  //
  if( _sim )
    {
      return;
    }

  // get hostname for xport
  char host[256];
  const char *sp=_device->devName();
  char *ip, *op;
  struct timeval now;
  cout<<"getting hostname"<<endl;
  ip=strstr(sp,":")+1;
  op=host;
   
  while(*ip!=':')
    {
      *op=*ip;
      op++;
      ip++;
    }
  *op='\0';
  cout<<"got hostname:"<<host<<endl;


  //
  // Create and configure an XPort 
  // Arguments (in order, configured in CameraDriver.h):
  //   TCP/IP port to use for Serial
  //   GPIO pin direction mask (which pins affect for input/output)
  //   GPIO pin direction levels (select input(0)/output(1))
  //   GPIO pin level mask (which pins affect for output level)
  //   GPIO pin active levels (select active LO(1)/active HI(0))
  //   
  //   here, we set all setting all GPIO pins as active low outputs.
  xport.configure(host,
		  GPIO_IP_PORT,SERIAL_IP_PORT1,
		  CFG_GPIO_DIR_MASK,
		  CFG_GPIO_DIR_LEVEL_MASK,
		  CFG_GPIO_LEVEL_MASK,
		  CFG_GPIO_ACTIVE_LEVEL_MASK);

  // TODO: make sure XPort power is on before trying to connect

  //
  // Connect to the XPort 
  // (both serial and gpio sockets)
  // we call connectxp with doConfig=FALSE so that no attempt
  // is made to set GPIO Direction or Active-Level.
  // !!! THIS ROUTINE DOES NOT SET ACTIVE LEVEL OR GPIO PIN DIRECTION !!!
  // ** Must have hardware flash defaults configured to OUTPUT **
  // ** OR must explicitly set directions in software          **
  // ** Setting either Direction or Active-Level causes        **
  // ** ALL GPIO PINS TO BE DISABLED until their state is set  **

  xport.connectxp(XPCON_ALL, FALSE);

  //
  //   Configure a XPortGPIO for the camera trigger
  //   Constructor arguments:
  //   an XPort reference
  //  GPIO pin select mask (which pins to activate when triggering camera)
  //  GPIO pin direction mask (which pins to set as outputs(1)/inputs(0))
  //  GPIO pin active level mask (which pins to set active HI(0)/active LO(1))
  //  GPIO pin state mask (which pins to set active(1)/inactive(0)))
  XPortResponse *response;

  camTrigger.configure(&xport,
		       CAMTRIGGER_GPIO_SELECT_MASK,
		       CAMTRIGGER_GPIO_DIR_MASK,
		       CAMTRIGGER_GPIO_ACTIVE_LEVEL_MASK,
		       CAMTRIGGER_GPIO_STATE_MASK);
	camTrigger.setDirection(0x1, response);   // output
	camTrigger.setActiveLevel(0x1, response); // active low

  // disable camera trigger initially
  camTrigger.setInactive();

  //
  //   Configure a XPortGPIO for the strobe disable
  //   Constructor arguments:
  //   an XPort reference
  //  GPIO pin select mask (which pins to activate when triggering)
  //  GPIO pin direction mask (which pins to set as outputs(1)/inputs(0))
  //  GPIO pin active level mask (which pins to set active HI(0)/active LO(1))
  //  GPIO pin state mask (which pins to set active(1)/inactive(0)))

if(!_sim) strobeDisable.configure(&xport,
			 STROBE_DISABLE_GPIO_SELECT_MASK,
			 STROBE_DISABLE_GPIO_DIR_MASK,
			 STROBE_DISABLE_GPIO_ACTIVE_LEVEL_MASK,
			 STROBE_DISABLE_GPIO_STATE_MASK);
    strobeDisable.setDirection(0x1, response);       //output 21 June 11 changed to 0x1
    strobeDisable.setActiveLevel(0x0, response); // active high

	
    // disable strobes initially
    strobeDisable.setInactive();


  // initialize the strobe warm-up delay
  // TODO: read from config file
  _strobeEnableStartDelaySec=0;  // Changed from 2 to 0 21 June

  //
  //   Configure a XPortGPIO for the Vicor 12V camera powerSupply enable
  //   Constructor arguments:
  //   an XPort reference
  //  GPIO pin select mask (which pins to activate when triggering)
  //  GPIO pin direction mask (which pins to set as outputs(1)/inputs(0))
  //  GPIO pin active level mask (which pins to set active HI(0)/active LO(1))
  //  GPIO pin state mask (which pins to set active(1)/inactive(0)))
  cameraDisable.configure(&xport,
			 CAMERA_DISABLE_GPIO_SELECT_MASK,
			 CAMERA_DISABLE_GPIO_DIR_MASK,
			 CAMERA_DISABLE_GPIO_ACTIVE_LEVEL_MASK,
			 CAMERA_DISABLE_GPIO_STATE_MASK);
  cameraDisable.setDirection(0x1, response);  // output 21 June 11 changed to 0x1
  cameraDisable.setActiveLevel(0x0, response);// active high

  Syslog::write("CameraDriver disabling 12V camera power supply");
  // enable vicor power supply
  cameraDisable.setInactive();                //21 June 11 changed to inactive


  //   Configure a XPortGPIO for the laser disable
  //   Constructor arguments:
  //   an XPort reference
  //  GPIO pin select mask (which pins to activate when triggering)
  //  GPIO pin direction mask (which pins to set as outputs(1)/inputs(0))
  //  GPIO pin active level mask (which pins to set active HI(0)/active LO(1))
  //  GPIO pin state mask (which pins to set active(1)/inactive(0)))

  // Note: LASER_DISABLE* not defined for XPort
  laserDisable.configure(&xport,
			 LASER_DISABLE_GPIO_SELECT_MASK,
			 LASER_DISABLE_GPIO_DIR_MASK,
			 LASER_DISABLE_GPIO_ACTIVE_LEVEL_MASK,
			 LASER_DISABLE_GPIO_STATE_MASK);

  laserDisable.setDirection(0x1, response);  // output 21 June 11 changed to 0x1
  laserDisable.setActiveLevel(0x0, response);// active high

  Syslog::write("CameraDriver enabling laser");
  // disable lasers
  laserDisable.setActive();

//
  // Create a BicamGPS 
  // Constructor arguments:
  // an XPort reference
  camGPS.configure(&xport);

  gettimeofday(&now,NULL);

  camGPS.update(now,
		Math::radToDeg(_position.latitude),
		Math::radToDeg(_position.longitude),
		_position.altitude);
}

/** Default Camera destructor */
Camera::~Camera()
{
  Syslog::write(" CameraDestructor Executing.");
  if( !_test)
    {
      _device->closeSD();
    }

  delete _log;
  delete _output;

  if( _sim )
    {
      return;
    }
  // disconnect XPort sockets
  printf("shutting down XPort sockets\n");
  xport.disconnectxp(XPCON_ALL);
}

/** Camera initialize method.
    satisfies TaskIF(?) interface.
*/
void Camera::initialize()
{

}
/**
   Read position data and periodically update camera GPS input.
*/
void Camera::readData()
{
  struct timeval now;
  static struct timeval previousUpdate;
  double elapsedSec;
  static Boolean init=FALSE;

  // initialize current time and previous update time
  gettimeofday(&now,NULL);
  if(init==FALSE){
    memcpy(&previousUpdate,&now,sizeof(struct timeval));
    init=TRUE;
  }

  //
  // Get current position
  //
  if( !_test )
    {
      _navigationIF->state(&_position, &_attitude);
    }
  else
    {
      _position.x += 0.2;        //Increment 20 cm.
      _position.latitude+=0.001;
      _position.longitude+=0.001;
      _position.altitude+=0.01;
      if(_position.latitude>35.0)
	{
	  _position.latitude=0.0;
	}
      if(_position.longitude>130.0)
	{
	  _position.longitude=0.0;
	}
      if(_position.altitude>10.0)
	{
	  _position.altitude=0.0;
	}
    }
  //
  // Update the GPS NMEA strings for the camera GPS serial input
  //
  // update() sets the GPS location (note that we only change four
  // parameters; the others are set in initialization)
 
  // writeNMEA() formats the NMEA strings
  // and sends them to the XPort.
  // GPS updates should be sent at a 1 Hz rate to
  // ensure that the camera maintains GPS lock; otherwise, the camera
  // will not record GPS metadata)

 if( !_sim )
    {
      // update current time
      gettimeofday(&now,NULL);

      // compute time since last update (current-previous)
      elapsedSec=((double)now.tv_sec+((double)now.tv_usec/1000000.0));
      elapsedSec-=((double)previousUpdate.tv_sec+((double)previousUpdate.tv_usec/1000000.0));
      // the loop runs every 200 ms, and we want to update
      // every 1 sec, so we have update at time > 0.8 sec
      if(elapsedSec >= 0.800000)
	{
	  //Syslog::write("elapsedSec=%2.6lf\n",elapsedSec);
	  // set previous update time = current time
	  memcpy(&previousUpdate,&now,sizeof(struct timeval));

	  // update NMEA field values (time, lat, lon, alt)
	  camGPS.update(now, 
			Math::radToDeg(_position.latitude),
			Math::radToDeg(_position.longitude),
			_position.altitude);
	  if( !_test )
	  Syslog::write("NMEA Update: %s\n",camGPS.getGPGGA()->toString());
	  // send NMEA to camera
	  camGPS.writeNMEA();
	}
    }
  
  //
  // Check to see if a client has requested photos:
  //
  _output->read();
  if( (_output->data.nPhotos || _output->data.onContinuously) 
       && (_output->data.cameraState == CameraIF::Ready)    ) 
    {
       if(_output->data.nPhotos) 
	  Syslog::write("CameraDriver::readData - starting a %d photo sequence.",
			_output->data.nPhotos);
       else
	  Syslog::write("CameraDriver::readData - Turning the camera on.");

      // Enable the strobes at the beginning of photo sequence
      // Delay ~ 2s after activation, before triggering
      // to allow strobes to charge.
      // Note that this must be done before running set to TRUE
      // to prevent the photo sequence to run
      // (There is probably a better place to define the start delay)
      if(!_sim) strobeDisable.setActive();
      sleep(_strobeEnableStartDelaySec);

      _nPhotos        = _output->data.nPhotos;
      _nSamplePeriods = _output->data.nSamplePeriods;
      _distance       = _output->data.distance;
      _first          = True;
      _running        = True;
      _output->data.cameraState = CameraIF::Busy;
      _xLast          = _position.x;
      _yLast          = _position.y;
      _output->data.nSamplePeriods = 0;
      _output->data.distance = 0.;
      _output->write();
    }      

  if( !_running )
    {
      return;
    }

  if( _output->data.onContinuously ) _nPhotos = 2;
  
  time_t localtime = time(NULL);

  if( _nSamplePeriods )
    {
      if( _nPhotos > 0 )
	{
	  if( (_samplePeriodCntr % _nSamplePeriods) == 0 )
	    {
	      _nPhotos--;
	      _photoCntr++;
	      if( _nPhotos <= 0 ) 
		{
		  // disable strobes at end of photo sequence
		  if(!_sim) strobeDisable.setInactive();
		  _running = False;
		  _output->data.cameraState = CameraIF::Ready;
		}
	      _samplePeriodCntr = 0;
	      //
	      //   pulse() activates the
	      //   XPort GPIO pin(s) selected in the 
	      //   XPort constructor by turning them on, waiting, then 
	      //   turning them off again
	      if( !_sim ) 
		{
		  camTrigger.pulse();
		}

	      //
	      // It would be nice to somehow embed the photo number _photoCntr.
	      //
              Syslog::write("CameraDriver::Took a photo at %s", ctime(&localtime));
	      _log->write();
	      _output->data.nPhotos = _nPhotos;
	      _output->data.photoCntr = _photoCntr;
	      _output->write();
	    }
	  _samplePeriodCntr++;
	}
    } 
  else  //_nSamplePeriods==0
    {
      double dx, dy;
      //
      // Take photos based on distance increment.
      //
      if(_first) 
	{
	  _dmg = 0.0;
	  _dmgIncr = 0.0;
	}
      dx = _position.x - _xLast;
      dy = _position.y - _yLast;
      _xLast = _position.x;
      _yLast = _position.y;
      _dmg += sqrt( dx*dx + dy*dy );
      _dmgIncr += sqrt( dx*dx + dy*dy );

      if( _dmgIncr >= _distance )
	{
	  _dmgIncr = 0.;
	  _nPhotos--;
	  _photoCntr++;
	  if( _nPhotos <= 0 ) 
	    {
	      // disable strobes at end of photo sequence
	      if(!_sim) strobeDisable.setInactive();
	      _running = False;
	      _output->data.cameraState = CameraIF::Ready;
	    }
	  //
	  // enable strobes and trigger camera
	  //
	  if( !_sim )
	    {
	      camTrigger.pulse();
	    }

	  //
	  // It would be nice to somehow embed the photo number _photoCntr.
	  //
	  Syslog::write("CameraDriver::Took a photo at %s.", ctime(&localtime));
	  _log->write();
	  _output->data.nPhotos = _nPhotos;
	  _output->data.photoCntr = _photoCntr;
	  _output->data.dmg = _dmg;
	  _output->write();
	}
    }
  _first = False;
}