#include "space.h"
#include "camera.h"

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

double fe_matrix[3][3]=
{
    {0,0,0},
    {0,0,0},
    {0,0,0}
};
fe_ipoint fe_translate_delta;

static double reducted_cos(const double a)
{
    double u= 1.0;
	const double a2 = a * a;
    for(int32_t p = 17; 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;
}

double fe_cos(double angle)
{
    angle = fe_modulo_2pi(angle);
    if (angle<0)
        angle=-angle;
    if (angle>=pi_sur_2)
        return -reducted_cos(angle - pi);
    return reducted_cos(angle);
}

double fe_sin(double angle)
{
	return fe_cos(angle - pi_sur_2);
}


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

void fe_vertex_translate(fe_ivertex * v)
{
    static fe_ipoint temp;
    temp.x = v->real.x - fe_translate_delta.x;
    temp.y = v->real.y - fe_translate_delta.y;
    temp.z = v->real.z - fe_translate_delta.z;

    v->translated.x = (double)(fe_matrix[0][0]*(double)temp.x + fe_matrix[0][1]*(double)temp.y + fe_matrix[0][2]*(double)temp.z);
    v->translated.z = (double)(fe_matrix[1][0]*(double)temp.x + fe_matrix[1][1]*(double)temp.y + fe_matrix[1][2]*(double)temp.z);
    v->translated.y = (double)(fe_matrix[2][0]*(double)temp.x + fe_matrix[2][1]*(double)temp.y + fe_matrix[2][2]*(double)temp.z);

    //v->translated.x*=10;
    //v->translated.y*=10;
    v->translated.x*=64;
    v->translated.y*=64;
    if (v->translated.z>0)
    {
        v->translated.x/=v->translated.z;
        v->translated.y/=v->translated.z;
    }
    else
    {
        v->translated.x*=32768*sgn(v->translated.z);
        v->translated.y*=32768*sgn(v->translated.z);
    }
    //(v->translated.x*1024)/v->translated.z;
    //(v->translated.y*1024)/v->translated.z;

    v->translated.x+=fe_x_mid;
    v->translated.y+=fe_y_mid;
}