/****************************************************************************/
/* Copyright (c) 2012 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : DeltaTServer.cc                                               */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 01/17/2013                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <process.h>

#include "System.h"
#include "Syslog.h"
#include "DeltaTIF_SK.h"
#include "DeltaTServer.h"
#include "FloatAttribute.h"
#include "IntegerAttribute.h"
#include "StringAttribute.h"

#define DEFAULT_SOS  1495.0
#define LOW_SOS      1400.0
#define HIGH_SOS     1600.0

static int sig_interval = 60;  // Update speed of sound at this interval (seconds)

static DeltaTServer* dts = NULL;

static int idts[2] = {DeltaTIF::Fwd, DeltaTIF::Side};

//////////////////////////////////////////////////////////////////////////////
// Function used to signal this server to acquire fresh sound speed data
void DeltaTServer::usr1SignalHandler(int sig)
{
   if (dts)
   {
      dts->get_sos_data(DeltaTIF::Side);
      dts->get_sos_data(DeltaTIF::Fwd);
   }
}


//////////////////////////////////////////////////////////////////////////////
// Constructor
DeltaTServer::DeltaTServer(const char *configFileName) 
  : _attributes("DeltaTServer"), _auto_sos(True)
{
  char attr_name[64];
  long auto_sos;

  sprintf(attr_name, "log83P");
  _attributes.add(new IntegerAttribute(attr_name, attr_name,
				      &_log83P, 0));
  sprintf(attr_name, "speedOfSound");
  _attributes.add(new FloatAttribute(attr_name, attr_name,
				     &_sos, DEFAULT_SOS));
  sprintf(attr_name, "AutoSOS");
  _attributes.add(new IntegerAttribute(attr_name, attr_name,
                  &auto_sos, 0));

  // get sonar attrs
  //
  char* prefix[2] = {"fwd", "side"};  // used to build the attr names
  for (int i = 0; i < 2; i++)
  {
    int this_idt = idts[i];

    // "fwdHost", "fwdName", ..., "sideHost", "sideName", ...
    //
    sprintf(attr_name, "%s%s", prefix[i], "Host");   // "fwdHost" / "sideHost"
    _attributes.add(new StringAttribute(attr_name, attr_name,
					&_host[this_idt], "NotSpecified"));
    sprintf(attr_name, "%s%s", prefix[i], "Name");
    _attributes.add(new StringAttribute(attr_name, attr_name,
					&_name[this_idt], "_Fwd"));
    sprintf(attr_name, "%s%s", prefix[i], "Port");
    _attributes.add(new StringAttribute(attr_name, attr_name,
					&_port[this_idt], "0"));
    sprintf(attr_name, "%s%s", prefix[i], "Phi");
    _attributes.add(new FloatAttribute(attr_name, attr_name,
				       &_phi[this_idt], 0.));
    sprintf(attr_name, "%s%s", prefix[i], "Theta");
    _attributes.add(new FloatAttribute(attr_name, attr_name,
				       &_theta[this_idt], 0.));
    sprintf(attr_name, "%s%s", prefix[i], "Psi");
    _attributes.add(new FloatAttribute(attr_name, attr_name,
				       &_psi[this_idt], 0.));
    sprintf(attr_name, "%s%s", prefix[i], "ProfileMinimumRange");
    _attributes.add(new IntegerAttribute(attr_name, attr_name,
					 &_pmr[this_idt], 1));
    sprintf(attr_name, "%s%s", prefix[i], "TiltAngle");
    _attributes.add(new IntegerAttribute(attr_name, attr_name,
					 &_tilt[this_idt], 0));
    sprintf(attr_name, "%s%s", prefix[i], "TriggerEnable");
    _attributes.add(new IntegerAttribute(attr_name, attr_name,
					 &_tenable[this_idt], 1));
    sprintf(attr_name, "%s%s", prefix[i], "TriggerEdge");
    _attributes.add(new IntegerAttribute(attr_name, attr_name,
					 &_tedge[this_idt], 1));
    sprintf(attr_name, "%s%s", prefix[i], "TriggerDelay");
    _attributes.add(new FloatAttribute(attr_name, attr_name,
				       &_tdelay[this_idt], 0.));
    sprintf(attr_name, "%s%s", prefix[i], "XducerOrientation");
    _attributes.add(new IntegerAttribute(attr_name, attr_name,
					 &_xdo[this_idt], 0));
    sprintf(attr_name, "%s%s", prefix[i], "ProfilePointFilter");
    _attributes.add(new IntegerAttribute(attr_name, attr_name,
					 &_ppf[this_idt], 2));
    sprintf(attr_name, "%s%s", prefix[i], "Gain");
    _attributes.add(new IntegerAttribute(attr_name, attr_name,
					 &_gain[this_idt], 12));
    sprintf(attr_name, "%s%s", prefix[i], "Range");
    _attributes.add(new IntegerAttribute(attr_name, attr_name,
					 &_range[this_idt], 100));
    sprintf(attr_name, "%s%s", prefix[i], "SectorSize");
    _attributes.add(new IntegerAttribute(attr_name, attr_name,
           &_sector[this_idt], 120));
  }

  AttributeParser::parse(configFileName, &_attributes);

  System::copyToLogDir(configFileName);

  _auto_sos = auto_sos == 0? False : True;

  Syslog::write("DeltaTServer config: 83P: %d, Fwd: %s, %s, %s\tSide: %s, %s, %s\n", _log83P,
		_name[DeltaTIF::Fwd], _host[DeltaTIF::Fwd], _port[DeltaTIF::Fwd],
		_name[DeltaTIF::Side], _host[DeltaTIF::Side], _port[DeltaTIF::Side]);

  // If a host for the foward-looking soanr beamformer is
  // specified, then create the objects for the sonar.
  // Do the same for the side-looking sonar as well.
  //
  char compound_name[256];
  _input[DeltaTIF::Fwd] = new DeltaTInput(DeltaTInput::makeName(compound_name,
						      (char*)_name[DeltaTIF::Fwd],
						      sizeof(compound_name)));
  _input[DeltaTIF::Side] = new DeltaTInput(DeltaTInput::makeName(compound_name,
						       (char*)_name[DeltaTIF::Side],
						       sizeof(compound_name)));
  _msgQ[DeltaTIF::Fwd] = new DeltaTMessage(MessageQueue::Write,
			   DeltaTMessage::makeName(compound_name,
						   (char*)_name[DeltaTIF::Fwd],
						   sizeof(compound_name)));
  _msgQ[DeltaTIF::Side] = new DeltaTMessage(MessageQueue::Write,
			   DeltaTMessage::makeName(compound_name,
						   (char*)_name[DeltaTIF::Side],
						   sizeof(compound_name)));

  _output[DeltaTIF::Fwd] = new DeltaTOutput(DeltaTOutput::makeName(compound_name,
						    (char*)_name[DeltaTIF::Fwd],
						    sizeof(compound_name)));
  _output[DeltaTIF::Side]= new DeltaTOutput(DeltaTOutput::makeName(compound_name,
						 (char*)_name[DeltaTIF::Side],
						 sizeof(compound_name)));

  System::copyToLogDir(configFileName);

  config_drivers();

//  attach_ctd_server(10);

  dts = this;

}

