#include "Wgs84.h"

#include "Gaussian.h"

namespace jf {

double GetN(const double& radB) {
    double cosB = cos(radB);
    double V = sqrt(1.0 + ep2 * cosB * cosB);
    double N = c / V;
    return N;
}

double GetX(const double& radB) {
    double sinB = sin(radB);
    double cosB = cos(radB);
    double al = a0 * radB;
    double sc = sinB * cosB;
    double ss = sinB * sinB;
    double X = al - sc * ((a2 - a4 + a6) + (2.0 * a4 - a6 * 16.0 / 3.0) * ss + a6 * 16.0 * ss * ss / 3.0);
    return X;
}
double GetLatByX(const double& X) {
    double Bfi0 = X / a0;
    double Bfi1 = 0.0;
    bool num = true;
    double minAngle = PI * 1e-9;
    double menus_minAngle = 0 - minAngle;
    double sinB, sinB2, cosB, FBfi, deltaB;
    while (num == true) {
        num = false;
        sinB = sin(Bfi0);
        sinB2 = sinB * sinB;
        cosB = cos(Bfi0);
        FBfi = 0.0 - sinB * cosB * ((a2 - a4 + a6) + sinB2 * (2.0 * a4 - 16.0 * a6 / 3.0) + sinB2 * sinB2 * a6 * 16.0 / 3.0);
        Bfi1 = (X - FBfi) / a0;

        deltaB = Bfi1 - Bfi0;
        if (deltaB < menus_minAngle || deltaB > minAngle) {
            num = true;
            Bfi0 = Bfi1;
        }
    }
    return Bfi1;
}
double GetV(const double& lat) {
    double cosB = cos(lat);
    double V = sqrt(1 + ep2 * cosB * cosB);
    return V;
}
/*
Array BLH2ECEF(const Array& ptBLH){
    double cosB = cos( Degree2Rad( ptBLH[0] ) );
    double sinB = sin( Degree2Rad( ptBLH[0] ) );
    double cosL = cos( Degree2Rad( ptBLH[1] ) );
    double sinL = sin( Degree2Rad( ptBLH[1] ) );

    double N = GetN( Degree2Rad( ptBLH[0] ) );

    double X = (N + ptBLH[2]) * cosB * cosL;
    double Y = (N + ptBLH[2]) * cosB * sinL;
    double Z = (N * (1 - e2) + ptBLH[2]) * sinB;
    return Array({X,Y,Z});
}
Array ECEF2BLH(const Array& ptXYZ){
    double  L_radian = atan(ptXYZ[1] / ptXYZ[0]);
    if(L_radian < 0){
        L_radian += PI;
    }
    double L = Rad2Degree(L_radian);
    double sqrtXY = sqrt(ptXYZ[0] * ptXYZ[0] + ptXYZ[1] * ptXYZ[1]);
    double t0 = ptXYZ[2] / sqrtXY;
    double p = c * e2 / sqrtXY;
    double k = 1 + ep2;
    double ti = t0;
    double ti1 = 0.0;
    int loop = 0;
    while(loop<10000){
        loop += 1;
        ti1 = t0 + p * ti / sqrt(k + ti * ti);
        if(abs(ti1 - ti) < 1e-12)
            break;
        ti=ti1;
    }
    double B_radian = atan(ti1);
    double N = GetN(B_radian);
    double H = sqrtXY / cos(B_radian) - N;
    double B = Rad2Degree(B_radian);
    return Array({B,L,H});
}
matrix GetR_ENU2ECEF(const double& radianL,const double& radianB){
    double sinB = sin(radianB);
    double cosB = cos(radianB);
    double sinL = sin(radianL);
    double cosL = cos(radianL);
    matrix R4 = matrix({{-sinL, -sinB * cosL, cosB * cosL},
                        {cosL, -sinB * sinL, cosB * sinL},
                        {0, cosB, sinB}});
    return R4;
}
double GetAngleDif(double distance){
    double angleRafian = distance / a;
    return Rad2Degree(angleRafian);
}
Array GetAngleDif(Array& distance){
    Array angleRafian = distance / a;
    return Rad2Degree(angleRafian);
}
*/

}  // namespace jf
