#include <perception_sf/sensor_gps.h>
#include <decition/gnss_coordinate_convert.h>
#include <control/can.h>
#include <QDebug>
#include <QNetworkDatagram>

static double   cast_8_byte_to_double(const uint8_t *b);
static float    cast_4_byte_to_float(const uint8_t *b);
static  int32_t cast_3_byte_to_int32(const uint8_t *b);

void iv::perception::GPSSensor::processSensor()
{

    return;
	//todo  GPS/惯导 设备接口 对接
    /*Initialize udp server, listen on port 3000.*/
    /*int sockfd;
    struct sockaddr_in addr;
    socklen_t addrlen;
    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (sockfd < 0)
    {
       printf("socket failed\n");
       exit(EXIT_FAILURE);
    }
    addrlen = sizeof(struct sockaddr_in);
    bzero(&addr, addrlen);
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = htonl(INADDR_ANY);
    addr.sin_port = htons(3000);
    if (bind(sockfd, (struct sockaddr*)(&addr), addrlen) < 0)
    {
       printf("bind fail\n");
       exit(EXIT_FAILURE);
    }*/
    QUdpSocket *udpsocket;//summer
    udpsocket = new QUdpSocket();
    udpsocket->bind(QHostAddress::Any, 3000);
 //   udpsocket->bind(3000);
 /*   if(!udpsocket->waitForConnected())
    {
        printf("bind fail\n");
        exit(EXIT_FAILURE);
    }*/
    char *Buf = new char[100] ;//summer unsigned char - char
    memset(Buf,0,100);
    unsigned char recvBuf[100] = {0};

	/*UDP initialization done. Next prepare for receiving data.*/

	iv::GPSData data(new iv::GPS_INS);
	int x, y, z;

	//iv::CarStatus currentCarStatus;
	//ODS("\n\n\n------>thread gps: %X\n\n\n\n", GetCurrentThreadId());
    while (!boost::this_thread::interruption_requested()) {
        int rec = 0;
        if(udpsocket->hasPendingDatagrams())
        {
  //          std::cout<<"have data."<<std::endl;
            QNetworkDatagram datagram = udpsocket->receiveDatagram();
            QByteArray ba = datagram.data();
            if(ba.size() != NOUTPUT_PACKET_LENGTH)
            {
                continue;
            }
            else
            {
                rec = NOUTPUT_PACKET_LENGTH;
                memcpy(recvBuf,ba.data(),NOUTPUT_PACKET_LENGTH);
            }
            datagram.clear();
        }
        else
        {
#ifdef Q_OS_LINUX
            usleep(1000);
#endif
#ifdef WIN32
            boost::this_thread::sleep(boost::posix_time::milliseconds(1));
//            Sleep(5);
#endif
            continue;
//            std::cout<<"running."<<std::endl;
 //           std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
/*        if(udpsocket->waitForReadyRead())
            rec = udpsocket->read(Buf,100);
        //recvBuf = (unsigned char*)Buf;
        convertStrToUnChar(Buf,recvBuf);//summer */
        //int rec = recvfrom(sockfd, recvBuf, 100, 0, (struct sockaddr *)(&addr), &addrlen);
		/*Check the receiving data*/
		//There must be 72 bytes in one package.
		if (rec != NOUTPUT_PACKET_LENGTH) {
			printf("ERR: rec must be 72 bytes\n");
			continue;
		}

		if (recvBuf[PI_SYNC] != NCOM_SYNC) {
			printf("ERR: head always be 0xE7\n");
			continue;
		}

        data->ins_status = recvBuf[PI_INS_NAV_MODE];
        if(data->ins_status == 0x0B)continue;

        //31.1150882 121.2211320 356.9
        //31.1150844 121.2210996 356.9 VV7_3

		/*  Start decoding
		Details in ncomman.pdf
		Byte		| 0	   | 1-20	| 21         | 22      | 23-60  | 61      | 62       | 63-70  | 71
		Description	| Sync | BatchA | NaviStatus | ChkSum1 | BatchB | ChkSum2 | StatusCh | BatchS | ChkSum3
		*/

		//start decoding -> Batch A
        //Time is transmitted as milliseconds into the current GPS minute. The range is 0–59, 999 ms.

		/*
		Acceleration x is the host object’s acceleration in the x-direction
		(i.e.after the IMU to host attitude matrix has been applied).
		It is a signed word in units of 1 × 10^(−4)  m / (s^2)
		*/
		x = cast_3_byte_to_int32(recvBuf + PI_ACCEL_X);
		y = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Y);
		z = cast_3_byte_to_int32(recvBuf + PI_ACCEL_Z);
		if (x != INV_INT_24) data->accel_x = x * ACC2MPS2;
		if (y != INV_INT_24) data->accel_y = y * ACC2MPS2;
		if (z != INV_INT_24) data->accel_z = z * ACC2MPS2;

		/*
		Angular rate x is the host object’s angular rate about its x-axis
		(i.e. after the IMU to host attitude matrix has been applied).
		It is a signed word in units of 1 × 10−5 radians/s
		*/
		x = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_X);
		y = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Y);
		z = cast_3_byte_to_int32(recvBuf + PI_ANG_RATE_Z);
		if (x != INV_INT_24) data->ang_rate_x = x * (RATE2RPS * RAD2DEG);
		if (y != INV_INT_24) data->ang_rate_y = y * (RATE2RPS * RAD2DEG);
		if (z != INV_INT_24) data->ang_rate_z = z * (RATE2RPS * RAD2DEG);

		/*
		The navigation status byte value should be 0–7, 10 or 20–22 to be valid
		for customer use. See page 10. A value of 11 indicates the packet
		follows NCOM structure-B and should be ignored.
		*/
		data->ins_status = recvBuf[PI_INS_NAV_MODE];
		//navi_status refer to imu status 2 3 4.
		//check this out in page 11 table 5.

		/*
		Checksum 1 allows the software to verify the integrity of bytes 1–21.
		The sync byte is ignored. In low-latency applications the inertial
		measurements in Batch A can be used to update a previous solution
		without waiting for the rest of the packet to be received. Contact Oxford
		Technical Solutions for source code to perform this function.
        */
        //31.1150796 121.2211047 355.8

		//start decoding Batch B
		/*
		Position, orientation and velocity output. See Table 6, for a detailed
		description.
		*/
		/*Byte23-30. The Latitude of the INS. It is a double in units of radians.*/
		/*Byte31-38. Longitude of the INS. It is a double in units of radians.*/
		/*Byte39-42. Altitude of the INS. It is a float in units of metres.*/
		/*double latitude;
		double longitude;
		memcpy(&latitude, recvBuf + 23, sizeof(double));
		memcpy(&longitude, recvBuf + 31, sizeof(double));
		latitude = latitude * 180.0 / M_PI;
		longitude = longitude * 180.0 / M_PI;
		printf("lat= %.20lf   lon= %.20lf  | sizeof(float)=%d\n", latitude, longitude, sizeof(float));
		*/
		data->gps_lat = cast_8_byte_to_double(recvBuf + PI_POS_LAT) * RAD2DEG;
		data->gps_lng = cast_8_byte_to_double(recvBuf + PI_POS_LON) * RAD2DEG;
		data->gps_z = cast_4_byte_to_float(recvBuf + PI_POS_ALT);

		/*Byte43-45. Byte46-48. Byte49-51. Velocity in units of 1 × 10^−4 m/s.*/
		x = cast_3_byte_to_int32(recvBuf + PI_VEL_N);
		y = cast_3_byte_to_int32(recvBuf + PI_VEL_E);
		z = cast_3_byte_to_int32(recvBuf + PI_VEL_D);
		if (x != INV_INT_24) data->vel_N = x * VEL2MPS;
		if (y != INV_INT_24) data->vel_E = y * VEL2MPS;
		if (z != INV_INT_24) data->vel_D = z * VEL2MPS;

		/*Byte52-54. Heading in units of 1 × 10−6 radians. Range ±pi.*/
		/*Byte55-57. Pitch in units of 1 × 10−6 radians. Range ±pi/2.*/
		/*Byte58-60. Roll in units of 1 × 10−6 radians. Range ±pi.*/
		x = cast_3_byte_to_int32(recvBuf + PI_ORIEN_H);
		y = cast_3_byte_to_int32(recvBuf + PI_ORIEN_P);
		z = cast_3_byte_to_int32(recvBuf + PI_ORIEN_R);
		if (x != INV_INT_24) data->ins_heading_angle = x * (ANG2RAD * RAD2DEG);
		if (y != INV_INT_24) data->ins_pitch_angle = y * (ANG2RAD * RAD2DEG);
		if (z != INV_INT_24) data->ins_roll_angle = z * (ANG2RAD * RAD2DEG);