// Return the _auto_sos configuration flag
//
Boolean DeltaTServer::auto_sos()
{
   return _auto_sos;
}

// Configure the drivers here.
// Writes messages to the msg queues to be processed by the drivers
// when they are started.
//
void DeltaTServer::config_drivers()
{
  // Set-up the drivers according to the config file.
  //

  for (int i = 0; i < 2; i++)
  {
    int this_idt = idts[i];

    DeltaTMessage::Message pmr(DeltaTMessage::SetProfileMinRange, (int)_pmr[this_idt]);
    _msgQ[this_idt]->write(&pmr);

    DeltaTMessage::Message tilt(DeltaTMessage::SetTiltAngle, (int)_tilt[this_idt]);
    _msgQ[this_idt]->write(&tilt);

    DeltaTMessage::Message ten(DeltaTMessage::SetTriggerEnable, (int)_tenable[this_idt]);
    _msgQ[this_idt]->write(&ten);

    DeltaTMessage::Message ted(DeltaTMessage::SetTriggerEdge, (int)_tedge[this_idt]);
    _msgQ[this_idt]->write(&ted);

    DeltaTMessage::Message tdly(DeltaTMessage::SetTriggerDelay, (float)_tdelay[this_idt]);
    _msgQ[this_idt]->write(&tdly);

    DeltaTMessage::Message xdo(DeltaTMessage::SetXducerOrientation, (int)_xdo[this_idt]);
    _msgQ[this_idt]->write(&xdo);

    DeltaTMessage::Message ppf(DeltaTMessage::SetProfilePointFilter, (int)_ppf[this_idt]);
    _msgQ[this_idt]->write(&ppf);

    DeltaTMessage::Message gain(DeltaTMessage::SetGain, (int)_gain[this_idt]);
    _msgQ[this_idt]->write(&gain);

    DeltaTMessage::Message range(DeltaTMessage::SetRange, (int)_range[this_idt]);
    _msgQ[this_idt]->write(&range);

    DeltaTMessage::Message sector(DeltaTMessage::SetSectorSize, (int)_sector[this_idt]);
    _msgQ[this_idt]->write(&sector);

    DeltaTMessage::Message auto_sos(DeltaTMessage::SetAutoSOS, _auto_sos? 1 : 0);
    _msgQ[this_idt]->write(&auto_sos);
  }

}

