/*
  smart_battery.c

  Copyright 20-Nov-2002 MBARI
  Written: 20-Nov-2002 Mark Sibenac
  Last mod: 20-Nov-2002 sib - creation

  Interface library to the Bluefin Smart Battery
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#ifdef _QNX
#include <sys/select.h>
#endif

#include "smart_battery.h"
#include "serial.h"
#include "error.h"
#include "util.h"

#define BATT_SERIAL_RETRY (3) /* 3x */

int battSetAddress (int batt_port, int batt_num, int address)
{
  char str[10];
  int ret;
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);
  ASSERT (address >= 0 && address < MAX_BATTS);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02X?8 %02X\xa", batt_num, address);

    DEBUG (2, "Setting battery #%03d (0x%02X) to #%03d (0x%02X)\n", batt_num, 
	   batt_num, address, address);
  
    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battSetAddress: sendStr returned %d\n", ret);
      return ret;
    }
    msleep(2000);
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 9)) < 7) // should get back 7 bytes
    {
      ERROR ("err: battSetAddress(%d, 0x%02x, %d): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, address, ret, str);
      //return ret;
    }
  }

  return (*(str+6) - '1');
}

int battSetState (int batt_port, int batt_num, BattState_t state)
{
  char str[10];
  int ret; 
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);
  ASSERT (state >= BATT_STATE_OFF && state < BATT_STATE_UNKNOWN);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xb%c\xa", batt_num, "fdcb"[state]);

    DEBUG (2, "Setting battery #%03d (0x%02X) in state %s\n", batt_num, batt_num,
	   BattStateStr(state));

    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battSetState: sendStr returned %d\n", ret);
      return ret;
    }

    msleep(2000);
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 9)) < 7) // should get back 7 bytes
    {
      ERROR ("err: battSetState(%d, 0x%02x, %d): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, (int)state, ret, str);
      //return ret;
    }
  }

  return (*(str+6) - '1');
}

int battQuerySummary (int batt_port, int batt_num, BattSummary_t *sum)
{
  char str[80], *pstr;
  int ret;
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xq0\xa", batt_num);

    DEBUG (2, "Getting battery #%03d (0x%02X) summary\n", batt_num, batt_num);

    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battQuerySummary: sendStr returned %d\n", ret);
      return ret;
    }
    msleep(2000);

    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 79)) < 58) // should be at least 58 bytes
    {
      ERROR ("err: battQuerySummary(%d, 0x%02x, ...): recvStr returned %d \"%s\" %2x\n",
	     batt_port, batt_num, ret, str, *str);
      //return ret;
    }
  }

  DEBUG (2, "batt #%02d (0x%02X) Summary = %s", batt_num, batt_num, str+6);
 
  if (strlen (str) < 58) // should be 60 with \r\n
    {
      ERROR ("err: battQuerySummary: recvStr only got %d bytes instead of 60\n",
	     strlen (str));
      return (-1);
    }

  pstr = str + 6; // battery state
  switch (*pstr) {
    case 'f' : sum->state = BATT_STATE_OFF;       break;
    case 'd' : sum->state = BATT_STATE_DISCHARGE; break;
    case 'c' : sum->state = BATT_STATE_CHARGE;    break;
    case 'b' : sum->state = BATT_STATE_BALANCE;   break;
    default  : sum->state = BATT_STATE_UNKNOWN;   break;
  }

  pstr += 1; // battery error state
  switch (*pstr) {
    case '-' : sum->error = BATT_ERROR_CLEAR;                  break;
    case 'V' : sum->error = BATT_ERROR_OVER_VOLTAGE;           break;
    case 'v' : sum->error = BATT_ERROR_UNDER_VOLTAGE;          break;
    case 'I' : sum->error = BATT_ERROR_OVER_CURRENT;           break;
    case 'C' : sum->error = BATT_ERROR_MAX_CELL_OVER_VOLTAGE;  break;
    case 'c' : sum->error = BATT_ERROR_MIN_CELL_UNDER_VOLTAGE; break;
    case 'T' : sum->error = BATT_ERROR_OVER_TEMP;              break;
    case 'W' : sum->error = BATT_ERROR_WATER_LEAK;             break;
    case 'H' : sum->error = BATT_ERROR_HW_PROTECT;             break;
    case 'h' : sum->error = BATT_ERROR_TRANS_HW_PROTECT;       break;
    case 'm' : sum->error = BATT_ERROR_WD_TIMEOUT;             break;
    default  : sum->error = BATT_ERROR_UNKNOWN;                break; 
  }

  pstr += 2; // battery voltage (##.## ) volts
  sum->pack_V = atof (pstr);

  pstr += 6; // battery current (##.## ) Amps
  sum->pack_I = atof (pstr);

  pstr += 6; // battery max temp (##.## ) deg C
  sum->max_temp = atof (pstr);

  pstr += 6; // battery min volt (#.## ) volts
  sum->min_volt = atof (pstr);

  pstr += 5; // battery min volt (#.## ) volts
  sum->max_volt = atof (pstr);

  pstr += 5; // water leak
  sum->water_leak = *pstr - '0';

  pstr += 2; // estimated capacity (-####.## ) WHr
  sum->capacity = atof (pstr);

  pstr += 9; // time (HH:MM:SS )
  sum->time = atoi(pstr)*3600 + atoi(pstr+3)*60 + atoi(pstr+6);

  pstr += 9; // master/slave (m/s)
  sum->mode = (*pstr=='m' ? BATT_MODE_MASTER : *pstr=='s' ? BATT_MODE_SLAVE :
	       BATT_MODE_UNKNOWN);

  return 0;
}

