#include <math.h>
#include "SceneGraphRoot.h"
#include "Camera.h"
#include "matrix_calc.h"
#include "Scheduler.h"
#include "TaskManager.h"

//static SceneGraphRoot *sgroot;

#if 1
static void
camera_move(SceneGraphPtr _node, void *sgroot_, int screen_w, int screen_h)
{
    SceneGraphRoot *sgroot = (SceneGraphRoot *)sgroot_;
    Pad *pad = sgroot->getController();
    CameraPtr node = (CameraPtr)_node;

#if 0
    if (pad->right.isHold()) {
	node->xyz[0] += 10.0f;
    } else if (pad->left.isHold()) {
	node->xyz[0] -= 10.0f;
    }

    if (pad->up.isPush() || pad->up.isHold()) {
	node->xyz[1] -= 2.0f;
    } else if (pad->down.isPush() || pad->down.isHold()) {
	node->xyz[1] += 2.0f;
    }
#endif

    /*
     * ここに show_dma_wait 表示をいれようか
     */

    /*
    if (pad->r1.isPush()) {
	show_time(manager);
    }
    */

    if (pad->r1.isPush() || pad->r1.isHold()) {
	node->xyz[2] += 10.0f;
    } else if (pad->l1.isPush() || pad->l1.isHold()) {
	node->xyz[2] -= 10.0f;
    }

    if (pad->r2.isHold()) {
	if (node->zd[0] <= 1.0f) {
	    node->zd[0] += 0.02f;
	}
	if (node->zd[2] >= 0.0f) {
	    node->zd[2] -= 0.02f;
	}
    } else if (pad->l2.isHold()) {
	if (node->zd[0] > -1.0f) {
	    node->zd[0] -= -0.02f;
	}
	if (node->zd[2] >= 0.0f) {
	    node->zd[2] -= 0.02f;
	}
    } else {
	node->zd[0] = 0.0f;
	node->zd[2] = 1.0f;
    }

}
#endif

#if 0
static void
Camera::camera_move(SceneGraphPtr _node, int screen_w, int screen_h)
{
    Pad *pad = sgroot->getController();
    CameraPtr node = (CameraPtr)_node;

    if (pad->r1.isPush() || pad->r1.isHold()) {
	node->xyz[2] += 10.0f;
    } else if (pad->l1.isPush() || pad->l1.isHold()) {
	node->xyz[2] -= 10.0f;
    }

    if (pad->r2.isHold()) {
	if (node->zd[0] <= 1.0f) {
	    node->zd[0] += 0.02f;
	}
	if (node->zd[2] >= 0.0f) {
	    node->zd[2] -= 0.02f;
	}
    } else if (pad->l2.isHold()) {
	if (node->zd[0] > -1.0f) {
	    node->zd[0] -= -0.02f;
	}
	if (node->zd[2] >= 0.0f) {
	    node->zd[2] -= 0.02f;
	}
    } else {
	node->zd[0] = 0.0f;
	node->zd[2] = 1.0f;
    }

}
#endif

static void
camera_collision(SceneGraphPtr node, void *sgroot_,int screen_w, int screen_h,
		 SceneGraphPtr tree)
{
}

/**
 * @param w Width of screen
 * @param h Height of screen
 */
Camera::Camera(float w, float h, SceneGraphRoot *sgroot_, TaskManager *manager)  : SceneGraph(manager)
{

    name = (char*)"Camera";
    sgroot = sgroot_;

    fov  = 60.0f;
    near = 100.0f;
    far  = 1000.0f;

    zd[0] =  0.0f;
    zd[1] =  0.0f;
    zd[2] =  1.0f;
    zd[3] =  1.0f;

    yd[0] = 0.0f;
    yd[1] = 1.0f;
    yd[2] = 0.0f;
    yd[3] = 1.0f;

    // Screen の真ん中を初期値とする
    xyz[0] = w/2.0f;
    xyz[1] = h/2.0f;

    // min(w, h) がちょうど一杯見えるような z の位置の計算
    xyz[2] = -(((xyz[1] < xyz[0]) ? xyz[1] : xyz[0])/tanf((fov/2.0f)*M_PI/180.0f));
    //xyz[2] = -200.0f;
    xyz[3] = 1.0f;

    m_view = new float[16];
    m_pers = new float[16];
    m_screen = new float[16];

    //matrix = (float*)malloc(sizeof(float)*16);
    //real_matrix = (float*)malloc(sizeof(float)*16);
    //texture_info = (texture_list*)malloc(sizeof(texture_list));
    
    this->set_move_collision(camera_move, camera_collision, (void *)sgroot);

    for(int i = 0; i < 16; i++) {
      real_matrix[i] = 0;
      if (i % 5 == 0) {
        real_matrix[i] = 1;
      }
    }
    

    update(w,h); // to make matrix[]
}

