/*
 *			GPAC - MPEG-4 Systems C Development Kit
 *
 *			Copyright (c) Jean Le Feuvre 2000-2005
 *					All rights reserved
 *
 *  This file is part of GPAC / 3D rendering plugin
 *
 *  GPAC is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2, or (at your option)
 *  any later version.
 *   
 *  GPAC is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *   
 *  You should have received a copy of the GNU General Public License
 *  along with GNU Make; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. 
 *
 */

#include "camera.h"


M4Camera *new_camera()
{
	M4Camera *tmp;
	SAFEALLOC(tmp, sizeof(M4Camera));
	tmp->zoom = FIX_ONE;
	return tmp;
}
void delete_camera(M4Camera *cam)
{
	if (cam) free(cam);
}

void camera_invalidate(M4Camera *cam)
{
	/*forces recompute of default viewpoint*/
	cam->had_viewpoint = 2;
	cam->had_nav_info = 1;
	cam->flags = CAM_IS_DIRTY;
	cam->zoom = FIX_ONE;
	cam->rot.x = cam->rot.y = 0;
	cam->navigate_mode = M4_NavNone;
}

static void camera_frustum_from_matrix(M4Camera *cam, M4Matrix *mx)
{
	u32 i;

	cam->planes[FRUS_LEFT_PLANE].normal.x = mx->m[3] + mx->m[0];
	cam->planes[FRUS_LEFT_PLANE].normal.y = mx->m[7] + mx->m[4];
	cam->planes[FRUS_LEFT_PLANE].normal.z = mx->m[11] + mx->m[8];
	cam->planes[FRUS_LEFT_PLANE].d = mx->m[15] + mx->m[12];

	cam->planes[FRUS_RIGHT_PLANE].normal.x = mx->m[3] - mx->m[0];
	cam->planes[FRUS_RIGHT_PLANE].normal.y = mx->m[7] - mx->m[4];
	cam->planes[FRUS_RIGHT_PLANE].normal.z = mx->m[11] - mx->m[8];
	cam->planes[FRUS_RIGHT_PLANE].d = mx->m[15] - mx->m[12];

	cam->planes[FRUS_BOTTOM_PLANE].normal.x = mx->m[3] + mx->m[1];
	cam->planes[FRUS_BOTTOM_PLANE].normal.y = mx->m[7] + mx->m[5];
	cam->planes[FRUS_BOTTOM_PLANE].normal.z = mx->m[11] + mx->m[9];
	cam->planes[FRUS_BOTTOM_PLANE].d = mx->m[15] + mx->m[13];

	cam->planes[FRUS_TOP_PLANE].normal.x = mx->m[3] - mx->m[1];
	cam->planes[FRUS_TOP_PLANE].normal.y = mx->m[7] - mx->m[5];
	cam->planes[FRUS_TOP_PLANE].normal.z = mx->m[11] - mx->m[9];
	cam->planes[FRUS_TOP_PLANE].d = mx->m[15] - mx->m[13];

	cam->planes[FRUS_FAR_PLANE].normal.x = mx->m[3] - mx->m[2];
	cam->planes[FRUS_FAR_PLANE].normal.y = mx->m[7] - mx->m[6];
	cam->planes[FRUS_FAR_PLANE].normal.z = mx->m[11] - mx->m[10];
	cam->planes[FRUS_FAR_PLANE].d = mx->m[15] - mx->m[14];

	cam->planes[FRUS_NEAR_PLANE].normal.x = mx->m[3] + mx->m[2];
	cam->planes[FRUS_NEAR_PLANE].normal.y = mx->m[7] + mx->m[6];
	cam->planes[FRUS_NEAR_PLANE].normal.z = mx->m[11] + mx->m[10];
	cam->planes[FRUS_NEAR_PLANE].d = mx->m[15] + mx->m[14];

	for (i=0; i<6; ++i) {
#ifdef M4_FIXED_POINT
		/*after some testing, it's just safer to move back to float here, the smallest drift will
		result in completely wrong culling...*/
		Float vx, vy, vz, nor;
		vx = FIX2FLT(cam->planes[i].normal.x);
		vy = FIX2FLT(cam->planes[i].normal.y);
		vz = FIX2FLT(cam->planes[i].normal.z);
		nor = (Float) sqrt(vx*vx + vy*vy + vz*vz);
		vx /= nor; vy /= nor; vz /= nor;
		cam->planes[i].d = FLT2FIX (FIX2FLT(cam->planes[i].d) / nor);
		cam->planes[i].normal.x = FLT2FIX(vx);
		cam->planes[i].normal.y = FLT2FIX(vy);
		cam->planes[i].normal.z = FLT2FIX(vz);
#else
		Float len = (Float)(1.0f / m4_vec_len(cam->planes[i].normal));
		cam->planes[i].normal = m4_vec_scale(cam->planes[i].normal, len);
		cam->planes[i].d *= len;
#endif

		/*compute p-vertex idx*/
		cam->p_idx[i] = m4_plane_get_p_vertex_idx(&cam->planes[i]);
	}
}

