#include "common.h"
#include "Window.h"
#include "Client.h"
#include <QtNetwork>
#include <QTcpSocket>
#include <QMutex>
#include <stdlib.h>
#include <iostream>
#include <QImage>
#include <QTimer>
#include "ArHudData.h"
#include "events.h"


#include <time.h>
#include <sys/timeb.h>

using namespace std;

#define USE_CLUSTER2 1


long long GetTime()
{
#if 1
	static struct _timeb mtime;
  _ftime(&mtime);
  return (1000*mtime.time + mtime.millitm);
#else
	struct timeval tp;
	gettimeofday(&tp, NULL);
	long long ms = tp.tv_sec * 1000 + tp.tv_usec / 1000; //get current timestamp in milliseconds
	return ms;
#endif
}

Client::Client(
							 const string hostAdress,
							 const int port):mPort(port), mConfirmCommand(0)
{
	mTargetSpeed = -1.0f;
	mTargetAcceleration = 0.0f;
  
	mHostAdress = hostAdress;
  mSocket = new QTcpSocket(this);

	mButtons = 0;
	
  connect(mSocket, SIGNAL(readyRead()), this, SLOT(ReadData()));
  connect(mSocket, SIGNAL(disconnected()), this, SLOT(Disconnected()));

  //  connect(mSocket, SIGNAL(error(QAbstractSocket::SocketError)),
  //	  this, SLOT(PrintError(QAbstractSocket::SocketError)));
	
	QTimer *timer = new QTimer(this);
	connect(timer, SIGNAL(timeout()), this, SLOT(ConnectHost()));
	timer->start(1000);

	timer = new QTimer(this);
	connect(timer, SIGNAL(timeout()), this, SLOT(UpdateTempomat()));
	timer->start(10);

	
	mMessageBufferIndex = 0;
	mControllerId = 0;
	mProfileId = 0;
	
	cout<<"Client started"<<endl;
	//  QTimer::singleShot(100, this, SLOT(ConnectHost()));
}

void
Client::SetControllerId(int id)
{
	mControllerId = id;
}



void
Client::Disconnected()
{
  cout<<"Disconnected.."<<endl;
  mSocket->abort();
  //  ConnectHost();
}

void
Client::UpdateTempomat()
{
	if (mTargetSpeed > 0.0f) {
		float diff = mTargetSpeed - mSpeed;
		if (diff > 0.0f) {
			float accel = min(1.0f, pow(diff/10.0f, 0.5f));
			SendControls(MAX_VALUE, accel, MAX_VALUE, 0.0f, mButtons);
		} else
			SendControls(MAX_VALUE, 0.0f, MAX_VALUE, 0.0f, mButtons);

	}

}

//1B - Event::EVENT_TYPE - register serializer for event listening, 1B - transformed (converts *ED, to *)
#define SDT_REGISTER_EVENT_LISTENER  14
#define SDT_INPUT_TRACKING  15
//4B - sceneID, x bytes binary message
#define SDT_VEHICLE_SIMULATION_DATA 64
//4B - sceneID, x bytes binary message
#define SDT_TRAFFIC_INFO_DATA 65

#define SDT_AR_HUD_DATA 67

