#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/time.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <cstring>
#include <cerrno>
#include <cstdlib>
#include "labjack.h"

const char	feedbackCommand[3]		= { 0xF8, 0x0E, 0x00 };
const char	feedbackResponse[3]	= { 0xF8, 0x1D, 0x00 };

LabjackException::LabjackException(const string &message, bool inclSysMsg) throw()
	: userMessage(message)
{
	if (inclSysMsg) {
		userMessage.append(": ");
		userMessage.append(strerror(errno));
	}
}

LabjackException::~LabjackException() throw()
{
}

const char *LabjackException::what() const throw()
{
	return userMessage.c_str();
}

#ifdef __NO_LABJACK__
Labjack::Labjack(const string &LabjackIPAddress)
#else
Labjack::Labjack(const string &LabjackIPAddress) : m_LabjackSocket(endpoint(LabjackIPAddress, LabjackTCP_port))
#endif
{
}

Labjack::~Labjack(void)
{
}

void Labjack::Update(double		winchDAC_voltage,
					 double		levelWindDAC_voltage,
					 double& 	motorTension,
					 double& 	flangeTension,
					 double&	busVoltage24,
					 bool&		isEndOfTravelLeftSwitchClosed,
					 bool&		isEndOfTravelRightSwitchClosed,
					 bool&		isCableLeftSwitchClosed,
					 bool&		isCableRightSwitchClosed)
	throw(LabjackException)
{
	uint32_t	bytesReceived;
	uint16_t	checksumTotal;
	uint16_t	fixedPointWinchVoltage;
	uint16_t	fixedPointLevelWindVoltage;
	uint16_t	fixedPointMotorTension;
	uint16_t	fixedPointFlangeTension;
	uint16_t	fixedPointBusVoltage24;

#ifdef __NO_LABJACK__
	return;
#endif

	// Convert DAC voltages to fixed point, using calibration data
	fixedPointWinchVoltage 		= analogToCalibratedBinaryVoltage(0, winchDAC_voltage);
	fixedPointLevelWindVoltage	= analogToCalibratedBinaryVoltage(1, levelWindDAC_voltage);

	//setting fixed point DAC voltages into the request message
	m_SendBuffer.dac_0_lsb = (uint8_t)( fixedPointWinchVoltage & (0x00FF) );
	m_SendBuffer.dac_0_msb = (uint8_t)( fixedPointWinchVoltage >> 8 );
	m_SendBuffer.dac_0_update	= 1;
	m_SendBuffer.dac_0_enabled	= 1;

	m_SendBuffer.dac_1_lsb = (uint8_t)( fixedPointLevelWindVoltage & (0x00FF) );
	m_SendBuffer.dac_1_msb = (uint8_t)( fixedPointLevelWindVoltage >> 8 );
	m_SendBuffer.dac_1_update	= 1;
	m_SendBuffer.dac_1_enabled	= 1;

	extendedChecksum((uint8_t*) &m_SendBuffer.checksum8, sizeof(m_SendBuffer));

	m_LabjackSocket.send(&m_SendBuffer, sizeof(m_SendBuffer));

	bytesReceived = m_LabjackSocket.recv(&m_ReceiveBuffer, sizeof(m_ReceiveBuffer));
	if (bytesReceived != sizeof(m_ReceiveBuffer))
		throw LabjackException("Insufficient feedback data received.");

	if (strncmp((char*) &m_ReceiveBuffer.command,
		(const char*) &feedbackResponse,
		sizeof(feedbackResponse)) != 0)
		throw LabjackException("Incorrect feedback response received.");

	checksumTotal = extendedChecksum16((uint8_t*) &m_ReceiveBuffer, 64);

	if ((uint8_t)((checksumTotal >> 8) & 0xff) != m_ReceiveBuffer.checksum16_msb)
		throw LabjackException("Bad checksum on received feedback response.");

	if ((uint8_t)(checksumTotal & 0xff) != m_ReceiveBuffer.checksum16_lsb)
		throw LabjackException("Bad checksum on received feedback response.");

	if (extendedChecksum8((uint8_t*) &m_ReceiveBuffer) != m_ReceiveBuffer.checksum8)
		throw LabjackException("Bad checksum on received feedback response.");

	isEndOfTravelLeftSwitchClosed	=  (m_ReceiveBuffer.fio_state & 0x01);
	isEndOfTravelRightSwitchClosed	= ((m_ReceiveBuffer.fio_state >> 1) & 0x01);
	isCableLeftSwitchClosed			= ((m_ReceiveBuffer.fio_state >> 2) & 0x01);
	isCableRightSwitchClosed		= ((m_ReceiveBuffer.fio_state >> 3) & 0x01);

	fixedPointMotorTension	= (m_ReceiveBuffer.ain_0_msb << 8) | m_ReceiveBuffer.ain_0_lsb;
	fixedPointFlangeTension	= (m_ReceiveBuffer.ain_1_msb << 8) | m_ReceiveBuffer.ain_1_lsb;
	motorTension			= binaryToCalibratedAnalogVoltage(m_SendBuffer.ain_0_bipolar_gain, fixedPointMotorTension);
	flangeTension			= binaryToCalibratedAnalogVoltage(m_SendBuffer.ain_1_bipolar_gain, fixedPointFlangeTension);
    busVoltage24			== binaryToCalibratedAnalogVoltage(m_SendBuffer.ain_2_bipolar_gain, fixedPointBusVoltage24);
}

