/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : DropWeight.cc                                                 */
/* Author   : rsm                                                           */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <time.h>
#include <i86.h>
#include "Gulper.h"
#define READ_TIMEOUT 3000           //Milliseconds
#define MAXRECORDBYTES 512
#include "PeriodicTask.h"
#include "SerialDevice.h"
#include "Syslog.h"

Boolean debug = False;

Gulper::Gulper(SerialDevice *device)
   : PeriodicTask("gulperDriver") 
{

   _device = device;
   _log = new GulperLog(this, DataLog::BinaryFormat);
   _output= new GulperOutput();
   _output->data.FireGulper = -1;
   _output->write();
   _command = new GulperCommand(MessageQueue::ReadWrite);

   //initialize the nmc network;
   _picservo = new CPicServo();
   _device->setLineFormat(19200,8,1,"NONE");
   _device->raw();
   int numModules = _picservo->NmcInit(_device, 19200);
   Syslog::write("%d modules found on gulper network\n", numModules);
   if (numModules != 5) {
	Syslog::write("Gulper - ERROR - number of modules not 5\n");
	exit(-1);
   }

   initialize();
      
   addPeriodicCallback(1000, (CallbackMethod)Gulper::readData);
}


Gulper::~Gulper()
{
   Syslog::write(" GulperDestructor Executing.");
   delete _log;
   delete _output;
   if (_picservo) delete _picservo;
   delete _command;
}



void Gulper::initialize()
{
    if (!_picservo) return;
    if (_picservo->nummod != 5) return;

    for (int node = 1; node <= 5; node++) {
	//disable amp
        _picservo->ServoStopMotor(node, MOTOR_OFF);
        _picservo->NmcDefineStatus(node, SEND_POS|SEND_AUX);
        //reset position - we assume the motors are starting in home
        _picservo->ServoResetPos(node);
        //set up I/O control for 3 phase commutation
        _picservo->ServoSetIoCtrl(node, THREEPHASE_OUTPUT);
        //set up servo control with default parameters
        _picservo->ServoSetGain(node, 200, 1000, 0, 0, 130, 0, 32767, 1, 1); 
        _picservo->ServoLoadTraj(node,
				 LOAD_VEL|LOAD_ACC,
				 0, 4500000, 20000, 0);
				 
     }
}

void Gulper::homeGulper(int node)
{
    if (node > 5) return; //right now max of 5 nodes
    _picservo->ServoSetHoming(node, ON_LIMIT1|HOME_MOTOR_OFF);
    _picservo->ServoStopMotor(node, AMP_ENABLE);
    //actually start the homing move
    _picservo->ServoLoadTraj(node,
			     VEL_MODE|ENABLE_SERVO|LOAD_VEL|LOAD_ACC|START_NOW,
			     0, -4500000, 20000, 0);

    //wait until home is achieved
    do {
	_picservo->NmcNoOp(node);
    } while (_picservo->NmcGetStat(node) & HOME_IN_PROG);
    Syslog::write("Node %d reached home\n", node);   
    //zero the position
    _picservo->ServoResetPos(node);
    
}

void Gulper::readData()
{
//   _output->read();
GulperCommand::Command cmdIn;

   while ((_command->read(&cmdIn)) > 0) {
        Syslog::write("got command to fire %d\n", cmdIn.GulperToFire);
    	fireGulper(cmdIn.GulperToFire);

   }

//   for (int node=1; node <=5; node++) {
//      _picservo->NmcNoOp(node);
//     if (_picservo->NmcGetStat(node) & MOVE_DONE) 
//          _picservo->ServoStopMotor(node, MOTOR_OFF);
//   }
 
}

void Gulper::fireGulper(long gulperToFire)
{
    //gulpers are numbers 0 to N-1. Each node fires four gulpers in sequence
    int node = gulperToFire / 4 + 1;
    if (node > 5) {
	Syslog::write("Gulper - gulperToFire %d decoded to node %d which is too big\n",
	              gulperToFire, node);
	return;
    }

    int gulper = (gulperToFire % 4) + 1;
    long pos = gulper * (-189233);
    _picservo->ServoStopMotor(node, AMP_ENABLE);
    _picservo->ServoLoadTraj(node, LOAD_POS|ENABLE_SERVO|START_NOW, pos, 0, 0, 0); 

    Syslog::write("Gulper - sending node %d to position %d\n", node, pos);
}