void
Client::ConnectHost()
{
  //  mSocket->bytesAvailable();
  
  if (mSocket->state() == QAbstractSocket::UnconnectedState) {
		cout<<"socket state="<<mSocket->state()<<endl;
		mSocket->abort();
		mConfirmCommand = 0;
		mSocket->connectToHost(mHostAdress.c_str(), mPort);
		if (mSocket->waitForConnected(500)) {
			cout<<"Connected to "<<mHostAdress.c_str()<<"\n"<<flush;
			// sleep for some time
			//			QThread::msleep(10);
#if USE_CLUSTER2
			// on connected send a registration message for simulation data
			//			SDT_REGISTER_EVENT_LISTENER = 39
			// SDT_VEHICLE_SIMULATION_DATA in VRUTSerialization.h = 40
			//			EVT_SCENE_VEHICLE_SIMULATION_DATA = 120
			if (0) {
				QDataStream out(mSocket);
				unsigned char buffer[3];
				buffer[0] = SDT_REGISTER_EVENT_LISTENER;
				buffer[1] = 120;
				//buffer[1] = 131;
				buffer[2] = 0;
				out.writeRawData((char *)buffer, 3);
			} else {
				QDataStream out(mSocket);
				unsigned char buffer[6];
				buffer[0] = SDT_REGISTER_EVENT_LISTENER;
				//				buffer[1] = 120;
				buffer[1] = EVT_SCENE_VEHICLE_SIMULATION_DATA;
				buffer[2] = 0;

				buffer[3] = SDT_REGISTER_EVENT_LISTENER;
				//				buffer[4] = 131;
				buffer[4] = EVT_SCENE_TRAFFIC_INFO_DATA;
				buffer[5] = 0;

				cout<<"EVT_SCENE_VEHICLE_SIMULATION_DATA="<<(int)EVT_SCENE_VEHICLE_SIMULATION_DATA<<endl;
				cout<<"EVT_SCENE_TRAFFIC_INFO_DATA="<<(int)EVT_SCENE_TRAFFIC_INFO_DATA<<endl;
				out.writeRawData((char *)buffer, 6);
				//out.writeRawData((char *)buffer, 3);
			}


#endif
			
			emit Connected();
		} else
			cout<<"Connection to "<<mHostAdress.c_str()<<" failed.\n";
  }
}

void
Client::SendArHud(
									const bool showSpeed,
									const bool showSigns,
									const bool showAcc,
									const bool showLaneAssist,
									const char laneLeft,
									const char laneRight,
									const bool showLaneAssistVehicle,
									const char vehicleDistance,
									const bool showRoadDistanceBars,
									const bool showRoadLeftStrip,
									const bool showRoadRightStrip,
									const bool showRoadVehicleHighlight,
									const char vehicleHighlight,
                  const bool showNavigation,
                  const bool showRoadNavigation,
									const char heightOffset
									)
{
  QDataStream out(mSocket);

	char message = SDT_AR_HUD_DATA; 
	out.writeRawData(&message, 1);

	ArHudData data;
	data.groupMask = ArHudData::GROUP_COMBI;
	if (showSpeed)
		data.statusMask |= ArHudData::SHOW_SPEED;
	if (showSigns)
		data.statusMask |= ArHudData::SHOW_SIGNS;


	data.groupMask |= ArHudData::GROUP_ACC;
	if (showAcc) {
		data.statusMask |= ArHudData::SHOW_ACC;
		data.accValue = 50; // just a test value
	}

	data.groupMask |= ArHudData::GROUP_LANE;
	if (showLaneAssist) {
		data.statusMask |= ArHudData::SHOW_LANE_ASSIST;
		data.laneAssistStripLeft = laneLeft;
		data.laneAssistStripRight = laneRight;
	}
	if (showLaneAssistVehicle) {
		data.statusMask |= ArHudData::SHOW_LANE_ASSIST_VEHICLE;
		data.statusMask |= ArHudData::SHOW_LANE_ASSIST_DISTANCE;
		data.vehicleDistance = vehicleDistance;
	}
	if (showRoadDistanceBars) {
		data.statusMask |= ArHudData::SHOW_ROAD_DISTANCE_BARS;
	}

	data.groupMask |= ArHudData::GROUP_ROAD_STRIPS;
	if (showRoadLeftStrip) {
		data.statusMask |= ArHudData::SHOW_ROAD_LEFT_STRIP;
	}
	if (showRoadRightStrip) {
		data.statusMask |= ArHudData::SHOW_ROAD_RIGHT_STRIP;
	}

	data.groupMask |= ArHudData::GROUP_ROAD_VEHICLE;
	if (showRoadVehicleHighlight) {
		data.statusMask |= ArHudData::SHOW_ROAD_VEHICLE_HIGHLIGHT;
		data.vehicleHighlight = vehicleHighlight;
	}

	data.groupMask |= ArHudData::GROUP_NAVIGATION;
	if (showNavigation) {
    data.statusMask |= ArHudData::SHOW_NAVIGATION;
  }
	if (showRoadNavigation) {
    data.statusMask |= ArHudData::SHOW_ROAD_NAVIGATION;
  }
  
	data.groupMask |= ArHudData::GROUP_POSITION;
	data.hudHeight  = heightOffset;
	
	out.writeRawData((const char *)&data, sizeof(ArHudData));

  cout<<"sizeof ArHudData="<<sizeof(ArHudData)<<endl;
  
}
 
									

