#include <QCoreApplication>

#include "modulecomm.h"
#include "xmlparam.h"
#include "odom.pb.h"
#include "gpsimu.pb.h"

#include <iostream>
#include <memory>

#include "math/gnss_coordinate_convert.h"

#include <cmath>

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}


Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
    // Abbreviations for the various angular functions
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);

    Quaternion q;
    q.w = cy * cp * cr + sy * sp * sr;
    q.x = cy * cp * sr - sy * sp * cr;
    q.y = sy * cp * sr + cy * sp * cr;
    q.z = sy * cp * cr - cy * sp * sr;

    return q;
}

//原文链接：https://blog.csdn.net/xiaoma_bk/article/details/79082629



static double glon0,glat0,gheight0,ghead0;
static void * gpa;


void ListenODOM(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)&index;
    (void)dt;
    (void)strmemname;

    iv::ros::odom xodom;
    if(xodom.ParseFromArray(strdata,nSize) == false)
    {
        std::cout<<" ListenODOM Fail. "<<std::endl;
        return;
    }
    std::cout<<" velocity : "<<xodom.twist_linear().x()<<std::endl;

    iv::gps::gpsimu xgpsimu;
    xgpsimu.set_msgtime(xodom.timestamp() );
    xgpsimu.set_speed(xodom.twist_linear().x());


    EulerAngles xAng;
    Quaternion quat;
    quat.x = xodom.pose_orientation().x();
    quat.y = xodom.pose_orientation().y();
    quat.z = xodom.pose_orientation().z();
    quat.w = xodom.pose_orientation().w();
    xAng = ToEulerAngles(quat);


    double x_o,y_o;
    GaussProjCal(glon0,glat0,&x_o,&y_o);
    double lon,lat;
    double hdg_o = (90 - ghead0)*M_PI/180.0;
    double rel_x = xodom.pose_position().x() * cos(hdg_o) - xodom.pose_position().y() * sin(hdg_o);
    double rel_y = xodom.pose_position().x() * sin(hdg_o) + xodom.pose_position().y() * cos(hdg_o);
    GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
    double hdg_c = hdg_o + xAng.yaw;

    hdg_c = M_PI/2.0 - hdg_c;
    double heading = hdg_c * 180.0/M_PI;
    while(heading < 0)heading = heading + 360;
    while(heading >360)heading = heading - 360;

    xgpsimu.set_lat(lat);
    xgpsimu.set_lon(lon);
    xgpsimu.set_height(xodom.pose_position().z() + gheight0);
    xgpsimu.set_heading(heading);
    xgpsimu.set_pitch(xAng.pitch);
    xgpsimu.set_roll(xAng.roll);

    double vx,vy;
    vx = fabs(xodom.twist_linear().x() *cos(xAng.yaw)
            + xodom.twist_linear().y() * sin(xAng.yaw));
    vy = fabs(xodom.twist_linear().x() *sin(xAng.yaw)
            + xodom.twist_linear().y() * cos(xAng.yaw));
    double ve = vx * cos(hdg_o) - vy * sin(hdg_o);
    double vn = vx * sin(hdg_o) + vy * cos(hdg_o);
    double vd = xodom.twist_linear().z() * (-1);
    xgpsimu.set_ve(ve);
    xgpsimu.set_vn(vn);
    xgpsimu.set_vd(vd);

    xgpsimu.set_ins_state(4);
    xgpsimu.set_rtk_state(6);
    xgpsimu.set_satnum1(1);
    xgpsimu.set_satnum2(1);

    int nbytesize = xgpsimu.ByteSize();
    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nbytesize]);
    if(xgpsimu.SerializeToArray(str_ptr.get(),nbytesize))
    {
        iv::modulecomm::ModuleSendMsg(gpa,str_ptr.get(),nbytesize);
    }
    else
    {
        std::cout<<" ListenODOM Serizlize Error."<<std::endl;
    }


}

void GetParam(std::string  strpath)
{
    iv::xmlparam::Xmlparam xp(strpath);
    glon0 = xp.GetParam("lon0",117.3548316);
    glat0 = xp.GetParam("lat0",39.0677445);
    ghead0 = 360.0;
    gheight0 = 0.0;
}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    GetParam("./driver_odomtogpsimu");

    gpa = iv::modulecomm::RegisterSend("hcp2_gpsimu",10000,1);
    iv::modulecomm::RegisterRecv("odom_ros",ListenODOM);


    return a.exec();
}