#include <fxengine/render/translate.h>

#include <gint/std/stdlib.h>
#include <gint/std/string.h>

const double pi = 3.141592653589793238462643383279;
const double pi2 = pi * 2;
const double pi_sur_2 = pi / 2;


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

// return a with -pi<=a<pi
double modulo_2pi(double a)
{
    while (a<=-pi)
		a += pi2;
    while (a>pi)
		a -= pi2;
    return a;
}

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

double cos(double angle)
{
    angle = modulo_2pi(angle);
    return cos_recursive(angle);
}

double sin(double angle)
{
	return cos(angle - pi_sur_2);
}


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

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

static FE_integer_position delta;


void render_translate(FE_integer_point * point)
{
    static FE_integer_position temp;
    temp.x = point->real.x - delta.x;
    temp.y = point->real.y - delta.y;
    temp.z = point->real.z - delta.z;

    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*=64;
    point->translated.y*=64;
    if (point->translated.z>0)
    {
        point->translated.x/=point->translated.z;
        point->translated.y/=point->translated.z;
    }
    else
    {
        point->translated.x*=32768*sgn(point->translated.z);
        point->translated.y*=32768*sgn(point->translated.z);
    }
    //(point->translated.x*1024)/point->translated.z;
    //(point->translated.y*1024)/point->translated.z;

    point->translated.x+=render_x_mid;
    point->translated.y+=render_y_mid;
}

void render_set(const double dh, const double dv, const double roulis, const FE_integer_position * camera)
{
    const double A=cos(dv);
    const double B=sin(dv);

    const double C=cos(roulis);
    const double D=sin(roulis);

    const double E=cos(dh);
    const double F=sin(dh);

    // raccourcis
    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;

    // assigner delta
    memcpy(&delta, camera, sizeof(FE_integer_position));
}