void Labjack::ConnectToLabjack(void)
{
#ifdef __NO_LABJACK__
	return;
#endif

	// Configures Labjack to set DAC0, DAC1, read FIO0-FIO3 and AI0 through AI1
	// Clear m_SendBuffer, giving most of our desired configuration
	//    RTFDS, if you doubt this!
	memset((void*) &m_SendBuffer,0, sizeof(m_SendBuffer));
	// Stick the "feedback" command in the message
	strncpy((char *) &m_SendBuffer.command, &feedbackCommand[0], sizeof(feedbackCommand));
	// Enable data from only AI0 and AI1
	m_SendBuffer.ain_mask_lsb			= 0x07;
	// The Labjack used in the ESP profiler only requires (and supports) 12 bits
	m_SendBuffer.ain_resolution 		= 12;
	// Configure AI0 and AI1 to support -5 to +5 volts
	m_SendBuffer.ain_0_bipolar_gain		= bipolar_gain_of_1;
	m_SendBuffer.ain_1_bipolar_gain		= bipolar_gain_of_1;
	m_SendBuffer.ain_2_bipolar_gain		= unipolar_gain_of_1;
	getCalibrationInfo(m_CalibrationInfo);
}

void Labjack::getCalibrationInfo(LabjackCalibrationInfo& calibrationInfo) throw (LabjackException)
{
	const char	calibrationRequest[3]	= { 0xF8, 0x01, 0x2A };
	const char	calibrationResponse[3]	= { 0xF8, 0x41, 0x2A };

	uint32_t	bytesReceived;
	uint8_t 	requestBuffer[8];
	uint8_t 	responseBuffer[136];

	memset((void*) requestBuffer, 0, sizeof(requestBuffer));
	strncpy((char*) &requestBuffer[1], calibrationRequest, sizeof(calibrationRequest));
	extendedChecksum(requestBuffer, 8);

	// Read Block 0 of Calibration storage
	m_LabjackSocket.send(requestBuffer, sizeof(requestBuffer));

	bytesReceived = m_LabjackSocket.recv(responseBuffer, sizeof(responseBuffer));
	if (bytesReceived != sizeof(responseBuffer))
		throw LabjackException("Insufficient Calibration Data Received from Block 0");

	if (strncmp((char*) &responseBuffer[1], calibrationResponse, sizeof(calibrationResponse)) != 0)
		throw LabjackException("Incorrect Calibration Response Received from Bloack 0");

	//	Convert calibration data to doubles
	//	Block data starts on byte 8 of the buffer
	//	This code sucks...  but it was lifted directly from the Labjack driver
	//	Ugh!!!
	calibrationInfo.unipolarSlope[0] = FPuint8ArrayToFPDouble(&responseBuffer[8], 0);
	calibrationInfo.unipolarOffset[0] = FPuint8ArrayToFPDouble(&responseBuffer[8], 8);
	calibrationInfo.unipolarSlope[1] = FPuint8ArrayToFPDouble(&responseBuffer[8], 16);
	calibrationInfo.unipolarOffset[1] = FPuint8ArrayToFPDouble(&responseBuffer[8], 24);
	calibrationInfo.unipolarSlope[2] = FPuint8ArrayToFPDouble(&responseBuffer[8], 32);
	calibrationInfo.unipolarOffset[2] = FPuint8ArrayToFPDouble(&responseBuffer[8], 40);
	calibrationInfo.unipolarSlope[3] = FPuint8ArrayToFPDouble(&responseBuffer[8], 48);
	calibrationInfo.unipolarOffset[3] = FPuint8ArrayToFPDouble(&responseBuffer[8], 56);

	// Read Block 1 of calibration storage
	requestBuffer[7] = (uint8_t)(0x01);
	extendedChecksum(requestBuffer, 8);

	m_LabjackSocket.send(requestBuffer, sizeof(requestBuffer));

	bytesReceived = m_LabjackSocket.recv(responseBuffer, sizeof(responseBuffer));
	if (bytesReceived != sizeof(responseBuffer))
		throw LabjackException("Insufficient Calibration Data Received from Block 1");

	if (strncmp((char*) &responseBuffer[1], calibrationResponse, sizeof(calibrationResponse)) != 0)
		throw LabjackException("Incorrect Calibration Response Received from Block 1");

	//	Continue convert raw calibration data to doubles
	//	Block data starts on byte 8 of the buffer
	calibrationInfo.bipolarSlope = FPuint8ArrayToFPDouble(&responseBuffer[8], 0);
	calibrationInfo.bipolarOffset = FPuint8ArrayToFPDouble(&responseBuffer[8], 8);

	// Read Block 2 of calibration storage
	requestBuffer[7] = (uint8_t)(0x02);
	extendedChecksum(requestBuffer, 8);

	m_LabjackSocket.send(requestBuffer, sizeof(requestBuffer));

	bytesReceived = m_LabjackSocket.recv(responseBuffer, sizeof(responseBuffer));
	if (bytesReceived != sizeof(responseBuffer))
		throw LabjackException("Insufficient Calibration Data Received from Block 2");

	if (strncmp((char*) &responseBuffer[1], calibrationResponse, sizeof(calibrationResponse)) != 0)
		throw LabjackException("Incorrect Calibration Response Received from Block 2");

	//block data starts on byte 8 of the buffer
	calibrationInfo.DACSlope[0] = FPuint8ArrayToFPDouble(&responseBuffer[8], 0);
	calibrationInfo.DACOffset[0] = FPuint8ArrayToFPDouble(&responseBuffer[8], 8);
	calibrationInfo.DACSlope[1] = FPuint8ArrayToFPDouble(&responseBuffer[8], 16);
	calibrationInfo.DACOffset[1] = FPuint8ArrayToFPDouble(&responseBuffer[8], 24);
	calibrationInfo.tempSlope = FPuint8ArrayToFPDouble(&responseBuffer[8], 32);
	calibrationInfo.tempSlopeLow = FPuint8ArrayToFPDouble(&responseBuffer[8], 48);
	calibrationInfo.calTemp = FPuint8ArrayToFPDouble(&responseBuffer[8], 64);
	calibrationInfo.Vref = FPuint8ArrayToFPDouble(&responseBuffer[8], 72);
	calibrationInfo.VrefDiv2 = FPuint8ArrayToFPDouble(&responseBuffer[8], 88);
	calibrationInfo.VsSlope = FPuint8ArrayToFPDouble(&responseBuffer[8], 96);

	// Read Block 3 of calibration storage
	requestBuffer[7] = (uint8_t)(0x03);
	extendedChecksum(requestBuffer, 8);

	m_LabjackSocket.send(requestBuffer, sizeof(requestBuffer));

	bytesReceived = m_LabjackSocket.recv(responseBuffer, sizeof(responseBuffer));
	if (bytesReceived != sizeof(responseBuffer))
		throw LabjackException("Insufficient Calibration Data Received from Block 3");

	if (strncmp((char*) &responseBuffer[1], calibrationResponse, sizeof(calibrationResponse)) != 0)
		throw LabjackException("Incorrect Calibration Response Received from Block 3");

	//	Continue converting calibration data to doubles
	//	Block data starts on byte 8 of the buffer
	calibrationInfo.hiResUnipolarSlope = FPuint8ArrayToFPDouble(&responseBuffer[8], 0);
	calibrationInfo.hiResUnipolarOffset = FPuint8ArrayToFPDouble(&responseBuffer[8], 8);

	/* reading block 4 from memory */
	requestBuffer[7] = (uint8_t)(0x04);    //Blocknum = 4
	extendedChecksum(requestBuffer, 8);

	m_LabjackSocket.send(requestBuffer, sizeof(requestBuffer));

	bytesReceived = m_LabjackSocket.recv(responseBuffer, sizeof(responseBuffer));
	if (bytesReceived != sizeof(responseBuffer))
		throw LabjackException("Insufficient Calibration Data Received from Block 3");

	if (strncmp((char*) &responseBuffer[1], calibrationResponse, sizeof(calibrationResponse)) != 0)
		throw LabjackException("Incorrect Calibration Response Received from Block 3");

	//	Continue converting calibration data to doubles
	//	Block data starts on byte 8 of the buffer
	calibrationInfo.hiResBipolarSlope = FPuint8ArrayToFPDouble(&responseBuffer[8], 0);
	calibrationInfo.hiResBipolarOffset = FPuint8ArrayToFPDouble(&responseBuffer[8], 8);
	calibrationInfo.prodID = 9;
}