#ifdef VV7_2
        data->ins_heading_angle = data->ins_heading_angle+1.9;
#endif
#ifdef VV7_3
        data->ins_heading_angle = data->ins_heading_angle-1.0;
#endif
        if(data->ins_heading_angle>360)data->ins_heading_angle = data->ins_heading_angle-360;
		if (data->ins_heading_angle < 0.0)
            data->ins_heading_angle += 360.0;

		if (recvBuf[PI_CHANNEL_INDEX] == PI_CHANNEL_INDEX) {
			data->rtk_status = recvBuf[PI_RTK_STATUS];
			data->gps_satelites_num = recvBuf[PI_SAT_NUM];
		}

        ServiceCarStatus.mRTKStatus = data->rtk_status;

		if (data->ins_status != 4) {
			data->valid = IMU_STATUS_ERR;
		} else if (data->rtk_status != 3 && data->rtk_status != 5 && data->rtk_status != 6) {
			data->valid = RTK_STATUS_ERR;
		} else {
			data->valid = RTK_IMU_OK;
		}

        GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
		
        ////从can中读取当前车速
        //data->speed = ServiceCarStatus.speed;
        if(data->ins_status != 0x0B)data->speed = sqrt(data->vel_N*data->vel_N + data->vel_E * data->vel_E)*3.6;
        else continue;

        if(data->gps_lat<0||data->gps_lat>90){
            continue;
        }

        ServiceCarStatus.speed = data->speed;
 //       qDebug("speed x is %g",ServiceCarStatus.speed);
		if(data->ins_status == 4)
            signal_gps_data->operator()(data);	//传输数据
		ServiceCarStatus.location->gps_lat = data->gps_lat;
		ServiceCarStatus.location->gps_lng = data->gps_lng;
		ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
		ServiceCarStatus.location->gps_x = data->gps_x;
        ServiceCarStatus.location->gps_y = data->gps_y;
        ServiceCarStatus.location->rtk_status = data->rtk_status;
        ServiceCarStatus.location->ins_status = data->ins_status;
 //       std::cout<<"lat = "<<ServiceCarStatus.location->gps_lat<<std::endl;
    }
    //close(sockfd);
    udpsocket->close();
}

