/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : IridiumDriver.cc                                              */
/* Author   : hans                                                          */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 11/01/2005                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "IridiumDriver.h"
#include "MathP.h"
#include "Syslog.h"
#include "string.h"

#define READ_TIMEOUT (6000)

IridiumDriver::IridiumDriver(SerialDevice *device):Task("IridiumDriver")
{
  try {
    _gpsIF = new GpsIF("gps");
  }
  catch(...) {
    Syslog::write("IridiumDriver::IridiumDriver - Failed to set up gps IF");
  }

  try {
    _layeredControlIF = new LayeredControlIF("layeredControl");
  }
  catch (...) {
    Syslog::write("IridiumDriver::IridiumDriver - Failed to set up layered control IF");
  }

  try {
    _bluefinBattOutput = new BluefinBattOutput(3, SharedData::ReadWrite);
  }
  catch (...) {
    Syslog::write("IridiumDriver::IridiumDriver - failed to set up battery IF");
  }

  _lastUpdateTime = 0;
  _device = device;
  Initialize();
}

IridiumDriver::~IridiumDriver()
{
  delete _device;
  delete _gpsIF;
  delete _layeredControlIF;
  delete _bluefinBattOutput;
}

void IridiumDriver::Initialize()
{
  char buf[20];
  int nbytes;
  char *ptr;

  _device->raw(20,10);
//  sprintf(buf,"AT+CBST=6,0,1\xd\xa");
//  _device->write(buf, strlen(buf));
//  if (nbytes = _device->read(buf, 6, READ_TIMEOUT)) {
//    if (ptr = strstr(buf,"OK")) {
//      printf("IridiumDriver::Initialize() - response was %s", ptr);
//    }
//  }
//  printf("set up CBST\n");

  sprintf(buf,"AT+CR=0\xd\xa");
  _device->write(buf, strlen(buf));
  if (nbytes = _device->read(buf, 6, READ_TIMEOUT)) {
    if (ptr = strstr(buf,"OK")) {
      printf("IridiumDriver::Initialize() - response was %s\n", ptr);
    }
  }
  printf("set up CR\n");
}

int IridiumDriver::getSignalQual()
{
  char buf[80];
  int qual = 0;
  int nbytes;
  char *ptr;

  sprintf(buf,"AT+CSQ\xd\xa");
  printf("sending %s\n", buf);
  _device->write(buf, strlen(buf));
  if (nbytes = _device->read(buf, 16, 20000)) {
    printf("buf was %d\n%s\n",nbytes,buf);
    buf[nbytes] = 0;
    if (ptr = strstr(buf,"+CSQ:")) {
      printf("IridiumDriver::getSignalQual() - response was %s\n", buf);
      if (sscanf(ptr,"+CSQ:%d", &qual) != 1) {
          printf("IridiumDriver::getSignalQual() - failed to read quality\n");
      }
    }    
  } else {
    printf("iridiumDriver::getSignalQual() - read timeout\n");
  }
  printf("signal quality is %d\n", qual);
  return qual;
}

int IridiumDriver::sendIridiumDAVUpdate(GpsIF::Fix *fix)
{
char cmd[80];
char buf[20];
int nbytes;
char *ptr;
int status;

    sprintf(cmd, "ATD00881621466972\xd\xa");
    _device->write(cmd, strlen(cmd));
    printf("DIALING\n");
    nbytes = _device->read(buf, 20, 120000);
    if (nbytes < 0) {
	printf("no response from connect\n");
	return (-1);
    } 
    printf("response was %s\n", buf);
    if (ptr = strstr(buf, "NO")) {
	status = 0;
        printf("negative reply was %s\n", ptr);
    } else if (ptr = strstr(buf,"BU")) {
	status = 0;
    } else if (ptr = strstr(buf,"CO")) {
	status = 1;
        sprintf(cmd,"%02d:%02d:%02d %2.6f %3.6f %2d\xd\xa",
	    fix->ggatime.hours, fix->ggatime.minutes, fix->ggatime.seconds,
	    Math::radToDeg(fix->latitude),
	    Math::radToDeg(fix->longitude),
            fix->nSatellites);
  	_device->write(cmd, strlen(cmd));
	printf("sending update %s\n", cmd);
        sleep(20);
        _device->write("+++", 3);
        if(nbytes = _device->read(buf, 6, 5000)) {
	    sprintf(cmd, "ATH0\xd\xa");
	    printf("HANGUP\n");
	    _device->write(cmd, strlen(cmd));
	    sleep(2);
        }
    } 
    return(status);
}

