#include <stdint.h>
#include <iostream>
#include <sstream>
#include <string>
#include "socket_stream.h"
#include "pressure_sensor_thread.h"

using namespace std;

const string responseHeader("#01CP=");
const double PSI_perBar 			= 14.5037738007;
const double BAR_perMeterOfSeaWater	= 10.0803;

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

void PressureSensorThread::Configure(uint32_t sampleRate)
{
	stringstream cmdStream;
	string		 sampleRateCmd;
	cmdStream << "*01I=R" << sampleRate << "\r";
	sampleRateCmd = cmdStream.str();
	command("*01S=\r");								// Get the serial #
	m_PressureSensorSocket->send("*01WE=RAM\r");	// Enable sequential parameter writes
	m_PressureSensorSocket->send("*01DU=PSI\r");	// Return data in PSI
	m_PressureSensorSocket->send(sampleRateCmd); 	// Set read rate to 1 sample/sec
	m_PressureSensorSocket->send("*01DS=0\r");		// No deadband
	m_PressureSensorSocket->send("*01MO=X2\r"); 	// No continuous readings on power up
	m_PressureSensorSocket->send("*01NE=OFF\r"); 	// Disable analog output to save power
	m_PressureSensorSocket->send("*01OP=A\r"); 		// Power Up Mode = no continuous readings
	m_PressureSensorSocket->send("*01OP=N\r"); 		// Power Up Mode = no checksum
	m_PressureSensorSocket->send("*01OP=E\r");		// Power Up Mode = Extended form
	m_PressureSensorSocket->send("*01OP=W\r");		// Power Up Mode = enable watchdog
	m_PressureSensorSocket->send("*01WE\r");		// Enable writes to parameter RAM
	m_PressureSensorSocket->send("*01SP=ALL\r");	// Store parameters in parameter RAM
	//command("*01CK\r");							// Check EEPROM
	m_PressureSensorSocket->send("*01WE\r");
	command("*01DU\r");
}

void* PressureSensorThread::Run(void* parameter)
{
	for(;;) {
		collectPressure();
	}
}

void PressureSensorThread::collectPressure(void)
{
	try {
		stringstream responseStream;
		string       response;
		m_PressureSensorSocket->send("*01P2\r");  	// Send Data continuously in ASCII
		for (;;) {
			m_PressureSensorSocket->recv(response);
			processResponse(response);
		}
	}
	catch (socket_error_ex& e) {
		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 PressureSensorThread::processResponse(string& response)
{
	string::size_type i;

	i = response.find(responseHeader);
	if (i != string::npos) {
		response.erase(i, responseHeader.length());
		stringstream responseStream(response);
		cout << "Sensor Response= " << response << endl;
		double gaugePressureIn_PSI;
		responseStream >> gaugePressureIn_PSI;
		cout << "Gauge Pressure =" << gaugePressureIn_PSI << endl;
		double gaugePressureInBar = gaugePressureIn_PSI / PSI_perBar;
		double depthInMetersForSeaWater = gaugePressureInBar * BAR_perMeterOfSeaWater;
		m_Controller.ReportDepth(depthInMetersForSeaWater);
		cout << "Depth= " << depthInMetersForSeaWater << endl;
	} else {
		cout << "Invalid Response from Pressure Sensor" << endl;
	}
}

void PressureSensorThread::command(const char * request)
{
	string response;
	cout << "Request: " << request;
	m_PressureSensorSocket->send(request);
	m_PressureSensorSocket->recv(response);
	cout << "Response: " << response << endl;
}
