#include <ctype.h>
#include "SerialDevice.h"
#include "adam6017.h"
#include "adam6017Output.h"
#define READ_TIMEOUT 2500

#define ADAM_PERIOD 1000 

adam6017::adam6017(SerialDevice *device) 
  : _device(device), 
    PeriodicTask("adam6017")
{
  _log = new adam6017Log(this, DataLog::BinaryFormat);
  _output = new adam6017Output();
  addPeriodicCallback(ADAM_PERIOD, (CallbackMethod)adam6017::sample);
  _output->data.deviceReady = True;
  _output->write();
}


adam6017::~adam6017()
{
  if (_log)  delete _log;
  if (_output) delete _output;
}

void adam6017::sample(void)
{
  unsigned char cmd[12], reply[100], *ptr;
  unsigned short sval[8];

  cmd[0] = cmd[1] = cmd[2] = cmd[3] = cmd[4] = 0x0; cmd[5] = 6;
  cmd[6] = 0x01; cmd[7] = 0x04; //station id 1, command 4 
  cmd[8] = 0x00; cmd[9] = 0x00; //start at register 4001
  cmd[10] = 0x00; cmd[11] = 0x08; //read all 8 channels
  ::write(_device->getFd(), cmd, 12);

  ::read(_device->getFd(), reply, 6); //throw away command header
  ::read(_device->getFd(), reply, 3); //read in header

  //  printf("station ID is %d\n", reply[0]);
  //  printf("function code is %d\n", reply[1]);
  //  printf("byte count is %d\n", reply[2]);

  //read data from module
  ::read(_device->getFd(), &reply[3], reply[2]);

  //timestampe it
  timespec sampleTime;
  clock_gettime(CLOCK_REALTIME, &sampleTime);
  _output->data.sampleTime.tv_sec = sampleTime.tv_sec;
  _output->data.sampleTime.tv_nsec = sampleTime.tv_nsec;

  //convert to engineering units (volts), stash raw values in output
  for (int i = 0; i <=7; i++) {
      sval[i] = _output->data.voltages[i] = reply[3+i*2]<<8|reply[4+i*2];
      _channel[i] = (sval[i]*(10.0/65535.0)) + -5.0; 
  }

  _log->write();
  _output->write();

} // void Sample(void)