#include "coord.h"
#include "FxEngine.h"


/* Cosinus and sinus implementation */

/// Math values
const double pi=3.141592653589793238462643383279;
const double pi2=pi*2;
const double pi_sur2=pi/2;

/// Internal Use
// cos function with 0<a<pi/2
static const int precision;
static double reducted_cos(const double a)
{
    double u=1.0; const double a2=a*a;
    // precision%2=1 && precision>1
    for(int 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<=-pi) a+=pi2;
    while (a>pi) a-=pi2;
    return a;
}


/// External functions
// Official FE_cos function
double FE_cos(double angle)
{
    angle=FE_modulo_2pi(angle);
    int angle_arrondi=(int)(angle/pi_sur2);
    switch (angle_arrondi)
    {
        case -2:
        return -reducted_cos(angle+pi);
        case 1:
        return -reducted_cos(angle);
        case -1:
        return reducted_cos(-angle);
        case 0:
        return reducted_cos(angle);
    }
    return 0;
}
// Official FE_sin function
double FE_sin(double angle)
{return FE_cos(angle-pi_sur2);}




static double matrice[4][4]=
{
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,1}
};

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;
    temp.x *=15;
    temp.y *=15;
    temp.z *=15;

    point->translated.x = (double)(matrice[0][0]*(double)temp.x + matrice[0][1]*(double)temp.y + matrice[0][2]*(double)temp.z);
    point->translated.z = (double)(matrice[1][0]*(double)temp.x + matrice[1][1]*(double)temp.y + matrice[1][2]*(double)temp.z);
    point->translated.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.translated.x/=point.translated.z;
    //point.translated.y/=point.translated.z;
}

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

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

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

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

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

    //matrice[3][0]=0;
    //matrice[3][1]=0;
    //matrice[3][2]=0;
    //matrice[3][3]=1;
}