/* glframe.c
 *
 * Camera
 *
 *
 */

#include <GL/glew.h>
#include <stdio.h>
#include "glframe.h"

void glframe_reset(GLFrame *frame)
{
    frame->v_location[0] = 0.0f;
    frame->v_location[1] = 0.0f;
    frame->v_location[2] = 0.0f;

    frame->v_forward[0] = 0.0f;
    frame->v_forward[1] = 0.0f;
    frame->v_forward[2] = -1.0f;

    frame->v_up[0] = 0.0f;
    frame->v_up[1] = 1.0f;
    frame->v_up[2] = 0.0f;
}

/* get a 4x4 transformation matrix that describes the camera orientation */
inline void glframe_get_camera_orientation(GLFrame *frame, M3DMatrix44f m)
{
    M3DVector3f x, z;

    /* make rotation matrix, z vector is reversed */
    z[0] = -frame->v_forward[0];
    z[1] = -frame->v_forward[1];
    z[2] = -frame->v_forward[2];

    /* x vector = y cross z */ 
    m3dCrossProductf(x, frame->v_up, z);

    /* matrix has no translation information and is transposed */
    #define M(row,col) m[col*4+row]
    M(0, 0) = x[0];
    M(0, 1) = x[1];
    M(0, 2) = x[2];
    M(0, 3) = 0.0;
    M(1, 0) = frame->v_up[0];
    M(1, 1) = frame->v_up[1];
    M(1, 2) = frame->v_up[2];
    M(1, 3) = 0.0;
    M(2, 0) = z[0];
    M(2, 1) = z[1];
    M(2, 2) = z[2];
    M(2, 3) = 0.0;
    M(3, 0) = 0.0;
    M(3, 1) = 0.0;
    M(3, 2) = 0.0;
    M(3, 3) = 1.0;
    #undef M 
}

/* perform viewing or modeling transformations */
inline void glframe_apply_camera_transform(GLFrame *frame, const int rot_only)
{
    if (rot_only)
    {
        gluLookAt(frame->v_location[0], frame->v_location[1], frame->v_location[2],
                  frame->v_location[0] + frame->v_forward[0],
                  frame->v_location[1] + frame->v_forward[1],
                  frame->v_location[2] + frame->v_forward[2],
                  frame->v_up[0], frame->v_up[1], frame->v_up[2]);

        return;
    }

    /* do it the hard way */
    M3DMatrix44f m;
    glframe_get_camera_orientation(frame, m);
    glMultMatrixf(m);

    if (!rot_only)
        glTranslatef(-frame->v_location[0], -frame->v_location[1],
                     -frame->v_location[2]);
}

inline void glframe_move_forward(GLFrame *frame, const float delta)
{
    /* move along direction of front direction */
    frame->v_location[0] += frame->v_forward[0] * delta;
    frame->v_location[1] += frame->v_forward[1] * delta;
    frame->v_location[2] += frame->v_forward[2] * delta;
}

/* move up or down */
inline void glframe_move_up(GLFrame *frame, const float delta)
{
    frame->v_location[0] += frame->v_up[0] * delta;
    frame->v_location[1] += frame->v_up[1] * delta;
    frame->v_location[2] += frame->v_up[2] * delta;
}

/* move left or right */
inline void glframe_move_right(GLFrame *frame, const float delta)
{
    M3DVector3f x;

    m3dCrossProductf(x, frame->v_up, frame->v_forward);
    frame->v_location[0] += x[0] * delta;
    frame->v_location[1] += x[1] * delta;
    frame->v_location[2] += x[2] * delta;
}

inline void glframe_translate_world(GLFrame *frame, const float x, const float y,
                             const float z)
{
    frame->v_location[0] += x;
    frame->v_location[1] += y;
    frame->v_location[2] += z;
}

inline void glframe_translate_local(GLFrame *frame, const float x, const float y,
                             const float z)
{
    glframe_move_forward(frame, z);
    glframe_move_up(frame, y);
    glframe_move_right(frame, x);
}

/* yaw */
void glframe_rotate_local_y(GLFrame *frame, const float angle)
{
    M3DMatrix44f rotmat;
    M3DVector3f newvect;

    /* create a rotation matrix around up vector */
    m3dRotationMatrix44f(rotmat, angle, frame->v_up[0], frame->v_up[1],
                         frame->v_up[2]);

    /* rotate forward pointing vector (inlined 3x3 transform) */
    newvect[0] = rotmat[0] * frame->v_forward[0]  + rotmat[4] *
                 frame->v_forward[1] + rotmat[8]  * frame->v_forward[2];
    newvect[1] = rotmat[1] * frame->v_forward[0]  + rotmat[5] *
                 frame->v_forward[1] + rotmat[9]  * frame->v_forward[2];
    newvect[2] = rotmat[2] * frame->v_forward[0]  + rotmat[6] *
                 frame->v_forward[1] + rotmat[10] * frame->v_forward[2];
    m3dCopyVector3f(frame->v_forward, newvect);
}