//////////////////////////////////////////////////////////////////////////////
// Destructor
DeltaTServer::~DeltaTServer()
{
  delete _input[DeltaTIF::Fwd];
  delete _output[DeltaTIF::Fwd];
  delete _msgQ[DeltaTIF::Fwd];
  delete _input[DeltaTIF::Side];
  delete _output[DeltaTIF::Side];
  delete _msgQ[DeltaTIF::Side];
}

DeviceIF::Status DeltaTServer::update_sos(float *sos)
{
   *sos = get_sos_data(DeltaTIF::Side);
   *sos = get_sos_data(DeltaTIF::Fwd);
   return DeviceIF::Ok;
}
//////////////////////////////////////////////////////////////////////////////
// Get sound speed from ctd. Attempt to attach a ctd server if necessary.
float DeltaTServer::get_sos_data(DeltaTIF::Sonar sonar)
{
  Syslog::write("delta_tServer::get_sos_data()");

  if (!_ctd) {
    attach_ctd_server(0);
  }

  float sos = 0.;
  TimeIF::TimeSpec sampleTime;
  if (_ctd) {
    _ctd->soundVelocity(&sos, &sampleTime);
    Syslog::write("DeltaTServer::get_sos_data() - update SOS: %f", sos);
    if (strcmp("0", _port[sonar])) { 
      Syslog::write("delta_tServer::get_sos_data(): Driver for sonar %d not active", sonar);
    }
    else
    {
      // Set the sos on the sonar
      //
      if (sos > LOW_SOS && sos < HIGH_SOS)
        set_sos(sonar, sos); 
      else
        Syslog::write("DeltaTServer::get_sos_data() - not in valid range (1460-1505), sticking with current value");
    }

  }
  return sos;
}

//////////////////////////////////////////////////////////////////////////////
// Set a signal (SIGUSR1) timer get sos data
void DeltaTServer::set_timer()
{
  // Describe signal info
  struct sigevent signalSpec;
  signalSpec.sigev_signo = SIGUSR1;

  // Create the timer
  timer_t timer = timer_create(CLOCK_REALTIME, &signalSpec);

  // Set the signal handler
  signal(SIGUSR1, DeltaTServer::usr1SignalHandler);

  //////////////////////////////////////////////
  // Set the timer
  struct itimerspec timerSetting;

  timerSetting.it_value.tv_sec = sig_interval;
  timerSetting.it_value.tv_nsec = 0;
  timerSetting.it_interval.tv_sec = sig_interval;
  timerSetting.it_interval.tv_nsec = 0;

  timer_settime(timer, 0, &timerSetting, 0);
}

//////////////////////////////////////////////////////////////////////////////
// Attempt to connect to the FastCat server. Non-fatal if none available.
void DeltaTServer::attach_ctd_server(int wait_time)
{
  try {
    _ctd = new FastCatIF("fastcat", "FastCat", wait_time);
    sig_interval = 15;
  }
  catch( Exception e ) {
    Syslog::write("delta_tServer -- Caught exception: %s", e.msg );
  }
  catch( ... ) {
    Syslog::write("delta_tServer -- Caught exception!\n");
  }

  if (!_ctd) {
    Syslog::write("DeltaTServer::get_sos_data() - no FastCat server available yet");
  }
}

///////////////////////////////////////////////////////////////////////////////
// DeltaTIF interface functions.
///////////////////////////////////////////////////////////////////////////////

// Set the profile point filter
DeviceIF::Status DeltaTServer::set_profile_point_filter(DeltaTIF::Sonar sonar, short ppf)
{
  if (ppf < 0 || ppf > 2) {
    Syslog::write("DeltaTServer: Filter value of %d is invalid (should be [0|1|2])",
		  ppf);
    return DeviceIF::Error;
  }

  DeltaTMessage::Message msg(DeltaTMessage::SetProfilePointFilter, (int)ppf);
  _msgQ[sonar]->write(&msg);

  return DeviceIF::Ok;
}