static double cast_8_byte_to_double(const uint8_t *b)
{
	union { double x; uint8_t c[8]; } u;
	u.c[0] = b[0];
	u.c[1] = b[1];
	u.c[2] = b[2];
	u.c[3] = b[3];
	u.c[4] = b[4];
	u.c[5] = b[5];
	u.c[6] = b[6];
	u.c[7] = b[7];
	return u.x;
}

static float cast_4_byte_to_float(const uint8_t *b)
{
	union { float x; uint8_t c[4]; } u;
	u.c[0] = b[0];
	u.c[1] = b[1];
	u.c[2] = b[2];
	u.c[3] = b[3];
	return u.x;
}

static int32_t cast_3_byte_to_int32(const uint8_t *b)
{
	union { int32_t x; uint8_t c[4]; } u;
	u.c[1] = b[0];
	u.c[2] = b[1];
	u.c[3] = b[2];
	return u.x >> 8;
}


void iv::perception::GPSSensor::convertStrToUnChar(char* str, unsigned char* UnChar)
{
    int i = strlen(str), j = 0, counter = 0;
    char c[2];
    unsigned int bytes[2];

    for (j = 0; j < i; j += 2)
    {
        if(0 == j % 2)
        {
            c[0] = str[j];
            c[1] = str[j + 1];
            sscanf(c, "%02x" , &bytes[0]);
            UnChar[counter] = bytes[0];
            counter++;
        }
    }
}

void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu)
{
    iv::GPSData data(new iv::GPS_INS);
    data->ins_status = xgpsimu.ins_state();
    data->rtk_status = xgpsimu.rtk_state();

    data->accel_x = xgpsimu.acce_x();
    data->accel_y = xgpsimu.acce_y();
    data->accel_z = xgpsimu.acce_z();
    data->ang_rate_x = xgpsimu.gyro_x();
    data->ang_rate_y = xgpsimu.gyro_y();
    data->ang_rate_z = xgpsimu.gyro_z();

    data->gps_lat = xgpsimu.lat();
    data->gps_lng = xgpsimu.lon();
    data->gps_z = xgpsimu.height();

    data->ins_heading_angle = xgpsimu.heading();
    data->ins_pitch_angle = xgpsimu.pitch();
    data->ins_roll_angle = xgpsimu.roll();

    data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;

    GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);

    ServiceCarStatus.mRTKStatus = data->rtk_status;

    ServiceCarStatus.mfGPSAcc = xgpsimu.acc_calc();

    ServiceCarStatus.speed = data->speed;
//       qDebug("speed x is %g",ServiceCarStatus.speed);
    if(data->ins_status == 4)
    {
        signal_gps_data->operator()(data);	//传输数据
    }

    ServiceCarStatus.location->gps_lat = data->gps_lat;
    ServiceCarStatus.location->gps_lng = data->gps_lng;
    ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
    ServiceCarStatus.location->gps_x = data->gps_x;
    ServiceCarStatus.location->gps_y = data->gps_y;
    ServiceCarStatus.location->rtk_status = data->rtk_status;
    ServiceCarStatus.location->ins_status = data->ins_status;

}