double Labjack::FPuint8ArrayToFPDouble(uint8_t *buffer, uint32_t startIndex)
{
  uint32_t resultDec = 0;
  uint32_t resultWh = 0;
  int i;

  for(i = 0; i < 4; i++)
  {
    resultDec += (uint32_t)buffer[startIndex + i] * pow(2, (i*8));
    resultWh += (uint32_t)buffer[startIndex + i + 4] * pow(2, (i*8));
  }

  return ( (double)((int32_t)resultWh) + (double)(resultDec)/4294967296.0 );
}

double Labjack::binaryToCalibratedAnalogVoltage(uint8_t gain, uint16_t fixedPointVoltage)
	throw (LabjackException)
{
	double slope;
	double offset;

	if (isCalibrationInfoValid() == false)
		throw LabjackException("binaryToCalibratedAnalogVoltage: Configuration data is corrupt.");

	switch( (uint32_t) gain ) {
	  case 0:
		slope = m_CalibrationInfo.unipolarSlope[0];
		offset = m_CalibrationInfo.unipolarOffset[0];
		break;
	  case 1:
		slope = m_CalibrationInfo.unipolarSlope[1];
		offset = m_CalibrationInfo.unipolarOffset[1];
		break;
	  case 2:
		slope = m_CalibrationInfo.unipolarSlope[2];
		offset = m_CalibrationInfo.unipolarOffset[2];
		break;
	  case 3:
		slope = m_CalibrationInfo.unipolarSlope[3];
		offset = m_CalibrationInfo.unipolarOffset[3];
		break;
	  case 8:
		slope = m_CalibrationInfo.bipolarSlope;
		offset = m_CalibrationInfo.bipolarOffset;
		break;
	  default:
		throw LabjackException("binaryToCalibratedAnalogVoltage error: invalid gain.");
	}

	return (slope * fixedPointVoltage) + offset;
}