/* pitch */
void glframe_rotate_local_x(GLFrame *frame, const float angle)
{
    M3DMatrix44f rotmat;
    M3DVector3f newfwdvec;
    M3DVector3f newupvec;
    M3DVector3f x, z;

    /* z vector is reversed */
    z[0] = -frame->v_forward[0];
    z[1] = -frame->v_forward[1];
    z[2] = -frame->v_forward[2];

    /* x vector = y cross z */
    m3dCrossProductf(x, frame->v_up, z);

    /* create a rotation matrix around x axis */
    m3dRotationMatrix44f(rotmat, angle, x[0], x[1], x[2]);

    /* rotate forward pointing vector (inlined 3x3 transform) */
    newfwdvec[0] = rotmat[0] * frame->v_forward[0]  + rotmat[4] *
                   frame->v_forward[1] + rotmat[8]  * frame->v_forward[2];
    newfwdvec[1] = rotmat[1] * frame->v_forward[0]  + rotmat[5] *
                   frame->v_forward[1] + rotmat[9]  * frame->v_forward[2];
    newfwdvec[2] = rotmat[2] * frame->v_forward[0]  + rotmat[6] *
                   frame->v_forward[1] + rotmat[10] * frame->v_forward[2];

    /* calculate new up vector */
    m3dCrossProductf(newupvec, x, newfwdvec);

    m3dCopyVector3f(frame->v_forward, newfwdvec);
    m3dCopyVector3f(frame->v_up, newupvec);
}

/* roll */
void glframe_rotate_local_z(GLFrame *frame, const float angle)
{
    M3DMatrix44f rotmat;
    M3DVector3f newupvec;

    m3dRotationMatrix44f(rotmat, angle, frame->v_forward[0], frame->v_forward[1],
                         frame->v_forward[2]);

    /* rotate forward pointing vector (inlined 3x3 transform) */
    newupvec[0] = rotmat[0]  * frame->v_up[0] + rotmat[4] * frame->v_up[1] +
                  rotmat[8]  * frame->v_up[2];
    newupvec[1] = rotmat[1]  * frame->v_up[0] + rotmat[5] * frame->v_up[1] +
                  rotmat[9]  * frame->v_up[2];
    newupvec[2] = rotmat[2]  * frame->v_up[0] + rotmat[6] * frame->v_up[1] +
                  rotmat[10] * frame->v_up[2];
    m3dCopyVector3f(frame->v_up, newupvec);
}

/* reset axes to make sure they are orthonormal, should be called occasionally
 * if the matrix is long-lived and frequently transformed
 */
void gl_frame_normalize(GLFrame *frame)
{
    M3DVector3f x;

    m3dCrossProductf(x, frame->v_up, frame->v_forward);

    /* use result to recalculate forward vector */
    m3dCrossProductf(frame->v_forward, x, frame->v_up);

    /* also check for unit length */
    m3dNormalizeVectorf(frame->v_up);
    m3dNormalizeVectorf(frame->v_forward);
}

/* assemble the matrix */
void glframe_get_matrix(GLFrame *frame, M3DMatrix44f m, const int rot_only)
{
    M3DVector3f x;

    m3dCrossProductf(x, frame->v_up, frame->v_forward);

    m3dSetMatrixColumn44f(m, x, 0);
    m[3] = 0.0f;

    m3dSetMatrixColumn44f(m, frame->v_up, 1);
    m[7] = 0.0f;

    m3dSetMatrixColumn44f(m, frame->v_forward, 2);
    m[11] = 0.0f;

    if (rot_only)
    {
        m[12] = 0.0f;
        m[13] = 0.0f;
        m[14] = 0.0f;
    }
    else
        m3dSetMatrixColumn44f(m, frame->v_location, 3);

    m[15] = 1.0f;
}

/* position as an object in the scene, this places and orients a coordinate
 * frame for other objects besides the camera
 */
void gl_frame_apply_actor_transform(GLFrame *frame)
{
    M3DMatrix44f rotmat;
    glframe_get_matrix(frame, rotmat, 0);

    /* apply rotation to the current matrix */
    glMultMatrixf(rotmat);
}

