/****************************************************************************/
/* Copyright (c) 2013 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Delta_T.cc                                                  */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 01/22/2013                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/

#include "Delta_T.h"
#include "BooleanAttribute.h"
#include "IntegerAttribute.h"
#include "FloatAttribute.h"
#include "StringAttribute.h"
#include "VerticalModeAttribute.h"
#include "MissionTimeAttribute.h"
#include "MathP.h"
#include "Syslog.h"

#define DEFAULT_STRING_ATTR "\"\""
#define NotSpecified -100000.0

Delta_T::Delta_T()
  : _server(NULL), Behavior(DeltaTBehaviorName, Sequential)
{
  _init = False;

  Boolean debug = False;

  //  Identifies which sonar to control 
  attributes.add(new IntegerAttribute("sonar",
                                      "Indicate which sonar to control (1=Side,0=Fwd.-1=>Both)",
				      &_sonar, -1));
  //  Controls logging on the MVC 
  attributes.add(new IntegerAttribute("logMode",
                                      "MVC Driver Logging on/off switch (1=On,0=Off)",
				      &_logMode, -1));
  //  Controls 837 data logging on the  Imagenex
  attributes.add(new IntegerAttribute("idtMode",
                                      "837 Data Logging on/off switch (1=On,0=Off)",
				      &_idtMode, -1));

  attributes.add(new FloatAttribute("soundSpeed",
                                      "Speed of Sound in m/sec (i.e., 1495.5)",
                  &_sos, -1.0));

  attributes.add(new IntegerAttribute("updateSOS",
                                    "Set to 1 to trigger SOS update using CTD",
                  &_update_sos, -1));

  attributes.add(new IntegerAttribute("autoSOS",
                     "Set to 1 to trigger automatic SOS update using CTD",
                  &_auto_sos, -1));

  attributes.add(new IntegerAttribute("receiveGain",
                                      "Gain in dB (1 to 20)",
				      &_gain, -1));
  attributes.add(new IntegerAttribute("gainEq",
                                      "Gain Equalization on/off switch (1=On,0=Off)",
				      &_gainEq, -1));
  attributes.add(new IntegerAttribute("range",
                                      "Set sonar range (1 to 100)",
				      &_range, -1));
  attributes.add(new IntegerAttribute("nBeams",
                                      "The number of beams to use (120, 240, 480)",
				      &_nBeams, -1));
  attributes.add(new IntegerAttribute("beamWidth",
                                      "Width of sonar beam (0=Wide,1=Normal,2=Narrow,3=Narrow-Mixed)",
				      &_width, -1));
  attributes.add(new IntegerAttribute("sectorSize",
                                      "Size of scan area (30deg, 60, 90, 120)",
				      &_size, -1));
  attributes.add(new IntegerAttribute("averaging",
                                      "Number of shots to average (0->off, 3, 5, 7)",
				      &_avg, -1));
  attributes.add(new IntegerAttribute("profile minimum range",
                                      "Profile Minimum Range (0 to 100 meters)",
				      &_pmr, -1));
  attributes.add(new IntegerAttribute("triggerEnable",
                                      "Trigger enable (1=>enable, 0=>not)",
				      &_tenable, -1));
  attributes.add(new IntegerAttribute("triggerEdge",
                                      "Trigger Edge   (1=>positive, 0=>negative)",
				      &_tedge, -1));
  attributes.add(new IntegerAttribute("triggerDelay",
                                      "Trigger delay in usec (0 to 1,000,000 usec)",
				      &_tdelay, -1));
  attributes.add(new IntegerAttribute("profilePointFiltering",
                                      "Profile Point Filter (0=>1st return, 1=>max return, 2=>bottom following)",
				      &_ppf, -1));
}

Delta_T::~Delta_T()
{
   if( !_server) 
   {
      delete _server;
      _server = NULL;
   }
}


Boolean Delta_T::shouldBehaviorStart( void ) 
{
  if (!_server) {
    try
    {
      _server = new DeltaTIF(DeltaTIFServerName);
    }
    catch (SharedObjectClient::MissingServer e) 
    {
      Syslog::write("Delta_T::Delta_T() - Missing server: %s; "
		    "continuing.\n",
		    e.msg);
      _server = NULL;
      return False;
    }
    Syslog::write("Delta_T::Delta_T() - Connected to server");
  }

  return horizontalSequence();
}

