/*
 * geomath.cpp
 *
 */


#include "geomath.h"

#include <math.h>

#define WGS84_K_SEMI_MAJOR_AXIS_A 	6378137.0				/*raggio terrestre equatoriale in m*/
#define WGS84_K_SEMI_MINOR_AXIS_B 	6356752.314245			/*raggio terrestre polare in m*/
#define WGS84_K_INVERSE_FLATTENING	298.257223563
#define WGS84_K_FLATTENING 			0.00335281066474748072	/* 1/f -> 1/WGS84_K_INVERSE_FLATTENING*/

#define WGS84_K_R_METERS			WGS84_K_SEMI_MINOR_AXIS_B
#define WGS84_K_F_METERS			WGS84_K_SEMI_MINOR_AXIS_B

#define WGS84_K_ECCENTRITIC			0.08181919084296430236	//=SQRT(1 - b^2 / a^2)
#define WGS84_K_ECCENTRITIC2		0.08209443795004196372	//=SQRT(a^2/b^2 - 1)


#define WGS84_K_E2					(WGS84_K_ECCENTRITIC*WGS84_K_ECCENTRITIC)

/*

PRO calc_tgt_lat_lon, $ ;Input
              az_tgt_loc_geo, $
              Range, $
              Lat_own , $
              Lon_own , $
              z_own_m , $
              el_tgt_loc_geo,$ ;elevation tgt in local geodetic. Nota:  uguale a quella in nav
              ; output
              Lat_tgt,$
              Lon_tgt

;--Opzioni di compilazione--
COMPILE_OPT STRICTARR

COMMON cmp_signal_data_rec_c , cmp_signal_data_rec



    ; Definizione delle costanti

a = double(6378137)     ;raggio terrestre equatoriale in m
b = double(6356752.3142)  ;raggio terrestre polare in m
e = SQRT(1 - b^2 / a^2)  ;eccentricit
e1 = SQRT(a^2/b^2 - 1)   ;seconda eccentricit




;componenti del vettore antenna-target nel sist di rif.local geodetic)
LOS_nav_x = Range*COS(el_tgt_loc_geo)*COS(az_tgt_loc_geo)
LOS_nav_y = Range*COS(el_tgt_loc_geo)*SIN(az_tgt_loc_geo)
LOS_nav_z = Range*SIN(el_tgt_loc_geo)


;componenti del vettore antenna-target nel sist di rif. NED
LOS_NED_x = LOS_nav_x
LOS_NED_y = - LOS_nav_y
LOS_NED_z = - LOS_nav_z


;calcolo delle componenti del vettore antenna target in ECEF.
;Per calcolarle si costruisce la matrice di rotazione
mat_ned_ecef= [ [COS(Lat_own),                           0,                   -SIN(Lat_own)], $
                [SIN(Lon_own)*SIN(Lat_own),        -COS(Lon_own),       SIN(Lon_own)*COS(Lat_own)], $
                [-COS(Lon_own)*SIN(Lat_own),       -SIN(Lon_own),      -COS(Lon_own)*COS(Lat_own)]  ]


LOS_ECEF= mmul(mat_ned_ecef,[[LOS_NED_x],[LOS_NED_y],[LOS_NED_z]])



N = a / SQRT(1 - e^2 * SIN(Lat_own)^2)
cc= 1 - e^2
ac_pos_ECEF = [   (N*cc+z_own_m)*SIN(Lat_own), $
                        -(N+z_own_m)*COS(Lat_own)*SIN(Lon_own), $
                        (N+z_own_m)*COS(Lat_own)*COS(Lon_own)]


;calcolo del vettore posizione del target in ECEF

tgt_ECEF = LOS_ECEF + ac_pos_ECEF

tgt_ECEF_x = tgt_ECEF[0]
tgt_ECEF_y = tgt_ECEF[1]
tgt_ECEF_z = tgt_ECEF[2]

p=SQRT(tgt_ECEF_z^2 + tgt_ECEF_y^2)    ;con questo ottengo la P
teta=ATAN( tgt_ECEF_x * a , b * p)

Lat_tgt = ATAN( tgt_ECEF_x + e1^2 * b * SIN(teta)^3 , p  - e^2 * a * COS(teta)^3 )
Lon_tgt = ATAN(-tgt_ECEF_y,tgt_ECEF_z)


N = a / SQRT(1 - e^2 * SIN(Lat_tgt)^2)
v=p/COS(Lat_tgt)
h=v-N


RETURN

END
 */

typedef double real_t;

namespace geomath
{
	struct m3x3_t
	{
		real_t m[3][3];
	};

	struct v3_t
	{
		real_t m[3];
	};

	static v3_t mmul3v3(const m3x3_t& x, const v3_t& v)
	{
		v3_t r=
		{{
			x.m[0][0]*v.m[0]+x.m[0][1]*v.m[1]+x.m[0][2]*v.m[2],
			x.m[1][0]*v.m[0]+x.m[1][1]*v.m[1]+x.m[1][2]*v.m[2],
			x.m[2][0]*v.m[0]+x.m[2][1]*v.m[1]+x.m[2][2]*v.m[2],
		}};
		return r;
	}

	static v3_t vadd(const v3_t& v1, const v3_t& v2) //tgt_ECEF = LOS_ECEF + ac_pos_ECEF;
	{
		v3_t r={{
				v1.m[0]+v2.m[0],
				v1.m[1]+v2.m[1],
				v1.m[2]+v2.m[2]}};
		return r;
	}

	static real_t pow2(real_t x) { return x*x;}
	static real_t pow3(real_t x) { return x*x*x;}