/*based on  Copyright (C) 1995  Stephen Chenney (schenney@cs.berkeley.edu)*/
SFRotation camera_get_orientation(SFVec3f pos, SFVec3f target, SFVec3f up)
{
	SFVec3f dir, tmp, v, axis, new_y;
	SFVec4f norm, inv_norm, y_quat, ny_quat, rot_y, rot;

	m4_vec_diff(dir, target, pos);
	m4_vec_norm(&dir);
	tmp = m4_vec_scale(dir, m4_vec_dot(up, dir));
	m4_vec_diff(v, up, tmp);
	m4_vec_norm(&v);
	axis.x = dir.y; axis.y = -dir.x; axis.z = 0;

	if (m4_vec_dot(axis, axis) < FIX_EPSILON) {
		if (dir.z> 0) {
			norm.x = 0; norm.y = FIX_ONE; norm.z = 0; norm.q = 0;
		} else {
			norm.x = 0; norm.y = 0; norm.z = 0; norm.q = FIX_ONE;
		}
	} else {
		m4_vec_norm(&axis);
		norm = m4_quat_from_axis_cos(axis, -dir.z);
	}
	/* Find the inverse rotation. */
    inv_norm.x = -norm.x; inv_norm.y = -norm.y; inv_norm.z = -norm.z; inv_norm.q = norm.q;
	/* Rotate the y. */
	y_quat.x = y_quat.z = y_quat.q = 0; y_quat.y = FIX_ONE;
	ny_quat = m4_quat_multiply(&norm, &y_quat);
	ny_quat = m4_quat_multiply(&ny_quat, &inv_norm);

	new_y.x = ny_quat.x; new_y.y = ny_quat.y; new_y.z = ny_quat.z;

	tmp = m4_vec_cross(new_y, v);

	if (m4_vec_dot(tmp, tmp) < FIX_EPSILON) {
		/* The old and new may be pointing in the same or opposite. Need
		** to generate a vector perpendicular to the old or new y.
		*/
		tmp.x = 0; tmp.y = -v.z; tmp.z = v.y;
		if (m4_vec_dot(tmp, tmp) < FIX_EPSILON) {
			tmp.x = v.z; tmp.y = 0; tmp.z = -v.x;
		}
	}
	m4_vec_norm(&tmp);

	rot_y = m4_quat_from_axis_cos(tmp, m4_vec_dot(new_y, v));

	/* rot_y holds the rotation about the initial camera direction needed
	** to align the up vectors in the final position.
	*/

	/* Put the 2 rotations together. */
	rot = m4_quat_multiply(&rot_y, &norm);
	return m4_quat_to_rotation(&rot);
}

