#include <adc_tools/transfer.h>
#include <decition_type.h>
#include <math.h>
#include <iostream>
#include <fstream>
using namespace std;
#define PI (3.1415926535897932384626433832795)



///大地转车体
iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
{
    double x_vehicle, y_vehicle;
    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);

    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;

    x_vehicle = -cos(angle) * distance;
    y_vehicle = sin(angle) * distance;
    return  iv::Point2D(x_vehicle, y_vehicle);
}

///车体转大地
iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
{
    iv::GPS_INS gps_ins;
    double x_vehicle, y_vehicle;
    double theta = atan2(x_path, y_path);

    double distance = sqrt(x_path * x_path + y_path * y_path);
    double angle = (pos.ins_heading_angle / 180 * PI + theta);

    x_vehicle = pos.gps_x + distance * sin(angle);
    y_vehicle = pos.gps_y + distance * cos(angle);
    gps_ins.gps_x = x_vehicle;
    gps_ins.gps_y = y_vehicle;

    return  gps_ins;
}

double iv::decition::GetDistance(iv::Point2D x1, iv::Point2D x2)
{
    return sqrt((x1.x - x2.x) * (x1.x - x2.x) + (x1.y - x2.y) * (x1.y - x2.y));
}

double iv::decition::GetDistance(iv::GPS_INS p1, iv::GPS_INS p2)
{
    return sqrt((p1.gps_x - p2.gps_x) * (p1.gps_x - p2.gps_x) + (p1.gps_y - p2.gps_y) * (p1.gps_y - p2.gps_y));
}




//GPS转大地坐标
void iv::decition::BLH2XYZ(iv::GPS_INS gp)
{
    int nFlag = 2;

    double B = gp.gps_lat;

    double L = gp.gps_lng;



    double N, E, h;
    double L0 = GetL0InDegree(L);//根据经度求中央子午线经度

    double a = 6378245.0;            //地球半径  北京6378245
    double F = 298.257223563;        //地球扁率
    double iPI = 0.0174532925199433; //2pi除以360，用于角度转换

    double f = 1 / F;
    double b = a * (1 - f);
    double ee = (a * a - b * b) / (a * a);
    double e2 = (a * a - b * b) / (b * b);
    double n = (a - b) / (a + b), n2 = (n * n), n3 = (n2 * n), n4 = (n2 * n2), n5 = (n4 * n);
    double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2;
    double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32;
    double gm = 15 * n2 / 16 - 15 * n4 / 32;
    double dt = -35 * n3 / 48 + 105 * n5 / 256;
    double ep = 315 * n4 / 512;

    B = B * iPI;
    L = L * iPI;
    L0 = L0 * iPI;

    double l = L - L0, cl = (cos(B) * l), cl2 = (cl * cl), cl3 = (cl2 * cl), cl4 = (cl2 * cl2), cl5 = (cl4 * cl), cl6 = (cl5 * cl), cl7 = (cl6 * cl), cl8 = (cl4 * cl4);
    double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
    double t = tan(B), t2 = (t * t), t4 = (t2 * t2), t6 = (t4 * t2);
    double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
    double yt = e2 * cos(B) * cos(B);
    N = lB;
    N += t * Nn * cl2 / 2;
    N += t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
    N += t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
    N += t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;

    E = Nn * cl;
    E += Nn * cl3 * (1 - t2 + yt) / 6;
    E += Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
    E += Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;

    E += 500000;
    if (nFlag == 1)
    {
        //UTM投影
        N = 0.9996 * N;
        E = 0.9996 * (E - 500000.0) + 500000.0;//Get y
    }
    if (nFlag == 2)
    {
        //UTM投影
        N = 0.9999 * N;
        E = 0.9999 * (E - 500000.0) + 250000.0;//Get y
    }

    //原
    //pt2d.x = N;
    //pt2d.y = E;
    //
    gp.gps_x = E - 280000;
    gp.gps_y = N - 4325000;

}


double iv::decition::GetL0InDegree(double dLIn)
{
    //3°带求带号及中央子午线经度(d的形式)
    //具体公式请参阅《常用大地坐标系及其变换》朱华统，解放军出版社138页
    double L = dLIn;//d.d
    double L_ddd_Style = L;
    double ZoneNumber = (int)((L_ddd_Style - 1.5) / 3.0) + 1;
    double L0 = ZoneNumber * 3.0;//degree
    return L0;
}