#include "coord.h"

const double FE_PI = 3.141592653589793238462643383279;
const double FE_2_PI = FE_PI * 2;
const double FE_PI_SUR_2 = FE_PI / 2;


static const int32_t precision=15; // précision (nombre obligatoirement impair) choisie de façon arbitraire

static double reducted_cos(const double a)
{
    double u= 1.0;
	const double a2 = a * a;
    for(int32_t p = precision; p>=1; p -= 2)
        u = 1 - a2 / (p * p + p) * u;
    return u;
}

// return a with -pi<=a<pi
double FE_modulo_2pi(double a)
{
    while (a<=-FE_PI)
		a += FE_2_PI;
    while (a>FE_PI)
		a -= FE_2_PI;
    return a;
}

static double cos_recursive(double angle)
{
	if (angle<0)
		return cos_recursive(-angle);
	if (angle>=FE_PI_SUR_2)
		return -reducted_cos(angle - FE_PI);
    return reducted_cos(angle); // OK
}

double FE_cos(double angle)
{
    angle = FE_modulo_2pi(angle);
    return cos_recursive(angle);
}

double FE_sin(double angle)
{
	return FE_cos(angle - FE_PI_SUR_2);
}


#define sgn(x) (x>=0?x:-x)

static double matrice[3][3]=
{
    {0,0,0},
    {0,0,0},
    {0,0,0}
};

void FE_calc(FE_point* point)
{
    static FE_position temp;
    temp.x = point->real.x - FE_user.x;
    temp.y = point->real.y - FE_user.y;
    temp.z = point->real.z - FE_user.z;

    point->rotated.x = (double)(matrice[0][0]*(double)temp.x + matrice[0][1]*(double)temp.y + matrice[0][2]*(double)temp.z);
    point->rotated.z = (double)(matrice[1][0]*(double)temp.x + matrice[1][1]*(double)temp.y + matrice[1][2]*(double)temp.z);
    point->rotated.y = (double)(matrice[2][0]*(double)temp.x + matrice[2][1]*(double)temp.y + matrice[2][2]*(double)temp.z);

    //point->translated.x*=10;
    //point->translated.y*=10;
    point->rotated.x *= 64;
    point->rotated.y *= 64;
    point->translated.z = point->rotated.z;
    if (point->translated.z>0)
    {
        point->translated.x = point->rotated.x / point->translated.z;
        point->translated.y = point->rotated.y / point->translated.z;
    }
    else
    {
        point->translated.x = point->rotated.x * 10000 * sgn(point->translated.z);
        point->translated.y = point->rotated.y * 10000 * sgn(point->translated.z);
    }
    // (point->translated.x*1000)/point->translated.z;
    // (point->translated.y*1000)/point->translated.z;

    point->translated.x+=63;
    point->translated.y+=31;
}


// FIABLE
void FE_set_matrice(void)
{
    const double A=FE_cos(FE_dv), B=FE_sin(FE_dv);
    const double C=FE_cos(FE_roulis), D=FE_sin(FE_roulis);
    const double E=FE_cos(FE_dh), F=FE_sin(FE_dh);

    const double AD=A*D, BD=B*D;

    matrice[0][0]=C*E;
    matrice[0][1]=-C*F;
    matrice[0][2]=D;

    matrice[1][0]=BD*E+A*F;
    matrice[1][1]=-BD*F+A*E;
    matrice[1][2]=-B*C;

    matrice[2][0]=-AD*E+B*F;
    matrice[2][1]=AD*F+B*E;
    matrice[2][2]=A*C;

}