int battQueryCellVolts (int batt_port, int batt_num, BattVolts_t *cells)
{
  char str[60], *pstr;
  int i, ret;
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xq1\xa", batt_num);

    DEBUG (2, "Getting battery #%03d (0x%02X) cell volts\n", batt_num, batt_num);

    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battQueryCellVolts: sendStr returned %d\n", ret);
      return ret;
    }
    msleep(2000);
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 59)) < 46) // should be at least 46 bytes
    {
      ERROR ("err: battQueryCellVolts(%d, 0x%02x, ...): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, ret, str);
      //return ret;
    }
  }

  for (pstr = str+6, i=0; *pstr && i<NUM_CELLS; pstr+=5, ++i)
    {
      double volt = atof(pstr);
      if (volt == 0.0)
	{
	  // break;
	}
      cells->cell_volts[i] = volt; // cell volts
      DEBUG (2, "cell volts[%d] = %f, ", i, volt);
    }

  DEBUG (2, "\n");

  return i;
}

int battQueryTemps (int batt_port, int batt_num, BattTemps_t *temps)
{
  char str[60], *pstr;
  int i, ret;
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xq2\xa", batt_num);

    DEBUG (2, "Getting battery #%03d (0x%02X) temps\n", batt_num, batt_num);

    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battQueryTemps: sendStr returned %d\n", ret);
      return ret;
    }
    msleep(2000); 
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 59)) < 35) // should be at least 35 bytes
    {
      ERROR ("err: battQueryTemps(%d, 0x%02x, ...): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, ret, str);
      //return ret;
    }
  }

  for (pstr = str+6, i=0; *pstr && i<NUM_TEMPS; pstr+=6, ++i)
    {
      double temp = atof(pstr);
      if (temp == 0.0)
	{
	  //break;
	}
      temps->pack_temps[i] = temp; // temperature in degrees C
      DEBUG (2, "temp[%d] = %f, ", i, temp);
    }

  DEBUG (2, "\n");

  return i;
}