uint16_t Labjack::analogToCalibratedBinaryVoltage(uint32_t whichDAC,
		double analogVoltage) throw (LabjackException)
{
	double slope;
	double offset;
	double tempBytesVoltage;

	if (!isCalibrationInfoValid())
		return -1;

	switch(whichDAC) {
		case 0:
		  slope = m_CalibrationInfo.DACSlope[0];
		  offset = m_CalibrationInfo.DACOffset[0];
		  break;
		case 1:
		  slope = m_CalibrationInfo.DACSlope[1];
		  offset = m_CalibrationInfo.DACOffset[1];
		  break;
		default:
		  throw LabjackException("Incorrect DAC #, only 0 and 1 can be used");
	}

	tempBytesVoltage = slope * analogVoltage + offset;

	//Checking to make sure bytesVoltage will be a value between 0 and 4095,
	//or that a uint16 overflow does not occur.  A too high analogVoltage
	//(above 5 volts) or too low analogVoltage (below 0 volts) will cause a
	//value not between 0 and 4095.
	if(tempBytesVoltage < 0)
		tempBytesVoltage = 0;

	if(tempBytesVoltage > 4095)
		tempBytesVoltage = 4095;

	return (uint16_t)tempBytesVoltage;
}

bool Labjack::isCalibrationInfoValid(void)
{
  if (m_CalibrationInfo.prodID != 9) {
	  printf("Error: Invalid calibration info.\n");
	  return false;
  } else
	  return true;
}