void
Client::SendControls(
										 const float wheel,
										 const float gas,
										 const float brake,
										 const float accel,
										 const int buttons
										 )
{
  QDataStream out(mSocket);
	const float multiplier = 543.0f;
#if !USE_CLUSTER2
	char buffer[256];
	//	old format until 18/10/2016
	//	sprintf(buffer,"break=%f accel=%f wheel=%f", b, a, w);

	//	const float multiplier = 1556.0f;
	sprintf(buffer,"tcp=1 wheel=%f gas=%f brake=%f accel=%f",
					wheel*multiplier, gas, brake, accel);
  out.writeRawData(buffer, strlen(buffer));
#else
	char message = SDT_INPUT_TRACKING; //SDT_INPUT_TRACKING
	out.writeRawData(&message, 1);
	
	// controller id
	// USE 0 for the default controlled vehicle!
	int id = mControllerId;
	out.writeRawData((char *)&id, 4);

	int type = 1; // DEVICE_FlyStick2
	out.writeRawData((char *)&type, 4);

	float quality = 0; 
	out.writeRawData((char *)&quality, 4);

	float matrix[16]; 
	out.writeRawData((char *)matrix, 16*4);
	
	//	int buttons = 0;
	int b = buttons | ((mProfileId & 7)<<10);
	
	out.writeRawData((char *)&b, 4);
	
	int numControls = 4;
	out.writeRawData((char *)&numControls, 4);

	float w = wheel*multiplier; 
	out.writeRawData((char *)&w, 4);
	out.writeRawData((char *)&gas, 4);
	out.writeRawData((char *)&brake, 4);
	out.writeRawData((char *)&accel, 4);
	int hand = 0;
	out.writeRawData((char *)&hand, 1);
	int fingers = 0;
	out.writeRawData((char *)&fingers, 4);
#endif

	
}