// Set the trigger delay
DeviceIF::Status DeltaTServer::set_trigger_delay(DeltaTIF::Sonar sonar, long delay)
{
  if (delay < 0 || delay > 1000000L) {
    Syslog::write("DeltaTServer: Trigger delay of %d is invalid (should be 0 to 1000000)",
		  delay);
    return DeviceIF::Error;
  }

  DeltaTMessage::Message msg(DeltaTMessage::SetTriggerDelay, (float)delay);
  _msgQ[sonar]->write(&msg);

  return DeviceIF::Ok;
}

// Set the trigger edge
DeviceIF::Status DeltaTServer::set_trigger_edge(DeltaTIF::Sonar sonar, short edge)
{
  if (edge < 0 || edge > 1) {
    Syslog::write("DeltaTServer: Trigger edge of %d is invalid (should be [0|1])",
		  edge);
    return DeviceIF::Error;
  }

  DeltaTMessage::Message msg(DeltaTMessage::SetTriggerEdge, (int)edge);
  _msgQ[sonar]->write(&msg);

  return DeviceIF::Ok;
}

// Set the trigger enable
DeviceIF::Status DeltaTServer::set_trigger_enable(DeltaTIF::Sonar sonar, short enable)
{
  if (enable < 0 || enable > 1) {
    Syslog::write("DeltaTServer: Trigger enable value of %d is invalid (should be [0|1])",
		  enable);
    return DeviceIF::Error;
  }

  DeltaTMessage::Message msg(DeltaTMessage::SetTriggerEnable, (int)enable);
  _msgQ[sonar]->write(&msg);

  return DeviceIF::Ok;
}

// Set the profile min range
DeviceIF::Status DeltaTServer::set_profile_min_range(DeltaTIF::Sonar sonar, short range)
{
  if (range < 0 || range > 100) {
    Syslog::write("DeltaTServer: Profile min range value of %d is invalid (should be 0 to 100)",
		  range);
    return DeviceIF::Error;
  }

  DeltaTMessage::Message msg(DeltaTMessage::SetProfileMinRange, (int)range);
  _msgQ[sonar]->write(&msg);

  return DeviceIF::Ok;
}

///////////////////////////////////////////////////////////////////////////////
// Set the receive gain
DeviceIF::Status DeltaTServer::set_gain(DeltaTIF::Sonar sonar, short gain)
{
  if (gain < 1 || gain > 20) {
    Syslog::write("DeltaTServer: Gain value of %d is invalid (should be 1 to 20)",
		  gain);
    return DeviceIF::Error;
  }

  DeltaTMessage::Message set_gain(DeltaTMessage::SetGain, gain);
  _msgQ[sonar]->write(&set_gain);

  return DeviceIF::Ok;
}


///////////////////////////////////////////////////////////////////////////////
// Set the averaging used in the beam forming app
DeviceIF::Status DeltaTServer::set_averaging(DeltaTIF::Sonar sonar, short avg)
{
  if (avg == 0 || avg == 1  //Averaging, 0,1=Off, 3, 5, 7 = #shots to average
      || avg == 3 || avg == 5 || avg == 7) {
    DeltaTMessage::Message set_avg(DeltaTMessage::SetAveraging, avg);
    _msgQ[sonar]->write(&set_avg);
    return DeviceIF::Ok;
  }
  else {
    Syslog::write("DeltaTServer: Averaging value of %d is invalid (0|1|3|5|7)", avg);
    return DeviceIF::Error;
  }
}


///////////////////////////////////////////////////////////////////////////////
// Set the beam width on the beam forming app
DeviceIF::Status DeltaTServer::set_beam_width(DeltaTIF::Sonar sonar, short width)
{
  //Beamwidth, 0=Wide, 1=Normal, 2=Narrow, 3=Narrow Mixed
  if (width >= 0 && width <=3) {
    DeltaTMessage::Message set_width(DeltaTMessage::SetBeamWidth, width);
    _msgQ[sonar]->write(&set_width);
    return DeviceIF::Ok;
  }
  else {
    Syslog::write("DeltaTServer: Beam width value of %d is invalid (0|1|2|3)", width);
    return DeviceIF::Error;
  }
}


