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

using namespace std;

const uint8_t responseHeader = '\xC2';

// 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)
{
	uint32_t bytesReceived;
	try {
		string       response;

		for (;;) {
			m_AccelerometerSensorSocket->send(SendAccelerationDataOnce);
			bytesReceived = m_AccelerometerSensorSocket->recv(response);
			if (bytesReceived == 31) {
				const uint8_t *buffer = reinterpret_cast<const uint8_t *> (response.c_str());
				processResponse(buffer);
			} else
				cout << "Bogus Response Length" << endl;
		}
	}
	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::processResponse(const uint8_t* buffer)
{
	uint16_t 	calculatedChecksum;
	uint16_t 	receivedChecksum;
	ControlThread::Kinematics k;
	if (buffer[0] == responseHeader) {
		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;
		}
	}
}

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;
}