void
Client::ReadData()
{
  // read the data to the buffer
  int bytes = mSocket->bytesAvailable();
  
	QDataStream in(mSocket);
  
  const int N = 256;
  char buff[N+1];

	static long long lastTime = 0, currentTime;
	
	while (bytes >= 1) {
		currentTime = GetTime();
		int time = (float)(currentTime - lastTime);
		lastTime = currentTime;

		char messageType;
		in.readRawData(&messageType, 1);
		switch (messageType) {
		case SDT_VEHICLE_SIMULATION_DATA: {
			//			cout<<(int)messageType<<" "<<bytes<<endl;
			// 1 + 11*4 + 3*12 + 3*16
			
			// read the info from the packet
			int sceneID;
			in.readRawData((char *)&sceneID, sizeof(int)); //4
			
			int vehicleID;
			in.readRawData((char *)&vehicleID, sizeof(int)); //4 
			
			int controllerID;
			in.readRawData((char *)&controllerID, sizeof(int)); //4
			
			float speed;
			in.readRawData((char *)&speed, sizeof(float)); //4
			
			float engineRPM;
			in.readRawData((char *)&engineRPM, sizeof(float)); //4
			
			float engineTorque;
			in.readRawData((char *)&engineTorque, sizeof(float)); //4
			
			int gear;
			in.readRawData((char *)&gear, sizeof(float)); //4
			
			float velocity[3];
			in.readRawData((char *)velocity, 3*sizeof(float)); //12
			
			float position[3];
			in.readRawData((char *)position, 3*sizeof(float)); //12
			
			float orientation[3];
			in.readRawData((char *)orientation, 3*sizeof(float)); //12

			float angularVelocity[3];
			in.readRawData((char *)angularVelocity, 3*sizeof(float)); //12

			float distance;
			in.readRawData((char *)&distance, sizeof(float)); //4
			
			int wheelContactInfo[4];
			in.readRawData((char *)wheelContactInfo, 4*sizeof(int)); //16
			
			int virtualWheelContactInfo[4];
			in.readRawData((char *)virtualWheelContactInfo, 4*sizeof(int)); //16
			
			float wheel;
			in.readRawData((char *)&wheel, sizeof(float)); //4
			float gas;
			in.readRawData((char *)&gas, sizeof(float)); //4
			float brake;
			in.readRawData((char *)&brake, sizeof(float)); //4
			
			int wheelSlipInfo[4];
			in.readRawData((char *)wheelSlipInfo, 4*sizeof(int)); //16
			
			int buttons;
			in.readRawData((char *)&buttons, sizeof(int)); //4

			float virtualWheelPosition[12];
			in.readRawData((char *)virtualWheelPosition, 4*3*sizeof(float)); //48

			float wheelPosition[12];
			in.readRawData((char *)wheelPosition, 4*3*sizeof(float)); //48

			int bytesLeft = mSocket->bytesAvailable();
			
			if (controllerID == 0) {
				mPosition = Vector3(position[0], position[1], position[2]);
				mOrientation = Vector3(orientation[0], orientation[1], orientation[2]);
				mSpeed = speed;
			}
			
			// only show for selected controller id
			if (controllerID == mControllerId) {
				emit Speed(speed);
				
				QString s;
				s.sprintf("bytes=%d, read=%d sceneID=%d vehicleID=%d controllerID=%d speed=%f, position=%f %f %f\nwheel=%f gas=%f brake=%f buttons=%d Engine torque=%f\nTarget Speed=%f Target acceleration=%f\nWheel contacts=%d %d %d %d\n virtContacts=%d %d %d %d\nwheelPos=%f %f %f virtWheelPos=%f %f %f Time=%d",
									bytes,
									bytes - bytesLeft,
									sceneID, vehicleID, controllerID,
									speed,
									position[0], position[1], position[2],
									wheel, gas, brake, buttons,
									engineTorque,
									mTargetSpeed,
									mTargetAcceleration,
									wheelContactInfo[0], wheelContactInfo[1], wheelContactInfo[2], wheelContactInfo[3], 
									virtualWheelContactInfo[0], virtualWheelContactInfo[1], virtualWheelContactInfo[2], virtualWheelContactInfo[3],
									wheelPosition[0], wheelPosition[1], wheelPosition[2], 
									virtualWheelPosition[0], virtualWheelPosition[1], virtualWheelPosition[2], 
									
									time);
				
				emit Message(s);
			}
			break;
		}
	case SDT_TRAFFIC_INFO_DATA: {
		cout<<"TRAFFIC INFO "<<bytes<<endl;
		// TRAFFIC INFO
		{
			int sceneID;
			TrafficInfoData data;
			in.readRawData((char *)&sceneID, sizeof(int)); //4
			in.readRawData((char *)&data, sizeof(TrafficInfoData)); //4
			if (data.controllerID == 0)
				mTrafficInfoData = data;
		}
		break;
	}
		default: {
			cout<<"strange data "<<(int)messageType<<" "<<bytes<<endl;
			bytes--;
			
			// strange data read all of them
			while (bytes > 0 ) {
				int read = std::min(bytes, N);
				in.readRawData(buff, read);
				bytes-=read;
			}
			break;
		}
		}
		bytes = mSocket->bytesAvailable();
	}
}

void
Client::PrintError(QAbstractSocket::SocketError socketError)
{
  switch (socketError) {
  case QAbstractSocket::RemoteHostClosedError:
	break;
  case QAbstractSocket::HostNotFoundError:
	cerr<<
	  "The host was not found. Please check the "
	  "host name and port settings."<<endl;
	break;
  case QAbstractSocket::ConnectionRefusedError:
	cerr<<"The connection was refused by the peer. "
	  "Make sure the fortune server is running, "
	  "and check that the host name and port "
	  "settings are correct."<<endl;
	break;
  default:
	cerr<<
	  "The following error occurred: "<<
	  toString(mSocket->errorString())<<endl;
  }
}