int IridiumDriver::sendIridiumSBDUpdate(GpsIF::Fix *fix,
					double elapsedTime,
					LayeredControlIF::MissionState state,
					LayeredControlIF::Name behaviorName,
					short behaviorIndex)
{
char cmd[300];
char buf[40];
int nbytes;
char *ptr;
int i,status=0;
unsigned short checksum = 0, sval;
int MOstatus, MOMSN,MTstatus,MTMSN,MTlength,MTqueued;

    sprintf(cmd,"%02d:%02d:%02d %2.6f %3.6f %2d %2.2f %1.2f %1d %2.2f %1.2f %1d %2.2f %1.2f %1d\0",
	    fix->ggatime.hours, fix->ggatime.minutes, fix->ggatime.seconds,
	    Math::radToDeg(fix->latitude),
	    Math::radToDeg(fix->longitude),
            fix->nSatellites,
	    _bluefinBattOutput->data[0].voltage,
	    _bluefinBattOutput->data[0].minvoltage,
	    _bluefinBattOutput->data[0].state,
	    _bluefinBattOutput->data[1].voltage,
	    _bluefinBattOutput->data[1].minvoltage,
	    _bluefinBattOutput->data[1].state,
	    _bluefinBattOutput->data[2].voltage,
	    _bluefinBattOutput->data[2].minvoltage,
	    _bluefinBattOutput->data[2].state);
    printf("cmd is %s\n", cmd);
    for (i = 0; i < strlen(cmd);i++) checksum += (unsigned char)(cmd[i]);
    swab((char *)&checksum, (char *)&sval, 2);
    checksum = sval;
    sprintf(buf, "AT+SBDWB=%d\xd\xa",strlen(cmd));
    _device->write(buf, strlen(buf));
    try {
    if (nbytes = _device->read(buf, 20, 5000)) {
      if (ptr = strstr(buf, "READY")) {
	_device->write(cmd, strlen(cmd));
	_device->write((char *)&checksum, sizeof(checksum));
      }
    }
    if (nbytes = _device->read(buf, 20, 5000)) {
	if (strstr(buf, "0")) {
	    printf("Message successfully written to ISU\n");
        } else {
	    printf("ERROR - AT+SBDWB returned %s(%d)\n", buf, nbytes);
        }
    }

    sprintf(cmd, "AT+SBDI\xd\xa");
    _device->write(cmd, strlen(cmd));
    if (nbytes = _device->read(buf, 40, 60000)) {
	if (ptr = strstr(buf,"+SBDI:")) {
	   sscanf(ptr,"+SBDI:%d,%d,%d,%d,%d,%d",
	     &MOstatus, &MOMSN,&MTstatus,&MTMSN,&MTlength,&MTqueued);
           if (MOstatus==1) status = 1;
	   printf("buffer was %s\n", ptr);
        }
    }
    }
    catch(...) {
	printf("caught an exception trying to send SBD\n");
    }
 
    return(status);
}

void IridiumDriver::run()
{
  GpsIF::Fix fix;
  LayeredControlIF::MissionState missionState;
  LayeredControlIF::Name behaviorName;
  double elapsedTime;
  short behaviorIndex;
  unsigned int IridiumGoodCnt;

  memset(&fix, 0, sizeof(GpsIF::Fix));  
  IridiumGoodCnt = 0;
  while (1) {
    _SignalQual = getSignalQual();
    if (_SignalQual > 0) IridiumGoodCnt++;
    if (_gpsIF) _gpsIF->getFix(&fix);
    if (_layeredControlIF)
      _layeredControlIF->state(&elapsedTime, &missionState,
			       behaviorName, &behaviorIndex);
    if (_bluefinBattOutput) {
      _bluefinBattOutput->read();
    }
    if ( (_SignalQual > 0) && (fix.quality != GpsIF::Invalid) ) {
      printf("time to send update\n"); 
      if (sendIridiumSBDUpdate(&fix, elapsedTime, missionState,
			       behaviorName, behaviorIndex)) {
        IridiumGoodCnt = 0;
	sleep(15*60);
      }
      tcflush(_device->getFd(), TCIOFLUSH);
    } else if (_SignalQual > 0 && IridiumGoodCnt > 130) {
      printf("sending an update with invalid GPS\n");
      fix.nSatellites = 0;
      if (sendIridiumSBDUpdate(&fix, elapsedTime, missionState,
		     	       behaviorName, behaviorIndex)) {
	IridiumGoodCnt = 0;
	sleep(10*60);
        }
    }
  }

}