///////////////////////////////////////////////////////////////////////////////
// Set the sector size on the beam forming app
DeviceIF::Status DeltaTServer::set_sector_size(DeltaTIF::Sonar sonar, short size)
{
  if (size == 30 || size == 60 || size == 90 || size == 120) {
    DeltaTMessage::Message set_size(DeltaTMessage::SetSectorSize, size);
    _msgQ[sonar]->write(&set_size);
    return DeviceIF::Ok;
  }
  else {
    Syslog::write("DeltaTServer: Invalid sector size value: %d (30|60|90|120)", size);
    return DeviceIF::Error;
  }
}


///////////////////////////////////////////////////////////////////////////////
// Set the number of beams on the beam forming app
DeviceIF::Status DeltaTServer::set_nbeams(DeltaTIF::Sonar sonar, short nb)
{
  if (nb == 120 || nb == 240 || nb == 480) {
    DeltaTMessage::Message set_nbeams(DeltaTMessage::SetNBeams, nb);
    _msgQ[sonar]->write(&set_nbeams);
    return DeviceIF::Ok;
  }
  else {
    Syslog::write("DeltaTServer: Invalid value for nbeams: %d (120|240|480)", nb);
    return DeviceIF::Error;
  }
}


///////////////////////////////////////////////////////////////////////////////
// Set gain equalization on the beam forming app on or off
DeviceIF::Status DeltaTServer::set_gain_eq(DeltaTIF::Sonar sonar, Boolean on)
{
  int on_off;
  if (on)
    on_off = 1;
  else
    on_off = 0;

  DeltaTMessage::Message set_gain_eq(DeltaTMessage::SetGainEq, on_off);
  _msgQ[sonar]->write(&set_gain_eq);

  return DeviceIF::Ok;
}


///////////////////////////////////////////////////////////////////////////////
// Turn logging on the DeltaT app on or off
DeviceIF::Status DeltaTServer::set_idt_logging(DeltaTIF::Sonar sonar, Boolean on)
{
  int on_off;
  if (on)
    on_off = 1;
  else
    on_off = 0;

  printf("Set idt logging %d\n", on_off);

  DeltaTMessage::Message set_logging(DeltaTMessage::SetLogging, on_off);
  _msgQ[sonar]->write(&set_logging);

  return DeviceIF::Ok;
}


DeviceIF::Status DeltaTServer::set_auto_sos(Boolean on)
{
   _auto_sos = on;
  int on_off;
  if (on)
    on_off = 1;
  else
    on_off = 0;

  DeltaTMessage::Message set_auto(DeltaTMessage::SetAutoSOS, on_off);
  printf("\t\tSending SetAutoSOS(%d)\n", on_off);
  _msgQ[0]->write(&set_auto);
  _msgQ[1]->write(&set_auto);


   return DeviceIF::Ok;
}

///////////////////////////////////////////////////////////////////////////////
// Set sos used by the sonar to calculate point values
DeviceIF::Status DeltaTServer::set_sos(DeltaTIF::Sonar sonar, float sos)
{
   Syslog::write("\t\t\tSetting sos to %f", sos);
   DeltaTMessage::Message set_sos(DeltaTMessage::SetSOS, sos);
   _msgQ[sonar]->write(&set_sos);

   return DeviceIF::Ok;
}


///////////////////////////////////////////////////////////////////////////////
// Set the sonar max range
DeviceIF::Status DeltaTServer::set_range(DeltaTIF::Sonar sonar, short range)
{
  DeltaTMessage::Message set_range(DeltaTMessage::SetRange, range);
  _msgQ[sonar]->write(&set_range);

  return DeviceIF::Ok;
}


///////////////////////////////////////////////////////////////////////////////
// Start local logging
void DeltaTServer::start(DeltaTIF::Sonar sonar)
{
  _input[sonar]->data.enabled = True;
  _input[sonar]->write();

  DeltaTMessage::Message start_log(DeltaTMessage::StartMVCLogging);
  Syslog::write("Sending start message thru msgQ %s\n", _msgQ[sonar]->name());
  _msgQ[sonar]->write(&start_log);
}


///////////////////////////////////////////////////////////////////////////////
// Stop local logging
void DeltaTServer::stop(DeltaTIF::Sonar sonar)
{
  _input[sonar]->data.enabled = False;
  _input[sonar]->write();

  DeltaTMessage::Message stop_log(DeltaTMessage::StopMVCLogging);
  _msgQ[sonar]->write(&stop_log);
}