void camera_update(M4Camera *cam)
{
	Fixed vlen, h, w, ar;
	SFVec3f corner, center;

	if (! (cam->flags & CAM_IS_DIRTY)) return;

	ar = m4_divfix(cam->width, cam->height);
	if (cam->is_3D) {
		/*setup perspective*/
		m4_mx_perspective(&cam->projection, cam->fieldOfView, ar, cam->z_near, cam->z_far);
		/*setup modelview*/
		m4_mx_lookat(&cam->modelview, cam->position, cam->target, cam->up);

		/*compute center and radius*/
		vlen = cam->z_far - cam->z_near;
		h = m4_mulfix(vlen , m4_tan(cam->fieldOfView / 2));
		w = m4_mulfix(h, ar);
		center.x = 0; center.y = 0; center.z = cam->z_near + vlen / 2;
		corner.x = w; corner.y = h; corner.z = vlen;
		m4_vec_diff(corner, corner, center);
		cam->radius = m4_vec_len(corner);
		m4_vec_diff(cam->center, cam->target, cam->position);
		m4_vec_norm(&cam->center);
		cam->center = m4_vec_scale(cam->center, cam->z_near + vlen/2);
		m4_vec_add(cam->center, cam->center, cam->position);
	} else {
		M4BBox b;
		Fixed hw, hh;
		hw = cam->width / 2;
		hh = cam->height / 2;
		cam->z_near = -INT2FIX(512);
		cam->z_far = INT2FIX(512);

		/*setup ortho*/
		m4_mx_ortho(&cam->projection, -hw, hw, -hh, hh, cam->z_near, cam->z_far);
		/*setup modelview*/
		m4_mx_init(cam->modelview);
		m4_mx_add_scale(&cam->modelview, cam->zoom, cam->zoom, FIX_ONE);
		m4_mx_add_translation(&cam->modelview, cam->trans.x, cam->trans.y, 0);
		if (cam->rot.x) m4_mx_add_rotation(&cam->modelview, cam->rot.x, FIX_ONE, 0, 0);
		if (cam->rot.y) m4_mx_add_rotation(&cam->modelview, cam->rot.y, 0, FIX_ONE, 0);
		if (cam->flags & CAM_HAS_VIEWPORT) m4_mx_add_matrix(&cam->modelview, &cam->viewport);

		/*compute center & radius*/
		b.max_edge.x = hw;
		b.max_edge.y = hh;
		b.min_edge.x = -hw;
		b.min_edge.y = -hh;
		b.min_edge.z = b.max_edge.z = (cam->z_near+cam->z_far) / 2;
		m4_bbox_refresh(&b);
		cam->center = b.center;
		cam->radius = b.radius;
	}
	/*compute frustum planes*/
	m4_mx_copy(cam->unprojection, cam->projection);
	m4_mx_add_matrix_4x4(&cam->unprojection, &cam->modelview);
	camera_frustum_from_matrix(cam, &cam->unprojection);
	/*also compute reverse PM for unprojections*/
	m4_mx_inverse_4x4(&cam->unprojection);
	cam->flags &= ~CAM_IS_DIRTY;
}

void camera_set_vectors(M4Camera *cam, SFVec3f pos, SFRotation ori, Fixed fov)
{
	Fixed sin_a, cos_a, icos_a, tmp;

	cam->fieldOfView = fov;
	cam->last_pos = cam->position;
	cam->position = pos;
	/*compute up & target vectors in local system*/
	sin_a = m4_sin(ori.q);
	cos_a = m4_cos(ori.q);
	icos_a = FIX_ONE - cos_a;
	tmp = m4_mulfix(icos_a, ori.z);
	cam->target.x = m4_mulfix(ori.x, tmp) + m4_mulfix(sin_a, ori.y);
	cam->target.y = m4_mulfix(ori.y, tmp) - m4_mulfix(sin_a, ori.x);
	cam->target.z = m4_mulfix(ori.z, tmp) + cos_a;
	m4_vec_norm(&cam->target);
	cam->target = m4_vec_scale(cam->target, -cam->vp_dist);
	m4_vec_add(cam->target, cam->target, pos);
	tmp = m4_mulfix(icos_a, ori.y);
	cam->up.x = m4_mulfix(ori.x, tmp) - m4_mulfix(sin_a, ori.z);
	cam->up.y = m4_mulfix(ori.y, tmp) + cos_a;
	cam->up.z = m4_mulfix(ori.z, tmp) + m4_mulfix(sin_a, ori.x);
	m4_vec_norm(&cam->up);
	cam->flags |= CAM_IS_DIRTY;
}

void camera_reset_viewpoint(M4Camera *cam, Bool animate)
{
	if (!animate || (cam->had_viewpoint==2) ) {
		camera_set_vectors(cam, cam->vp_position, cam->vp_orientation, cam->vp_fov);
		cam->last_pos = cam->vp_position;
		return;
	}
	if (cam->is_3D) {
		cam->start_pos = cam->position;
		cam->start_ori = camera_get_orientation(cam->position, cam->target, cam->up);
		cam->start_fov = cam->fieldOfView;
		cam->end_pos = cam->vp_position;
		cam->end_ori = cam->vp_orientation;
		cam->end_fov = cam->vp_fov;
	} else {
		Fixed tmp;
		cam->start_zoom = cam->zoom;
		cam->start_trans = cam->trans;
		cam->start_rot = cam->rot;
		tmp = cam->rot.x;
		while (tmp<0) tmp += M4_2PI; while (tmp>M4_2PI) tmp-=M4_2PI;
		cam->start_rot.x = tmp;
		tmp = cam->rot.y;
		while (tmp<0) tmp += M4_2PI; while (tmp>M4_2PI) tmp-=M4_2PI;
		cam->start_rot.y = tmp;
	}
	cam->flags |= CAM_IS_DIRTY;
	cam->anim_start = 0;
	cam->anim_len = 1000;
}