int battGetEnergy (int batt_port, int batt_num, double *energy)
{
  char str[20];
  int ret;
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xe0\xa", batt_num);

    DEBUG (2, "Getting battery #%03d (0x%02X) capacity\n", batt_num, batt_num);

    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battGetEnergy: sendStr returned %d\n", ret);
      return ret;
    }

    msleep(2000);
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 19)) < 7)
    {
      ERROR ("err: battGetEnergy(%d, 0x%02x, ...): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, ret, str);
      //return ret;
    }
  }

  *energy = atof(str+6);
  DEBUG (2, "Battery #%03d (0x%02X) capacity is %0.2f WHr\n", batt_num,
	 batt_num, *energy);

  return 0;
}

int battSetEnergy (int batt_port, int batt_num, double energy)
{
  char str[10];
  int ret;
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xe8 %0.2f\xa", batt_num, energy);

    DEBUG (2, "Setting battery #%03d (0x%02X) capacity with %0.2f WHr\n", 
	   batt_num, batt_num, energy);
    
    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battSetEnergy: sendStr returned %d\n", ret);
      return ret;
    }

    msleep(2000);
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 9)) < 7)
    {
      ERROR ("err: battSetEnergy(%d, 0x%02x, ...): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, ret, str);
      //return ret;
    }
  }

  return (*(str+6) - '0');
}

int battSetStreaming (int batt_port, int batt_num, int mode)
{
  char str[10];
  int ret;
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);
  ASSERT (mode == 0 || mode == 1);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xs%d\xa", batt_num, mode);

    DEBUG (2, "Setting battery #%03d (0x%02X) as MASTER\n", batt_num, batt_num);

    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battSetStreaming: sendStr returned %d\n", ret);
      return ret;
    }

    msleep(2000);
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 9)) < 7)
    {
      ERROR ("err: battSetStreaming(%d, 0x%02x, ...): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, ret, str);
      //return ret;
    }
  }

  return (*(str+6) - '0');
}

int battSetMaster (int batt_port, int batt_num, int mode)
{
  char str[10];
  int ret;
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);
  ASSERT (mode == BATT_MODE_MASTER || mode == BATT_MODE_SLAVE);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xm%d\xa", batt_num, mode);

    DEBUG (2, "Setting battery #%03d (0x%02X) as %s\n", batt_num, batt_num,
	   BattModeStr(mode));

    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battSetMaster: sendStr returned %d\n", ret);
      return ret;
    }

    msleep(2000);
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 9)) < 7)
    {
      ERROR ("err: battSetMaster(%d, 0x%02x, ...): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, ret, str);
      //return ret;
    }
  }

  return (*(str+6) - '0');
}

int battGetMaster (int batt_port, int batt_num, int *mode)
{
  char str[10];
  int ret;
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xm2\xa", batt_num);

    DEBUG (2, "Getting battery master info on battery#%03d (0x%02X)\n", 
	   batt_num, batt_num);

    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battGetMaster: sendStr returned %d\n", ret);
      return ret;
    }

    msleep(2000);
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 9)) < 7)
    {
      ERROR ("err: battGetMaster(%d, 0x%02x, ...): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, ret, str);
      //return ret;
    }
  }

  *mode = *(str+6) - '0';
  DEBUG (2, "Battery #%03d (0x%02X) Mode = %d\n", batt_num, batt_num, *mode);

  return *mode;
}

int battSetWatchdog (int batt_port, int batt_num, int mode)
{
  char str[10];
  int ret;
  int retry;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);
  ASSERT (mode == BATT_WD_ENABLE || mode == BATT_WD_DISABLE);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xw%c\xa", batt_num, "fe"[mode]);

    DEBUG (2, "%s battery #%03d (0x%02X) watchdog\n", 
	   (mode ? "Enabling" : "Disabling"), batt_num, batt_num);

    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battSetWatchdog: sendStr returned %d\n", ret);
      return ret;
    }

    msleep(2000);
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 9)) < 7)
    {
      ERROR ("err: battSetWatchdog(%d, 0x%02x, ...): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, ret, str);
      //return ret;
    }
  }

  return (*(str+6) - '0');
}