void Delta_T::execute()
{
  DeviceIF::Status status;
  Boolean debug = True;

  Boolean both = _sonar == -1;
  DeltaTIF::Sonar this_sonar = _sonar == 0? DeltaTIF::Fwd : DeltaTIF::Side;

  Syslog::write("Delta_T::Delta_T() - Running delta_t");
  // Perform the actions
  if (_logMode >= 0)
  {
    unsigned char errorCnt = 0;
    if (_logMode != 0)
    {
      Syslog::write("Delta_T Behavior: Starting logging");
      if (both) {
	_server->start(DeltaTIF::Fwd);
	_server->start(DeltaTIF::Side);
      }
      else
	_server->start(this_sonar);
    }
    else
    {
      Syslog::write("Delta_T Behavior: Stopping logging");
      if (both) {
	_server->stop(DeltaTIF::Fwd);
	_server->stop(DeltaTIF::Side);
      }
      else
	_server->stop(this_sonar);
    }
  }

  if (_idtMode >= 0)
  {
    if (_idtMode != 0)
    {
      Syslog::write("Delta_T Behavior: Starting 837 Data logging");
      if (both) {
	_server->set_idt_logging(DeltaTIF::Fwd, True);
	_server->set_idt_logging(DeltaTIF::Side, True);
      }
      else
	_server->set_idt_logging(this_sonar, True);
    }
    else
    {
      Syslog::write("Delta_T Behavior: Stopping 837 Data logging");
      if (both) {
	_server->set_idt_logging(DeltaTIF::Fwd, False);
	_server->set_idt_logging(DeltaTIF::Side, False);
      }
      else
	_server->set_idt_logging(this_sonar, False);
    }
  }

  if (_sos >= 0.0) {
    Syslog::write("Delta_T Behavior: Setting sos to %f", _sos);
    if (both) {
      _server->set_sos(DeltaTIF::Fwd, _sos);
      _server->set_sos(DeltaTIF::Side, _sos);
    }
    else
      _server->set_sos(this_sonar, _sos);
  }

  if (_update_sos >= 0) {
    float sos;
    _server->update_sos(&sos);
    Syslog::write("Delta_T Behavior: Updated sos to %f", sos);
    _update_sos = -1;  // Just do it once
  }

  if (_auto_sos >= 0) {
    _server->set_auto_sos(_auto_sos > 0? True : False);
    Syslog::write("Delta_T Behavior: AutoSOS to %d", _auto_sos);
    _auto_sos = -1;  // Just do it once
  }

  if (_gain > 0) {
    Syslog::write("Delta_T Behavior: Setting gain to %d", _gain);
    if (both) {
      _server->set_gain(DeltaTIF::Fwd, _gain);
      _server->set_gain(DeltaTIF::Side, _gain);
    }
    else
      _server->set_gain(this_sonar, _gain);
  }

  if (_gainEq >= 0) {
    Syslog::write("Delta_T Behavior: Setting gain eq to %d", _gainEq);
    if (both) {
      _server->set_gain_eq(DeltaTIF::Fwd, _gainEq);
      _server->set_gain_eq(DeltaTIF::Side, _gainEq);
    }
    else
      _server->set_gain_eq(this_sonar, _gainEq);
  }

  if (_range > 1) {
    Syslog::write("Delta_T Behavior: Setting range to %d", _range);
    if (both) {
      _server->set_range(DeltaTIF::Fwd, _range);
      _server->set_range(DeltaTIF::Side, _range);
    }
    else
      _server->set_range(this_sonar, _range);
  }

  if (_nBeams > 0) {
    Syslog::write("Delta_T Behavior: Setting beams to %d", _nBeams);
    if (both) {
      _server->set_nbeams(DeltaTIF::Fwd, _nBeams);
      _server->set_nbeams(DeltaTIF::Side, _nBeams);
    }
    else
      _server->set_nbeams(this_sonar, _nBeams);
  }

  if (_width >= 0) {
    Syslog::write("Delta_T Behavior: Setting beam width to %d", _width);
    if (both) {
      _server->set_beam_width(DeltaTIF::Fwd, _width);
      _server->set_beam_width(DeltaTIF::Side, _width);
    }
    else
      _server->set_beam_width(this_sonar, _width);
  }

  if (_size >= 30) {
    Syslog::write("Delta_T Behavior: Setting sector size to %d", _size);
    if (both) {
      _server->set_sector_size(DeltaTIF::Fwd, _size);
      _server->set_sector_size(DeltaTIF::Side, _size);
    }
    else
      _server->set_sector_size(this_sonar, _size);
  }

  if (_avg >= 0) {
    Syslog::write("Delta_T Behavior: Setting averaging to %d", _avg);
    if (both) {
      _server->set_averaging(DeltaTIF::Fwd, _avg);
      _server->set_averaging(DeltaTIF::Side, _avg);
    }
    else
      _server->set_averaging(this_sonar, _avg);
  }

  if (_pmr >= 0) {
    Syslog::write("Delta_T Behavior: Setting profile min range to %d", _pmr);
    if (both) {
      _server->set_profile_min_range(DeltaTIF::Fwd, _pmr);
      _server->set_profile_min_range(DeltaTIF::Side, _pmr);
    }
    else
      _server->set_profile_min_range(this_sonar, _pmr);
  }

  if (_tenable >= 0) {
    Syslog::write("Delta_T Behavior: Setting trigger enable to %d", _tenable);
    if (both) {
      _server->set_trigger_enable(DeltaTIF::Fwd, _tenable);
      _server->set_trigger_enable(DeltaTIF::Side, _tenable);
    }
    else
      _server->set_trigger_enable(this_sonar, _tenable);
  }

  if (_tedge >= 0) {
    Syslog::write("Delta_T Behavior: Setting trigger edge to %d", _tedge);
    if (both) {
      _server->set_trigger_edge(DeltaTIF::Fwd, _tedge);
      _server->set_trigger_edge(DeltaTIF::Side, _tedge);
    }
    else
      _server->set_trigger_edge(this_sonar, _tedge);
  }

  if (_tdelay >= 0) {
    Syslog::write("Delta_T Behavior: Setting trigger delay to %d", _tdelay);
    if (both) {
      _server->set_trigger_delay(DeltaTIF::Fwd, _tdelay);
      _server->set_trigger_delay(DeltaTIF::Side, _tdelay);
    }
    else
      _server->set_trigger_delay(this_sonar, _tdelay);
  }

  if (_ppf >= 0) {
    Syslog::write("Delta_T Behavior: Setting profile point filter to %d", _ppf);
    if (both) {
      _server->set_profile_point_filter(DeltaTIF::Fwd, _ppf);
      _server->set_profile_point_filter(DeltaTIF::Side, _ppf);
    }
    else
      _server->set_profile_point_filter(this_sonar, _ppf);
  }

  setState(Finished);

  return;
}


Boolean Delta_T::validInput()
{
  Boolean valid = False;

  if (_sonar == 0 || _sonar == 1 || _sonar == -1)
    valid = True;

  return valid;
}