/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : _kearfott.cc                                                  */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*-----------------------------------------------------------------------*
  CLASS: kearfott

  DESCRIPTION: Main file for octans driver

  AUTHOR: Hans Thomas

  $Id: _kearfottTest.cc,v 1.8 2001/08/02 02:00:15 hthomas Exp $
 *-----------------------------------------------------------------------*/
#include <i86.h>
#include <sys/sched.h>
#include <errno.h>
#include "System.h"
#include "Kearfott.h"
#include "Syslog.h"
#include "Task.h"



int main(int argc, char **argv)
{
     Boolean debug = True;
     Kearfott *kearfott = 0;
     SerialDevice *device = 0;
     Kearfott::KEARFOTT_OP_TYPE opData;
     Kearfott::KEARFOTT_CONFIG_TYPE config;
     Boolean sendConfig = False, sendOpData = False;

     // Set task priority
     if (System::setTaskPriority(NonCriticalDriverPriority) == -1)
       return 1;

     SerialParameters params(&argc, argv);

     if (argc >= 2) {
       if (!strcmp(argv[1], "config")) {

       } else if (!strcmp(argv[1], "stalign")) {
	 sendOpData = True;
	 opData.modes = STALIGNC|DOPPING|DOPENAB|ZUPENAB;
	 opData.validity = VDPTH|VPOS;
       } else if (!strcmp(argv[1], "align")) {
	 sendOpData = True;
	 opData.modes = GPSENAB|DOPPING|DOPENAB;
	 opData.validity = VDPTH|VERTDAMP_GPS;
       } else if (!strcmp(argv[1], "dopalign")) {
	 sendOpData = True;
	 opData.modes = DOPPING|DOPENAB;
	 opData.validity = VDPTH|VPOS;
       } else if (!strcmp(argv[1], "operate")) {
	 sendOpData = True;
	 opData.modes = DOPPING|DOPENAB;
	 opData.validity = VDPTH;
       } else if (!strcmp(argv[1], "restart")) {
	 sendOpData = True;
	 opData.modes = RESTART;
	 opData.validity = VDPTH|VERTDAMP_GPS|VPOS;
       } else {
	 sendOpData = False;
	 opData.modes = 0x00;
	 opData.validity = 0x00;
       }
     } else {
       sendOpData = False;
     }

     opData.init_lat = (36.76153/8.3819e-8);
     opData.init_lon = (-121.86935/8.3819e-8);
     opData.idepth=0;
     opData.salinity = 3350; opData.cepos = 100;
     opData.forcomm = 0;

     //set up configuration message
     config.valpha = config.vbeta = config.vgamma = 0;
     config.configBits = TSTMSGSEN;
     config.satime = 30;
     config.vdelx = config.vdely = config.vdelz = 0;
     config.gpslx = config.gpsly = 0; config.gpslz = 15;
     config.doplx = -1;
     config.doply = 4;
     config.doplz = -18;
     sendConfig = True;

     try {
       try {
	 device = new SerialDevice(&params);
       }
       catch(...) {
       }

       kearfott = new Kearfott(device, False);
       if (sendConfig == True) {
	 kearfott->sendConfigData(&config);
       }
       if (sendOpData == True) {
	 kearfott->sendOpData(&opData);
       }
       delay(1000);
       opData.validity &= ~VPOS;
       kearfott->sendOpData(&opData);
       kearfott->displayNavigationMessages(50);
       kearfott->run();
     }
     catch(Exception e) {
       Syslog::write("kearfott - caught Exception: %s\n", e.msg);
     }
     catch(...) {
       Syslog::write("kearfott - Exception caught\n");
     }

     delete kearfott;
     delete device;

     return 0;
}