#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.isPush() || pad->r2.isHold()) {
	node->xyz[0] += 2.0f;
    } else if (pad->l2.isPush() || pad->l2.isHold()) {
	node->xyz[0] -= 2.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";

    lookat_base[0] =  0.0f;
    lookat_base[1] =  0.0f;
    lookat_base[2] =  0.0f;
    lookat_base[3] =  0.0f;

    up_base[0] =  0.0f;
    up_base[1] = 10.0f;
    up_base[2] =  0.0f;
    up_base[3] =  0.0f;

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

    //xyz[0] = 960.0f;
    //xyz[1] = 540.0f;
    xyz[2] = -1000.0f;
    
    this->set_move_collision(camera_move, camera_collision);
}

void
Camera::getLookAt(float *lookat)
{
    subVector(lookat, this->lookat_base, this->xyz);
    normalize(lookat, lookat);
}

void
Camera::getUp(float *up, float *lookat)
{
    outerProduct(up, this->up_base, lookat);
    normalize(up, up);
}

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::update(int w, int h)
{
    // うーん。。。
    float cx[4];
    float cy[4];
    float cz[4];
    float view[16]; // view transform matrix
    float pers[16]; // perspective transform matrix
    float screen[16]; // screen transform matrix
    float tmp[16];

    getLookAt(cz);
    getUp(cx, cz);
    outerProduct(cy, cz, cx);

    // view tansform
#if 1
    createViewTransformMatrix(view, cx, cy, cz);
    createPerspectiveTransformMatrix(pers, (float)w/(float)h);
    createScreenTransformMatrix(screen, (float)w, (float)h);
    
    matrix4x4(tmp, view, pers);
    matrix4x4(matrix, tmp, screen);
#else
    get_matrix(matrix, angle, xyz, NULL);
#endif
}