//	Adds checksum to a data packet for normal command format.
void Labjack::normalChecksum( uint8_t *packet, uint32_t bytesInPacket)
{
	packet[0] = normalChecksum8(packet, bytesInPacket);
}


//	Adds checksum to a data packet for extended command format.
void Labjack::extendedChecksum( uint8_t *packet, uint32_t bytesInPacket)
{
	uint16_t a;

	a = extendedChecksum16(packet, bytesInPacket);
	packet[4] = (uint8_t)(a & 0xff);
	packet[5] = (uint8_t)((a / 256) & 0xff);
	packet[0] = extendedChecksum8(packet);
}

//	Returns the Checksum8 for a normal command data packet
uint8_t Labjack::normalChecksum8( uint8_t *packet, uint32_t bytesInPacket)
{
	uint16_t	i;
	uint16_t 		a;
	uint16_t	bb;

	//Sums bytes 1 to n-1 unsigned to a 2 byte value. Sums quotient and
	//remainder of 256 division.  Again, sums quotient and remainder of
	//256 division.
	for(i = 1, a = 0; i < bytesInPacket; i++)
		a+=(uint16_t)packet[i];

	bb = a / 256;
	a = (a - 256 * bb) + bb;
	bb = a / 256;

	return (uint8_t)((a-256*bb)+bb);
}

//	Returns the Checksum16 for a extended command data packet.
uint16_t Labjack::extendedChecksum16(uint8_t *packet, uint32_t bytesInPacket)
{
	uint32_t	i;
	uint32_t	a;

	a = 0;
	//Sums bytes 6 to n-1 to a unsigned 2 byte value
	for(i = 6; i < bytesInPacket; i++)
	a += (uint16_t)packet[i];

	return a;
}

//	Returns the Checksum8 for a extended command data packet
uint32_t Labjack::extendedChecksum8( uint8_t *packet)
{
	uint32_t	i;
	uint32_t	a;
	uint32_t	bb;

	//Sums bytes 1 to 5. Sums quotient and remainder of 256 division. Again, sums
	//quotient and remainder of 256 division.
	for (i = 1, a = 0; i < 6; i++)
		a+=(uint16_t)packet[i];

	bb = a / 256;
	a = (a - 256 * bb) + bb;
	bb = a / 256;

	return (uint8_t)((a - 256 * bb) + bb);
}