///////////////////////////////////////////////////////////////////////////////
// Is local logging enabled?
Boolean DeltaTServer::enabled(DeltaTIF::Sonar sonar)
{
  _output[sonar]->read();

  return _output[sonar]->data.enabled;
}


///////////////////////////////////////////////////////////////////////////////
//
void DeltaTServer::name(DeviceIF::Name name)
{
  strcpy(name, "DeltaTServer");
}


///////////////////////////////////////////////////////////////////////////////
// Get the most recent range value
double DeltaTServer::range(DeltaTIF::Sonar sonar)
{
  _output[sonar]->read();
  return _output[sonar]->data.dt_range;
}


///////////////////////////////////////////////////////////////////////////////
// Get the number of beams in the most recent sonar data packet
unsigned short DeltaTServer::nbeams(DeltaTIF::Sonar sonar)
{
  _output[sonar]->read();
  return _output[sonar]->data.nbeams;
}


///////////////////////////////////////////////////////////////////////////////
// Get the most recent ping sample interval
unsigned short DeltaTServer::interval(DeltaTIF::Sonar sonar)
{
  _output[sonar]->read();
  return _output[sonar]->data.dt_interval;
}


///////////////////////////////////////////////////////////////////////////////
// Get the most recent update time
void DeltaTServer::update_time(DeltaTIF::Sonar sonar, TimeIF::TimeSpec *updateTime)
{
  _output[sonar]->read();
  updateTime->seconds      = _output[sonar]->data.update_time.seconds;
  updateTime->nanoSeconds  = _output[sonar]->data.update_time.nanoSeconds;

  return;
}


///////////////////////////////////////////////////////////////////////////////
// Get the most recent driver output
void DeltaTServer::get(DeltaTIF::Sonar sonar, DeltaTIF::Data *data)
{
  _output[sonar]->read();
  _input[sonar]->read();
  data->enabled = _input[sonar]->data.enabled;
  data->update_time.seconds      = _output[sonar]->data.update_time.seconds;
  data->update_time.nanoSeconds  = _output[sonar]->data.update_time.nanoSeconds;
  data->range = _output[sonar]->data.dt_range;
  data->interval = _output[sonar]->data.dt_interval;
  data->nbeams = _output[sonar]->data.nbeams;

  for (int i = 0; i < data->nbeams; i++) {
    data->beam_ranges[i] = _output[sonar]->data.beam_ranges[i];
    data->intensities[i] = (short)_output[sonar]->data.intensities[i];
  }

}


///////////////////////////////////////////////////////////////////////////////
// Start the DeltaT driver program
int DeltaTServer::spawnAuxTasks()
{
  // Use configuration values to determine what drivers to create
  //
  char program[256];
  char errorBuf[256];
  char buf[256];
  pid_t pid;

  const char* argv[10];
  int argc = 0;
  // These sonars have a beamformer that sends to a specified port,
  // so a driver for both those ports will be created
  //
  Syslog::write("DeltaTServer: spawning driver...");
  argv[argc++] = strdup("delta_t");

  // Logging 83P?
  //
  if (_log83P)
    argv[argc++] = "-p";

  // Build arg list for the forward sonar if present.
  // A port of "0" in the config file means no sonar in that position.
  //
  if (0 != strcmp("0", _port[DeltaTIF::Fwd])) { 
    argv[argc++] = strdup("-f");
    sprintf(buf, "%s:%s", _name[0], _port[0]);
    argv[argc++] = strdup(buf);
  }

  // Build arg list for the side sonar if present.
  // A port of "0" in the config file means no sonar in that position.
  //
  if (0 != strcmp("0", _port[DeltaTIF::Side])) { 
    argv[argc++] = strdup("-s");
    sprintf(buf, "%s:%s", _name[1], _port[1]);
    argv[argc++] = strdup(buf);
  }
  argv[argc] = (char *)0;

  for (int i = 0; i < argc; i++)
    printf("%s  ", argv[i]);
  printf("\n");

  switch ((pid = fork())) {
     
  case 0:
    // In child
    // Execute driver program

    execvp(argv[0], argv);

    // If execution arrives here, we have encountered an error
    sprintf(errorBuf, 
	    "Task::spawnServer() - exec() of \"%s\" failed", program);
    perror(errorBuf);
    break;

  case -1:
    perror("Task::spawnServer() - fork() failed");
    return -1;
  }

  return 0;

}