#pragma once

//  ---------------------
//  Constants
//  ---------------------

//	#define PI (3.14159265358979323846)

	const double toRad = PI/180;
	const double toDeg = 180.0/PI;

	const float toRadF = (float)toRad;
	const float toDegF = (float)toDeg;

//	----------------------------------------------------------------------------
//	constants
//	----------------------------------------------------------------------------

	enum { N, E, D, U = D };

	enum {  PHI,  THE,  PSI };		// xyz = rpy = phi, theta, psi (old order)
	enum { xPSI, xTHE, xPHI };		// ypr = zyx = psi, theta, phi (new order)

//  ---------------------
//  Functions
//  ---------------------

	void getDirCosinesX(double (*eu)[3],double (*dc)[3][3]);
	void getDirCosinesF(float (*eu)[3],float (*dc)[3][3]);
	void addDirCosinesX(double (*eu)[3], double (*dc)[3][3], int preMult = 0);
	void addDirCosinesF(float (*eu)[3], float (*dc)[3][3], int preMult = 0);
	void addDirCosinesX(double (*eu)[3], double (*dcIn)[3][3], double (*dcOut)[3][3], int preMult = 0);
	void rotToYprEulerX(double (*rotPtr)[3][3], double (*eulers)[3]);
	void rotToYprEulerF(float (*rotPtr)[3][3], float (*eulers)[3]);
	void yprEulerToRotX(double (*eulers)[3], double (*rotPtr)[3][3]);
	void yprEulerToRot(double (*eulers)[3], double (*rotPtr)[3][3]);
	void yprEulerToRot(float (*eulers)[3], double (*rotPtr)[3][3]);
	void quatIntX(double (*q)[4], double (*omega)[3]);
	double quatNormalize(double (*q)[4]);
	void quatToRotX(double (*q)[4], double (*rotPtr)[3][3]);
	void rotToQuatX(double (*dc)[3][3], double (*q)[4]);
	void yprEulerToQuatX(double (*eulers)[3], double (*q)[4]);
	void rotate3d(double pt[], double rol, double pit, double yaw);
	void rotate3d(double *p0,double *p1,double *p2, double rol, double pit, double yaw);
	void mult3by3times3by1(double (*a)[3][3], double (*b)[3], double (*c)[3]);
	void mult3by3times3by1(float (*a)[3][3], float (*b)[3], float (*c)[3]);
	void mult3by3times3by3(double (*a)[3][3], double (*b)[3][3], double (*c)[3][3]);
	void mult3by3times3by3F(float (*a)[3][3], float (*b)[3][3], float (*c)[3][3]);
	void multT3by3times3by1(double(*a)[3][3], double(*b)[3], double(*c)[3]);
	void transpose3by3(double (*a)[3][3], double (*b)[3][3]);
	double determinant3x3(double (*a)[3][3]);
	bool invert3by3(double (*a)[3][3], double (*b)[3][3]);

	void passiveTarget(
		double (*acpos)[3],					// aircraft pos [m] NEU
		double altT,						// target altitude [m] NEU
		double vfov,						// vertical FOV [deg]
		double (*dc)[3][3],					// dircos for LOS to Earth
		double (*ptCen)[3],					// result for center of frame
		double (*ptTop)[3]);				// result for top of frame
	void passiveTarget(
		double (*acpos)[3],					// aircraft pos [m] NEU
		double altT,						// target altitude [m] NEU
		double (*dc)[3][3],					// dircos for LOS to Earth
		double (*ptCen)[3]);				// result for center of frame
	int circle_circle_intersection(
		double x0, double y0, double r0,		// center and radius of 1st circle
		double x1, double y1, double r1,		// center and radius of 2nd circle
		double *xi, double *yi,					// 1st intersection point
		double *xi_prime, double *yi_prime);	// 2nd intersection point
	int line_line_intersection(
		 double x11, double y11,			// line1 p1
		 double x21, double y21,			// line1 p2
		 double x12, double y12,			// line2 p1
		 double x22, double y22,			// line2 p2
		 double *x, double *y);				// result
	double line_length(
		 double x1, double y1,				// p1
		 double x2, double y2);				// p2
	int LineLineIntersect(
		double(*p1)[3], double(*p2)[3],
		double(*p3)[3], double(*p4)[3],
		double(*pa)[3], double(*pb)[3]);
	double angleDiff(double a2,double a1);
	float angleDiff(float a2,float a1);
	double addAngle(double a2,double a1);
	double linelength(double (*a)[3],double (*b)[3]);
	double linelength(const double *a, const double *b);
	double linelength(const double *a);
	double signedMagnitude(const double *a);
	void crossProduct(double (*a)[3], double (*b)[3], double (*c)[3]);
	void crossProduct(double *a, double *b, double *c);
	double arcSineDeg(double y, double r);
	double arcSineDeg(double s);

//	----------------------------------------------------------------------------
//	Geo
//	----------------------------------------------------------------------------

	void ecefToGeodetic(double (*ecef)[3], double (*position)[3]);
	void geodeticToEcef(double (*position)[3], double (*ecef)[3]);
	void earthToRe2t(double (*position)[3], double (*Re2t)[3][3]);
