#include <math.h>
#include "SceneGraphRoot.h"
#include "Camera.h"
#include "sys.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_)
{
    name = (char*)"Camera";
    sgroot = sgroot_;

    fov  = 60.0f;
    near = 0.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];
    
    this->set_move_collision(camera_move, camera_collision, (void *)sgroot);

#if SPE_CREATE_POLYGON

#endif

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

}

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]*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 */
    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;

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

    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);
    
    matrix4x4(tmp, this->m_pers, this->m_screen);
    matrix4x4(this->matrix, this->m_view, tmp);
#else
    get_matrix(matrix, angle, xyz, NULL);
#endif
}