Camera::~Camera(void)
{
    delete [] m_view;
    delete [] m_pers;
    delete [] m_screen;
}

void
Camera::updateView(void)
{
    float radx,rady,radz;
    float cx[4], cy[4], cz[4], p[4];
    float tm[16];

    radx = angle[0]*M_PI/180;
    rady = angle[1]*M_PI/180;
    radz = angle[2]*M_PI/180;

    float sinx = sin(radx);
    float cosx = cos(radx);
    float siny = sin(rady);
    float cosy = cos(rady);
    float sinz = sin(radz);
    float cosz = cos(radz);

    /* View Transform */
    tm[0] = cosz*cosy+sinz*sinx*siny;
    tm[1] = sinz*cosx;
    tm[2] = -cosz*siny+sinz*sinx*cosy;
    tm[3] = 0.0f;
    tm[4] = -sinz*cosy+cosz*sinx*siny;
    tm[5] = cosz*cosx;
    tm[6] = sinz*siny+cosz*sinx*cosy;
    tm[7] = 0.0f;
    tm[8] = cosx*siny;
    tm[9] = -sinx;
    tm[10] = cosx*cosy;
    tm[11] = 0.0f;
    tm[12] = 0.0f;
    tm[13] = 0.0f;
    tm[14] = 0.0f;
    tm[15] = 1.0f;

    /* transform */

    applyMatrix(cz, tm, zd);
    applyMatrix(cy, tm, yd);
    applyMatrix(p, tm, xyz);

    /* define view axis of coordinates */

    outerProduct(cx, cy, cz);
    normalize(cx, cx);
    normalize(cz, cz);
    outerProduct(cy, cz, cx);

    m_view[ 0] = cx[0];
    m_view[ 1] = cy[0];
    m_view[ 2] = cz[0];
    m_view[ 3] = 0.0f;

    m_view[ 4] = cx[1];
    m_view[ 5] = cy[1];
    m_view[ 6] = cz[1];
    m_view[ 7] = 0.0f;
    m_view[ 8] = cx[2];
    m_view[ 9] = cy[2];
    m_view[10] = cz[2];
    m_view[11] = 0.0f;

    m_view[12] = innerProduct(xyz, cx)*(-1);
    m_view[13] = innerProduct(xyz, cy)*(-1);
    m_view[14] = innerProduct(xyz, cz)*(-1);
    m_view[15] = 1.0f;
}

void
Camera::updatePerspective(float w, float h)
{
    float sx, sy, sz;
    float aspect = w/h;

    sy = (1.0f/tanf((fov/2.0f)*M_PI/180.0f));
    sx = sy/aspect;
    //sz = far/(far+near);
    sz = far/(far-near);

    m_pers[ 0] = sx;
    m_pers[ 1] = 0.0f;
    m_pers[ 2] = 0.0f;
    m_pers[ 3] = 0.0f;

    m_pers[ 4] = 0.0f;
    m_pers[ 5] = sy;
    m_pers[ 6] = 0.0f;
    m_pers[ 7] = 0.0f;

    m_pers[ 8] = 0.0f;
    m_pers[ 9] = 0.0f;
    m_pers[10] = sz;
    m_pers[11] = 1.0f;

    m_pers[12] = 0.0f;
    m_pers[13] = 0.0f;
    m_pers[14] = -near*sz;
    m_pers[15] = 0.0f;
}