void camera_move_to(M4Camera *cam, SFVec3f pos, SFVec3f target, SFVec3f up)
{
	if (!cam->anim_len) {
		cam->start_pos = cam->position;
		cam->start_ori = camera_get_orientation(cam->position, cam->target, cam->up);
		cam->start_fov = cam->fieldOfView;
	}
	cam->end_pos = pos;
	cam->end_ori = camera_get_orientation(pos, target, up);
	cam->end_fov = cam->fieldOfView;

	cam->flags |= CAM_IS_DIRTY;
	cam->anim_start = 0;
	cam->anim_len = 100;
}

void camera_stop_anim(M4Camera *cam)
{
	cam->anim_len = 0;
}

void camera_jump(M4Camera *cam)
{
	/*no "double-jump" :)*/
	if (cam->jumping) return;
	cam->anim_start = 0;
	cam->anim_len = 1000;
	cam->jumping = 1;
	cam->flags |= CAM_IS_DIRTY;
}


Bool camera_animate(M4Camera *cam)
{
	u32 now;
	Fixed frac;
	if (!cam->anim_len) return 0;

	if (cam->jumping) {
		if (!cam->anim_start) {
			cam->anim_start = M4_GetSysClock();
			cam->dheight = 0;
			return 1;
		}
		cam->position.y -= cam->dheight;
		cam->target.y -= cam->dheight;

		now = M4_GetSysClock() - cam->anim_start;
		if (now > cam->anim_len) {
			cam->anim_len = 0;
			cam->jumping = 0;
			cam->flags |= CAM_IS_DIRTY;
			return 1;
		}
		frac = FLT2FIX ( ((Float) now) / cam->anim_len);
		if (frac>FIX_ONE / 2) frac = FIX_ONE - frac;
		cam->dheight = m4_mulfix(cam->avatar_size.y, frac);
		cam->position.y += cam->dheight;
		cam->target.y += cam->dheight;
		cam->flags |= CAM_IS_DIRTY;
		return 1;
	}

	if (!cam->anim_start) {
		cam->anim_start = M4_GetSysClock();
		now = 0;
		frac = 0;
	} else {
		now = M4_GetSysClock() - cam->anim_start;
		if (now > cam->anim_len) {
			cam->anim_len = 0;
			if (cam->is_3D) {
				camera_set_vectors(cam, cam->end_pos, cam->end_ori, cam->end_fov);
			} else {
				cam->zoom = FIX_ONE;
				cam->trans.x = cam->trans.y = cam->rot.x = cam->rot.y = 0;
				cam->flags |= CAM_IS_DIRTY;
			}
			return 1;
		} else {
			frac = FLT2FIX( ((Float) now) / cam->anim_len);
		}
	}

	if (cam->is_3D) {
		SFVec3f pos, dif;
		SFRotation rot;
		Fixed fov;
		rot = Rotation_Interpolate(cam->start_ori, cam->end_ori, frac);
		m4_vec_diff(dif, cam->end_pos, cam->start_pos);
		dif = m4_vec_scale(dif, frac);
		m4_vec_add(pos, cam->start_pos, dif);
		fov = m4_mulfix(cam->end_fov - cam->start_fov, frac) + cam->start_fov;
		camera_set_vectors(cam, pos, rot, fov);
	} else {
		cam->zoom = m4_mulfix(FIX_ONE - cam->start_zoom, frac) + cam->start_zoom;
		frac = FIX_ONE - frac;
		cam->rot.x = m4_mulfix(frac, cam->start_rot.x);
		cam->rot.y = m4_mulfix(frac, cam->start_rot.y);
		cam->trans.x = m4_mulfix(frac, cam->start_trans.x);
		cam->trans.y = m4_mulfix(frac, cam->start_trans.y);
		cam->flags |= CAM_IS_DIRTY;
	}
	return 1;
}


SFVec3f camera_get_pos_dir(M4Camera *cam)
{
	SFVec3f v;
	m4_vec_diff(v, cam->position, cam->target);
	m4_vec_norm(&v);
	return v;
}
SFVec3f camera_get_target_dir(M4Camera *cam)
{
	SFVec3f v;
	m4_vec_diff(v, cam->target, cam->position);
	m4_vec_norm(&v);
	return v;
}
SFVec3f camera_get_right_dir(M4Camera *cam)
{
	SFVec3f v, pos;
	pos = camera_get_pos_dir(cam);
	v = m4_vec_cross(cam->up, pos );
	m4_vec_norm(&v);
	return v;
}