/* rotate in world coordinates */
void glframe_rotate_world(GLFrame *frame, const float angle, const float x,
                          const float y, const float z)
{
    M3DMatrix44f rotmat;

    /* create the rotation matrix */
    m3dRotationMatrix44f(rotmat, angle, x, y, z);

    M3DVector3f newvect;

    newvect[0] = rotmat[0] * frame->v_up[0] + rotmat[4] *
                 frame->v_up[1] + rotmat[8] * frame->v_up[2];
    newvect[1] = rotmat[1] * frame->v_up[0] + rotmat[5] *
                 frame->v_up[1] + rotmat[9] * frame->v_up[2];
    newvect[2] = rotmat[2] * frame->v_up[0] + rotmat[6] *
                 frame->v_up[1] + rotmat[10] * frame->v_up[2];

    m3dCopyVector3f(frame->v_up, newvect);

    /* transform the forward axis */
    newvect[0] = rotmat[0] * frame->v_forward[0] + rotmat[4] *
                 frame->v_forward[1] + rotmat[8] * frame->v_forward[2];
    newvect[1] = rotmat[1] * frame->v_forward[0] + rotmat[5] *
                 frame->v_forward[1] + rotmat[9] * frame->v_forward[2];
    newvect[2] = rotmat[2] * frame->v_forward[0] + rotmat[6] *
                 frame->v_forward[1] + rotmat[10] * frame->v_forward[2];

    m3dCopyVector3f(frame->v_forward, newvect);
}

/* rotate around a local axis */
void glframe_rotate_local(GLFrame *frame, const float angle, const float x,
                          const float y, const float z)
{
    M3DVector3f world_vec;
    M3DVector3f local_vec;
    m3dLoadVector3f(local_vec, x, y, z);

    glframe_local_to_world(frame, local_vec, world_vec);
    glframe_rotate_world(frame, angle, world_vec[0], world_vec[1], world_vec[2]);
}

/* convert coordinate systems, do the transformation represented by the rotation
 * and position on the point
 */
void glframe_local_to_world(GLFrame *frame, const M3DVector3f local, M3DVector3f world)
{
    M3DMatrix44f rotmat;

    glframe_get_matrix(frame, rotmat, 1);

    world[0] = rotmat[0] * local[0] + rotmat[4] * local[1] + rotmat[8] * local[2];
    world[1] = rotmat[1] * local[0] + rotmat[5] * local[1] + rotmat[9] * local[2];
    world[2] = rotmat[2] * local[0] + rotmat[6] * local[1] + rotmat[10] * local[2];

    /* translate the point */
    world[0] += frame->v_location[0];
    world[1] += frame->v_location[1];
    world[2] += frame->v_location[2];
}

/* change world coordinates into "local" coordinates */
void glframe_world_to_local(GLFrame *frame, const M3DVector3f world, M3DVector3f local)
{
    /* translate the origin */
    M3DVector3f new_world;
    new_world[0] = world[0] - frame->v_location[0];
    new_world[1] = world[1] - frame->v_location[1];
    new_world[2] = world[2] - frame->v_location[2];

    /* create the rotation matrix based on the vectors */
    M3DMatrix44f rotmat;
    M3DMatrix44f invmat;
    glframe_get_matrix(frame, rotmat, 1);

    /* do the rotation based on inverted matrix */
    if (m3dInvertMatrix44f(invmat, rotmat) == -1)
        fprintf(stderr, "glframe: m3dInvertMatrix44f() failed\n");

    local[0] = invmat[0] * new_world[0] + invmat[4] *
               new_world[1] + invmat[8] * new_world[2];
    local[1] = invmat[1] * new_world[0] + invmat[5] *
               new_world[1] + invmat[9] * new_world[2];
    local[2] = invmat[2] * new_world[0] + invmat[6] *
               new_world[1] + invmat[10] * new_world[2];
}

/* transform a point by frame matrix */
void glframe_transform_point(GLFrame *frame, const M3DVector3f src, M3DVector3f dst)
{
    M3DMatrix44f m;

    /* rotate and translate */
    glframe_get_matrix(frame, m, 0);

    dst[0] = m[0] * src[0] + m[4] * src[1] + m[8]  * src[2] + m[12];
    dst[1] = m[1] * src[0] + m[5] * src[1] + m[9]  * src[2] + m[13];
    dst[2] = m[2] * src[0] + m[6] * src[1] + m[10] * src[2] + m[14];
}

/* rotate a vector by frame matrix */
void glframe_rotate_vector(GLFrame *frame, M3DVector3f src, M3DVector3f dst)
{
    M3DMatrix44f m;
    glframe_get_matrix(frame, m, 1); /* rotate only */

    dst[0] = m[0] * src[0] + m[4] * src[1] + m[8]  * src[2];
    dst[1] = m[1] * src[0] + m[5] * src[1] + m[9]  * src[2];
    dst[2] = m[2] * src[0] + m[6] * src[1] + m[10] * src[2];
}