#include <stdint.h>
#include <iostream>
#include <sstream>
#include <string>
#include "control_thread.h"
#include "accelerometer_sensor_thread.h"

using namespace std;

const uint8_t accelerationAndAngularRateResponse	= '\xC2';
const uint8_t continuousCommandConfirmation 		= '\xC4';

string SendAccelerationDataContinuously("\xC4\xC1\x29\xC2\r");
// string SendAccelerationDataOnce("\xC2\r");

AccelerometerSensorThread::	AccelerometerSensorThread(ControlThread& controller)
	: m_Controller(controller)
{
	determineEndianness();
}

void* AccelerometerSensorThread::Run(void* parameter)
{
	for(;;) {
		collectAcceleration();
	}
}

void AccelerometerSensorThread::collectAcceleration(void)
{
	try {
		uint8_t		buffer[32];

		m_AccelerometerSensorSocket->send(SendAccelerationDataContinuously);
		for (;;) {
			readN(buffer,1);
			if (buffer[0] == accelerationAndAngularRateResponse) {
				readN(&buffer[1],30);
				processAccelerationAndAngularRateResponse(buffer);
			} else if (buffer[0] == continuousCommandConfirmation) {
				readN(&buffer[1],7);
				processContinuousCommandConfirmation(buffer);
			}
		}
	}
	catch (socket_error_ex& e) {
		cout << "Lost Connection to accelerometer" << endl;
		cout << "Exception: " << e.what() << endl;
		cout << "Err=" << e.whaterr() << endl;
		cout << "Txt=" << e.whatsystext() << endl;
	}
	catch (socket_error& e) {
		cout << "Exception: " << e.what() << endl;
	}
}

void AccelerometerSensorThread::processAccelerationAndAngularRateResponse(const uint8_t* buffer)
{
	uint16_t 	calculatedChecksum;
	uint16_t 	receivedChecksum;
	ControlThread::Kinematics k;

	calculatedChecksum = checksum(buffer,28);
	receivedChecksum = extractReceivedChecksum(const_cast<uint8_t*>(&buffer[29]));
	if (calculatedChecksum == receivedChecksum) {
		k.x_AccelerationInG 		= extractFloat(&buffer[1]);
		k.y_AccelerationInG			= extractFloat(&buffer[5]);
		k.z_AccelerationInG			= extractFloat(&buffer[9]);
		k.x_AngularRateInRadSec		= extractFloat(&buffer[13]);
		k.y_AngularRateInRadSec		= extractFloat(&buffer[17]);
		k.z_AngularRateInRadSec		= extractFloat(&buffer[21]);
		m_Controller.ReportKinematics(k);
	} else {
		cout << "Bad Checksum Received" << endl;
	}
}

void AccelerometerSensorThread::processContinuousCommandConfirmation(const uint8_t *buffer)
{
}

uint16_t AccelerometerSensorThread::checksum(const uint8_t *buffer, uint32_t length)
{
	uint16_t checksum = 0;
	for (uint32_t i = 0; i <= length; i++)
		checksum += *buffer++;
	return checksum;
}

void AccelerometerSensorThread::Connect(string& ipAddress, uint16_t portNumber)
{
	m_AccelerometerSensorSocket = new tcp_client_socket(endpoint(ipAddress, portNumber));
}

void AccelerometerSensorThread::determineEndianness(void)
{
	uint16_t word = 0x0001;
	char *byte = (char *) &word;
	m_CPU_ByteOrder = (byte[0]) ? LittleEndian : BigEndian;
}

float AccelerometerSensorThread::extractFloat(const uint8_t *bytes)
{
	float f = 0;
	if (m_CPU_ByteOrder == LittleEndian) {
		((uint8_t *) (&f))[0] = bytes[3];
		((uint8_t *) (&f))[1] = bytes[2];
		((uint8_t *) (&f))[2] = bytes[1];
		((uint8_t *) (&f))[3] = bytes[0];
	} else {
		((uint8_t *) (&f))[0] = bytes[0];
		((uint8_t *) (&f))[1] = bytes[1];
		((uint8_t *) (&f))[2] = bytes[2];
		((uint8_t *) (&f))[3] = bytes[3];
	}
	return f;
}

uint16_t AccelerometerSensorThread::extractReceivedChecksum(uint8_t* buffer)
{
	uint16_t receivedChecksum = 0;
	if (m_CPU_ByteOrder == LittleEndian) {
		receivedChecksum = buffer[0] << 8 | buffer[1];
	} else {
		receivedChecksum = buffer[1] << 8 | buffer[0];
	}
	return receivedChecksum;
}

void AccelerometerSensorThread::readN(void *buffer, uint16_t length)
{
	uint32_t bytesReceived;
	char *bufptr = reinterpret_cast<char*>(buffer);

	do {
		bytesReceived = m_AccelerometerSensorSocket->recv(bufptr, length);
		bufptr += bytesReceived;
		length -= bytesReceived;
	} while (length > 0);
}