void
Camera::updateScreen(float _w, float _h)
{
    float w = _w/2.0f;
    float h = _h/2.0f;

    m_screen[ 0] = w;
    m_screen[ 1] = 0.0f;
    m_screen[ 2] = 0.0f;
    m_screen[ 3] = 0.0f;

    m_screen[ 4] = 0.0f;
    m_screen[ 5] = h;
    m_screen[ 6] = 0.0f;
    m_screen[ 7] = 0.0f;

    m_screen[ 8] = 0.0f;
    m_screen[ 9] = 0.0f;
    m_screen[10] = 1.0f;
    m_screen[11] = 0.0f;

    m_screen[12] = w;
    m_screen[13] = h;
    m_screen[14] = 0.0f;
    m_screen[15] = 1.0f;    
}

void
Camera::setCamera(float *pose)
{
    //memcpy(xyz, &pose[12], sizeof(float)*4);
}

void
Camera::update(float w, float h)
{
#if 1
    float tmp[16];

    updateView();
    updatePerspective(w, h);
    updateScreen(w, h);

/*
    float det = determinant(this->m_screen);
    printf ("m_screen det %f\n",det);

    float inv_m[16];
    float re_m[16];
    inverseMatrix(inv_m, this->m_screen);
    matrix4x4(re_m, this->m_screen, inv_m);
    for (int i = 0; i < 16; i++) printf("%f ",re_m[i]);
    printf("\n");
*/
    matrix4x4(tmp, this->m_pers, this->m_screen);
    matrix4x4(this->matrix, this->m_view, tmp);
    //matrix4x4(this->real_matrix, this->real_matrix, this->m_view);

/*
    //float normal_temp[4] = { 0.0490661252103785, 0.00483240745105885, -0.998783842077487, 0};
    float normal_temp[4] = { 0.0, -0.0, 1.0, 0.0};
    float result_v[4];
    applyMatrix(result_v, normal_temp, matrix);
    printf("nomral_v \n");
    for (int i = 0; i < 4; i++) printf(" %f ", result_v[i]);
    printf("\n");
    normalize(result_v, result_v);
    printf("nomralize \n");
    for (int i = 0; i < 4; i++) printf(" %f ", result_v[i]);
    printf("\n");

    float object_temp[4] = { 20, 20, 20, 1};
    float light_temp[4] = { 30, 10, 40, 1 };
    float re_object[4];
    float re_light[4];
    float world_light_vec[4];

    for (int i = 0; i < 4; i++) world_light_vec[i] = light_temp[i] - object_temp[i];

    printf("light nomralize 1\n");
    normalize(world_light_vec, world_light_vec);
    for (int i = 0; i < 4; i++) printf(" %f ", world_light_vec[i]);
    printf("\n");

    float p = innerProduct(world_light_vec, normal_temp);
    printf("no mul matrix p %f\n",p);

    applyMatrix(re_object, object_temp, matrix);
    applyMatrix(re_light, light_temp, matrix);

    re_object[0] /= re_object[3];
    re_object[1] /= re_object[3];
    re_object[2] /= re_object[3];

    re_light[0] /= re_light[3];
    re_light[1] /= re_light[3];
    re_light[2] /= re_light[3];

    for (int i = 0; i < 4; i++) world_light_vec[i] = re_light[i] - re_object[i];
    printf("light nomralize 2\n");
    for (int i = 0; i < 4; i++) printf(" %f ", world_light_vec[i]);
    printf("\n");
    printf("light nomralize 3\n");
    normalize(world_light_vec, world_light_vec);
    for (int i = 0; i < 4; i++) printf(" %f ", world_light_vec[i]);
    printf("\n");

    p = innerProduct(world_light_vec, result_v);
    printf("mul matrix p %f\n",p);
*/

    /* 法線ベクトル、光源ベクトル に使うビュー座標変換までの行列 */
    for (int i = 0; i < 16; i++) this->real_matrix[i] = this->m_view[i];

    //matrix4x4(real_matrix, m_view, m_pers);


#else
    get_matrix(matrix, angle, xyz, NULL);
#endif
}