	static real_t ATAN(real_t y, real_t x) { return atan2(y, x);}
};

static inline real_t SIN(real_t x) { return sin(x);}
static inline real_t COS(real_t x) { return cos(x);}

static inline real_t SQRT(real_t x) { return sqrt(x);}

void geomath::wgs84_rngaz_to_geo(float& tgt_latitude, float& tgt_longitue, float tgt_rng, float tgt_az, float tgt_el, float ref_lat, float ref_lon)
{
	const real_t a = WGS84_K_SEMI_MAJOR_AXIS_A;	//raggio terrestre equatoriale in m
	const real_t b = WGS84_K_SEMI_MINOR_AXIS_B;	//raggio terrestre polare in m
	const real_t e = WGS84_K_ECCENTRITIC;
	const real_t e2 = e*e;
	const real_t e1 = WGS84_K_ECCENTRITIC2;

	const real_t el_tgt_loc_geo=tgt_el;
	const real_t az_tgt_loc_geo=tgt_az;
	const real_t Range=tgt_rng;

	const real_t LOS_nav_x = Range*COS(el_tgt_loc_geo)*COS(az_tgt_loc_geo);
	const real_t LOS_nav_y = Range*COS(el_tgt_loc_geo)*SIN(az_tgt_loc_geo);
	const real_t LOS_nav_z = Range*SIN(el_tgt_loc_geo);

	const real_t LOS_NED_x = LOS_nav_x;
	const real_t LOS_NED_y = - LOS_nav_y;
	const real_t LOS_NED_z = - LOS_nav_z;

	const real_t Lat_own=ref_lat;
	const real_t Lon_own=ref_lon;

	const real_t z_own_m=0;

	m3x3_t mat_ned_ecef=
			{{
					{COS(Lat_own),                           0,                   -SIN(Lat_own)},
	                {SIN(Lon_own)*SIN(Lat_own),        -COS(Lon_own),       SIN(Lon_own)*COS(Lat_own)},
	                {-COS(Lon_own)*SIN(Lat_own),       -SIN(Lon_own),      -COS(Lon_own)*COS(Lat_own)}
			}};

	v3_t tmp={{LOS_NED_x,LOS_NED_y,LOS_NED_z}};

	v3_t LOS_ECEF=mmul3v3(mat_ned_ecef, tmp);

	const real_t sin_Lat_own=SIN(Lat_own);

	const real_t N = a / SQRT(1 - e2 * (sin_Lat_own*sin_Lat_own));
	const real_t cc= 1 - e2;

	const v3_t ac_pos_ECEF =
	{{
			(N*cc+z_own_m)*SIN(Lat_own),
		     -(N+z_own_m)*COS(Lat_own)*SIN(Lon_own),
		     (N+z_own_m)*COS(Lat_own)*COS(Lon_own)
	}};


	const v3_t tgt_ECEF = vadd(LOS_ECEF, ac_pos_ECEF);

	const real_t tgt_ECEF_x = tgt_ECEF.m[0];
	const real_t tgt_ECEF_y = tgt_ECEF.m[1];
	const real_t tgt_ECEF_z = tgt_ECEF.m[2];

	const real_t p=SQRT(pow2(tgt_ECEF_z) + pow2(tgt_ECEF_y)); //    ;con questo ottengo la P
	const real_t teta=ATAN( tgt_ECEF_x * a , b * p);

	const real_t Lat_tgt = ATAN( tgt_ECEF_x + pow2(e1) * b * pow3(SIN(teta)) , p  - e2 * a * pow3(COS(teta)) );
	const real_t Lon_tgt = ATAN(-tgt_ECEF_y,tgt_ECEF_z);

#if 0
	N = a / SQRT(1 - e2 * pow2(SIN(Lat_tgt)));
	const real_t v=p/COS(Lat_tgt);
	const real_t h=v-N;
#endif

	tgt_latitude=Lat_tgt;
	tgt_longitue=Lon_tgt;
}




void geomath::th_rngaz_to_geo(float& p_phi, float& p_lambda, const float n_phi, const float n_lambda, float th/*true heading*/, float psi/*ptaz*/, float p_rng, float p_az)
{
	const float R_WGS84 = 6378131.6341;
	const float F_WGS84 = 0.0033528;
	//constexpr float R_WGS84 = 6378137.0;
	//constexpr float F_WGS84 = 0.00335281066474748072;
	//constexpr float F_WGS84 = (3.35281068231769422166749745962564e-03)*1852; //*GC_K_NM_TO_METERS;


    //#[ operation rngAzToGeo(ncPosType,float,float,ncPosType&)
    //- LV 4.3.4	NAV_TO_LAT(?,?,?,?,?)–NAV_TO_LON(?,?,?,?,?) – Range/Azimuth to Geo transformation,
    //pg 94 Rev1.5 SRS for cursor management for grifo-f_br.doc -}
    float sinPhi = sinf( n_phi );
    float rEast  = R_WGS84 * ( 1 + (F_WGS84 * sinPhi * sinPhi) );
    float rNorth = R_WGS84 * ( 1 - (F_WGS84 * ( 2 - (3 * sinPhi * sinPhi ))) );
    float az = th  - psi - p_az; //in_app.true_heading

    float sinA = sinf( az );
    float cosA = cosf( az );
    float cosN = cosf( n_phi );

    float dPhi    = (p_rng * cosA) / rNorth;
    float dLambda = (p_rng * sinA) /( rEast * cosN );

    p_phi    = n_phi + dPhi;
    p_lambda = n_lambda + dLambda;
    //#]
}



