// PositioningStubImpl.cpp
#include "../src-gen/v1/commonapi/PositioningStubImpl.hpp"

#include "../../../../gpsd-3.20_dev/gpsdclient.c"



double PositioningStubImpl::getLatitude(){
	return latitude;
}

double PositioningStubImpl::getLongitude(){
	return longitude;
}

double PositioningStubImpl::getAltitude(){
	return altitude;
}

double PositioningStubImpl::getHorizontalSpeed(){
	return horizontalSpeed;
}

double PositioningStubImpl::getVerticalSpeed(){
	return verticalSpeed;
}

double PositioningStubImpl::getHeading(){
	return heading;
}

double PositioningStubImpl::getLatitudeAccuracy(){
	return laterror;
}

double PositioningStubImpl::getLongitudeAccuracy(){
	return longerror;
}

double PositioningStubImpl::getAltitudeAccuracy(){
	return alterror;
}

int PositioningStubImpl::readGpsData() {
#if 1

	if (!gps_waiting(&gpsdata, 500000)) {
		std::cout << "[ERROR] No info to read out !!!" << std::endl;
		return -2;
	}

	*message = '\0';
	std::cout << "[INFO] Latest info available !!!" << std::endl;
	if (gps_read(&gpsdata, message, sizeof(message)) == -1) {
		std::cout << "[ERROR] Failed to read out from socket !!! " << std::endl;
		return -3;
	} else {
		std::cout << "[INFO] Able to read data" << std::endl;
	}

	return 0;

}

void PositioningStubImpl::update_gps_panel(struct gps_data_t *gpsdata, char *message)
/* This gets called once for each new GPS sentence. */
{

    /* Fill in the latitude. */
	if (gpsdata->fix.mode >= MODE_2D) {

		latitude = gpsdata->fix.latitude;
	}

    /* Fill in the longitude. */
	if (gpsdata->fix.mode >= MODE_2D) {

		longitude = gpsdata->fix.longitude;
	}

	 if (gpsdata->fix.mode >= MODE_3D) {
		 altitude = gpsdata->fix.altMSL;
	 }

	 /* Fill in the horizontal speed. */
	 horizontalSpeed = gpsdata->fix.speed;

	 // Fill in the vertical speed
	 verticalSpeed = gpsdata->fix.climb;

	 // Fill in the heading
	 if (gpsdata->fix.mode >= MODE_2D) {
		 heading = gpsdata->fix.track;

	 }

	 // Fill latitude accuracy
	 laterror = gpsdata->fix.epx;

	 // Fill longitude accuracy
	 longerror = gpsdata->fix.epy;

	 // Fill altitude accuracy
	 alterror = gpsdata->fix.epv;
}

PositioningStubImpl::PositioningStubImpl(){

	memset(&gpsdata, 0, sizeof(struct gps_data_t));

	memset(&source, 0, sizeof(struct fixsource_t));

	gpsd_source_spec(NULL, &source);

	/* Open the stream to gpsd. */
	if (gps_open(source.server, source.port, &gpsdata) != 0) {
		std::cout << "[ERROR] GPSD not started !!!!" << std::endl;
		///TODO: throw exception
	} else {
		std::cout << "[INFO] Connected to GPSD !!!" << std::endl;

		gps_stream(&gpsdata, WATCH_ENABLE, source.device);
	}
}

PositioningStubImpl::~PositioningStubImpl(){ }

void PositioningStubImpl::getCoordinates(const std::shared_ptr<CommonAPI::ClientId> _client, getCoordinatesReply_t _reply) {

	if(readGpsData() == 0) {
		if (gpsdata.fix.mode >= MODE_2D ) {
			std::cout << "[INFO] Updated coordinate info available !!!!" << std::endl;
			std::cout << "[INFO] Latitude: " << gpsdata.fix.latitude
					<< ", longitude: " << gpsdata.fix.longitude
					<< ", altitude: " << gpsdata.fix.altMSL << std::endl;

			_reply(gpsdata.fix.latitude, gpsdata.fix.longitude, gpsdata.fix.altMSL);
		}

	}

};


void PositioningStubImpl::getSpeed(const std::shared_ptr<CommonAPI::ClientId> _client, getSpeedReply_t _reply){

	if(readGpsData() == 0) {
		std::cout << "[INFO] Updated speed info available !!!!" << std::endl;
		std::cout << "[INFO] Horizontal Speed: " << gpsdata.fix.speed
				<<", Vertical Speed: " << gpsdata.fix.climb << std::endl;

		_reply(gpsdata.fix.speed, gpsdata.fix.climb);

	}
}


void PositioningStubImpl::getHeading(const std::shared_ptr<CommonAPI::ClientId> _client, getHeadingReply_t _reply){
	if(readGpsData() == 0) {
		// Fill in the heading
		 if (gpsdata.fix.mode >= MODE_2D) {
			 std::cout << "[INFO] Updated heading info available !!!!" << std::endl;
			 std::cout << "[INFO] Heading: " << gpsdata.fix.track << std::endl;

			 _reply(gpsdata.fix.track);

		 }
	}
}


void PositioningStubImpl::getAccuracy(const std::shared_ptr<CommonAPI::ClientId> _client, getAccuracyReply_t _reply){
	if(readGpsData() == 0) {
		// Fill in the accuracy
		std::cout << "[INFO] Updated accuracy info available !!!!" << std::endl;
		std::cout << "[INFO] Latitude Accuracy: " << gpsdata.fix.epx
				<< " Longitude Accuracy: " << gpsdata.fix.epy
				<< " Altitude Accuracy: " << gpsdata.fix.epv << std::endl;

		_reply(gpsdata.fix.epx, gpsdata.fix.epy, gpsdata.fix.epv);

	}
}