#include <math.h>
#include "SceneGraphRoot.h"
#include "Camera.h"
#include "sys.h"

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

    if (pad->r2.isHold()) {
	node->angle[1] += 2.0f;
	if (node->angle[1] > 360.0f) {
	    node->angle[1] -= 360.0f;
	}
    } else if (pad->l2.isHold()) {
	node->angle[1] -= 2.0f;
	if (node->angle[1] < 0.0f) {
	    node->angle[1] += 360.0f;
	}
    }

#if 0
    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

    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;
    }
}

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

Camera::Camera(void)
{
    name = (const char*)"Camera";

    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;

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

    //xyz[0] = 320.0f;
    //xyz[1] = 240.0f;
    //xyz[2] = -100.0f;
    //xyz[3] = 1.0f;
    
    this->set_move_collision(camera_move, camera_collision);
}

void
Camera::createRotMatrix(float *m)
{
    float radx,rady,radz;
    float cx[4], cy[4], cz[4];
    float p[4];
    float *matrix = new float[16];

    radx = angle[0]*3.14/180;
    rady = angle[1]*3.14/180;
    radz = angle[2]*3.14/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 */
    matrix[0] = cosz*cosy+sinz*sinx*siny;
    matrix[1] = sinz*cosx;
    matrix[2] = -cosz*siny+sinz*sinx*cosy;
    matrix[3] = 0.0f;
    matrix[4] = -sinz*cosy+cosz*sinx*siny;
    matrix[5] = cosz*cosx;
    matrix[6] = sinz*siny+cosz*sinx*cosy;
    matrix[7] = 0.0f;
    matrix[8] = cosx*siny;
    matrix[9] = -sinx;
    matrix[10] = cosx*cosy;
    matrix[11] = 0.0f;
    matrix[12] = 0.0f;
    matrix[13] = 0.0f;
    matrix[14] = 0.0f;
    matrix[15] = 1.0f;

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

    // matrix 使い回すためにクリア
    unitMatrix(matrix);

    outerProduct(cx, cy, cz);
    normalize(&matrix[0], cx);
    normalize(&matrix[8], cz);
    outerProduct(&matrix[4], &matrix[8], &matrix[0]);
    transMatrix(matrix, matrix, p);
    inversMatrix(m, matrix);
}

void
Camera::createViewTransformMatrix(float *view, float *cx, float *cy, float *cz)
{
    view[ 0] = cx[0];
    view[ 1] = cy[0];
    view[ 2] = cz[0];
    view[ 3] = 0.0f;

    view[ 4] = cx[1];
    view[ 5] = cy[1];
    view[ 6] = cz[1];
    view[ 7] = 0.0f;

    view[ 8] = cx[2];
    view[ 9] = cy[2];
    view[10] = cz[2];
    view[11] = 0.0f;

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

void
Camera::createPerspectiveTransformMatrix(float *pm, float aspect)
{
    float sx, sy, sz;

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

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

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

    pm[ 8] = 0.0f;
    pm[ 9] = 0.0f;
    pm[10] = sz;
    pm[11] = -near*sz;

    pm[12] = 0.0f;
    pm[13] = 0.0f;
    pm[14] = -1.0f;
    pm[15] = 0.0f;
}

void
Camera::createScreenTransformMatrix(float *sm, float _w, float _h)
{
    float w = _w/2.0f;
    float h = _h/2.0f;
    float r = far/(far-near);

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

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

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

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

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

void
Camera::update(int w, int h)
{
#if 0
    // うーん。。。
    float view[16]; // view transform matrix
    float pers[16]; // perspective transform matrix
    float screen[16]; // screen transform matrix
    float tmp[16];

    // view tansform
    createRotMatrix(view);
    createPerspectiveTransformMatrix(pers, (float)w/(float)h);
    createScreenTransformMatrix(screen, (float)w, (float)h);
    
    matrix4x4(tmp, view, pers);
    matrix4x4(matrix, tmp, screen);
    //matrix4x4(tmp, pers, view);
    //matrix4x4(matrix, screen, tmp);
#else
    get_matrix(matrix, angle, xyz, NULL);
#endif
}