int battGetVersion (int batt_port, int batt_num, char *ver, int ver_len)
{
  char str[50];
  int ret;
  int retry;

  ver[0] = 0; // nullify just in case it does not succeed

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);

  for (ret=-1,retry=0; retry<BATT_SERIAL_RETRY && ret<=0; ++retry)
  {
    sprintf(str, "#%02Xz0\xa", batt_num);

    DEBUG (2, "Getting battery info on battery#%03d (0x%02X)\n", batt_num, 
	   batt_num);

    flushRecv (batt_port);

    if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battGetVersion: sendStr returned %d\n", ret);
      return ret;
    }

    msleep(2000);
    str[0] = 0;
    if ((ret = recvStr (batt_port, str, 49)) < 34)
    {
      ERROR ("err: battGetVersion(%d, 0x%02x, ...): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, ret, str);
      //return ret;
    }
  }

  strncpy (ver, str+6, ver_len-1);

  return 0;
}

int battCrash (int batt_port, int batt_num)
{
  char str[20];
  int ret;

  ASSERT (batt_num >= 0 && batt_num < MAX_BATTS);

  sprintf(str, "#%02Xp3\xa", batt_num);

  DEBUG (2, "Crashing battery#%03d (0x%02X)\n", batt_num, batt_num);

  flushRecv (batt_port);

  if ((ret = sendStr (batt_port, str)) < 0)
    {
      ERROR ("err: battCrash: sendStr returned %d\n", ret);
      return ret;
    }

  msleep(2000);
  if ((ret = recvStr (batt_port, str, 49)) < 7)
    {
      ERROR ("err: battCrash(%d, 0x%02x, ...): recvStr returned %d \"%s\"\n",
	     batt_port, batt_num, ret, str);
      return ret;
    }

  if ((ret < 7) || (*(str+6) != '1'))
    {
      ERROR ("err: battCrash received %d chars back: %s\n", ret, str);
      return -1;
    }

  return 0;
}

/* sm_batts must be at least MAX_BATTS deep */
int battSync (int batt_port, SmartBattery_t *sm_batts)
{
  int ret, i;
  char str[800], *pstr;
  struct timeval tv;
  fd_set readFds;

  for (i=0; i<MAX_BATTS; ++i)
    {
      sm_batts[i].batt_alive = 0;
    }
  
  flushRecv (batt_port);

  if ((ret = sendStr (batt_port, "!?\r\n")) < 0)
    {
      ERROR ("err: battSync: sendStr returned %d\n", ret);
      return ret;
    }

  msleep(2048); // wait twice as long as the theoretical 1024ms

  *str = 0;

  tv.tv_sec = 1; // set timeout to 1 second
  tv.tv_usec = 0;
/*      
  FD_ZERO (&readFds);
  FD_SET (batt_port, &readFds);
      
  if ((ret = select (batt_port+1, &readFds, 0, 0, &tv)) < 0)
    { 
      ERROR ("err: battSync select failed %d: %s\n", ret, strerror(errno));
      return -1;
    }
  else if (ret == 0) // timeout
    {
      ERROR ("err: battSync select failed with timeout.\n"
	     "Are you sure there is a battery connected???\n");
      return -1;
    }
  // else we got something
*/

  if ((ret = read (batt_port, str, 799)) < 0)
    {
      ERROR ("err: battSync: recvStr returned %d\n", ret);
      return ret;
    }

  for (pstr = str, i=0; *pstr && i<MAX_BATTS; pstr+=3, ++i)
    {
      int batt_num = ahextoi(pstr);
      if (!batt_num)
	break;
      sm_batts[batt_num].batt_alive=1; // set flag
      sm_batts[batt_num].address=batt_num;
      DEBUG (1, "found battery dec=%03d hex=%02x\n", batt_num, batt_num);
    }

  return i;  
}