/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : BogusLayeredControl.cc                                        */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <stdio.h>
#include <unistd.h>
#include <process.h>
#include "BogusLayeredControl.h"
#include "LayeredControlServer.h"

#define LayeredControlServerName "lcServer"

BogusLayeredControl::BogusLayeredControl(const char *name)
  : PeriodicTask(name, LayeredControlName, 1)
{
  _output = new LayeredControlOutput(LayeredControlOutputName, 
				     SharedData::ReadWrite);

  // Initialize command structure
  _command.depthMode = LayeredControlIF::DmInitial;
  _command.depth = 0.;
  _command.headingMode = LayeredControlIF::HmInitial;
  _command.heading = 0.;
  _command.speedMode = LayeredControlIF::SmInitial;
  _command.speed = 0.;

  addCallback(1000, (CallbackMethod )callback);
}


BogusLayeredControl::~BogusLayeredControl()
{
}


void BogusLayeredControl::callback()
{
  while (1) {

    fprintf(stderr, "BogusLayeredControl::run() - generate command\n");

    // Fill in command structure with some bogus values
    _command.depthMode = LayeredControlIF::Elevator;
    _command.depth += 1.;
    _command.headingMode = LayeredControlIF::Waypoint;
    _command.heading += 2.;
    _command.speedMode = LayeredControlIF::Current;
    _command.speed += 3.;

    // Make output available to server and notify
    _output->write(&_command);

    triggerEvent(LayeredControlIF::OutputGenerated);

    // Sleep for a while
    sleep(3);
  }
}