/*
 * physics.c
 *
 * Calculate what happens
 *
 * Copyright (c) 2008 Thomas White <taw27@cam.ac.uk>
 *
 * This file is part of Thrust3D - a silly game
 *
 * Thrust3D 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 3 of the License, or
 * (at your option) any later version.
 *
 * Thrust3D 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 Thrust3D.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <SDL.h>
#include <math.h>
#include <sys/time.h>
#include <time.h>

#include "types.h"
#include "model.h"
#include "game.h"
#include "utils.h"
#include "audio.h"

/* Acceleration due to gravity in metres per millisecond per millisecond */
#define GRAVITY (9.81e-6)

/* Acceleration due to booster rocket, metres per ms per ms */
#define THRUST (GRAVITY*2.0)

/* Acceleration due to forwards/backwards thrusters, m per ms per ms */
#define FTHRUST (GRAVITY*0.5)

/* Air friction in (m per ms per ms) per (m per ms) */
#define FRICTION 0.001

/* Lander craft turning speed in radians per ms per ms */
#define YAWTHRUST (M_PI/1500000)

/* Conversion factor between friction and 'yawthrust' */
#define TORQUE 1.5

static int physics_point_is_inside_hull(double cx, double cy, double cz, double *fvert, int nfvert,
					double nx, double ny, double nz) {

	int i;
	double p1x, p1y, p1z;
	double p2x, p2y, p2z;
	
	p1x = fvert[3*0 + 0];
	p1y = fvert[3*0 + 1];
	p1z = fvert[3*0 + 2];
	
//	printf("Testing if (%5.2f %5.2f %5.2f) is inside plane: \n", cx, cy, cz);
//	for ( i=0; i<nfvert; i++ ) {
//		printf("%3i : %5.2f %5.2f %5.2f\n", i, fvert[3*i + 0], fvert[3*i + 1], fvert[3*i + 2]);
//	}
	
	for ( i=1; i<=nfvert; i++ ) {
		
		double lx, ly, lz;
		double qx, qy, qz;
		double px, py, pz;
		
		if ( i < nfvert ) {
			p2x = fvert[3*i + 0];
			p2y = fvert[3*i + 1];
			p2z = fvert[3*i + 2];
		} else {
			p2x = fvert[3*0 + 0];
			p2y = fvert[3*0 + 1];
			p2z = fvert[3*0 + 2];
		}
			
		/* Calculate 'left' vector = n ^ p1->p2 */
		qx = p2x-p1x;  qy = p2y-p1y;  qz = p2z-p1z;
		lx = ny*qz - nz*qy;
		ly = - nx*qz + nz*qx;
		lz = nx*qy - ny*qx;
		px = cx - p1x;  py = cy - p1y;  pz = cz - p1z;
		if ( px*lx + py*ly + pz*lz < 0.0 ) return 0;
		
		p1x = p2x;  p1y = p2y;  p1z = p2z;
		
	}
	
	return 1;

}

static int physics_will_collide_face(double sx, double sy, double sz, double vx, double vy, double vz,
					double nx, double ny, double nz, double *fvert, int nfvert, double dt,
					double *ttc) {

	double px, py, pz;
	double pdotn, sdotn, vdotn;
	double cx, cy, cz;
	double t;
	
	/* Range test */
	px = fvert[3*0 + 0];
	py = fvert[3*0 + 1];
	pz = fvert[3*0 + 2];
	
	pdotn = px*nx + py*ny + pz*nz;
	sdotn = sx*nx + sy*ny + sz*nz;
	vdotn = vx*nx + vy*ny + vz*nz;
	if ( vdotn == 0.0 ) return 0;		/* Collision happens infinitely far in the future */
	t = (pdotn - sdotn) / vdotn;
	
	if ( t <= 0.0 ) return 0;	/* Collided in the past */
	if ( t > dt ) return 0;		/* Not going to collide this step */
	
	/* Boundary test */
	cx = sx + vx * t;
	cy = sy + vy * t;
	cz = sz + vz * t;
	if ( physics_point_is_inside_hull(cx, cy, cz, fvert, nfvert, nx, ny, nz) == 1 ) {
		*ttc = t;
		return 1;
	}
	
	return 0;
	
}

/* Check for collision with all faces in a primitive */
static int physics_check_collide_all_faces(ModelInstance *obj, ModelInstance *other, double dt, int a,
						double sx, double sy, double sz,
						CollisionSpec *coll, Room *room, Game *game) {

	int found = 0;
	
	switch ( other->model->coll_primitives[a]->type ) {
	
		case PRIMITIVE_QUADS : {
			
			/* Faces are quads */
			int f;
		
			for ( f=0; f<other->model->coll_primitives[a]->num_vertices/4; f++ ) {
			
				double face[3*4];
				int q;
				double ttc;
				
				const double nx = other->model->coll_primitives[a]->normals[3*(4*f)+0];
				const double ny = other->model->coll_primitives[a]->normals[3*(4*f)+1];
				const double nz = other->model->coll_primitives[a]->normals[3*(4*f)+2];
				
				/* Skip if moving from the back to the front of this quad */
				if ( nx*obj->vx + ny*obj->vy + nz*obj->vz > 0.0 ) continue;
				
				for ( q=0; q<4; q++ ) {
					const double ox = other->x;
					const double oy = other->y;
					const double oz = other->z;
					face[3*q + 0] = ox + other->model->coll_primitives[a]->vertices[3*((4*f)+q) +0];
					face[3*q + 1] = oy + other->model->coll_primitives[a]->vertices[3*((4*f)+q) +1];
					face[3*q + 2] = oz + other->model->coll_primitives[a]->vertices[3*((4*f)+q) +2];
					face[3*q + 0] += 10.0*(room->rx - game->cur_room_x);
					face[3*q + 1] += 10.0*(room->ry - game->cur_room_y);
					face[3*q + 2] += 10.0*(room->rz - game->cur_room_z);
				}
				
				if ( physics_will_collide_face(obj->x+sx, obj->y+sy, obj->z+sz,
								obj->vx, obj->vy, obj->vz,
								nx, ny, nz, face, 4, dt, &ttc) != 0 ) {
					
					/* Update 'coll' if this collision happens sooner than the current best */
					if ( ttc < coll->ttc ) {
						coll->ttc = ttc;
						coll->nx = nx;
						coll->ny = ny;
						coll->nz = nz;
						coll->cx = obj->x + ttc*obj->vx;
						coll->cy = obj->y + ttc*obj->vy;
						coll->cz = obj->z + ttc*obj->vz;
						coll->obj = other;
						found = 1;
					}
					
				}
			}
			break;
			
		}
	
	}
	
	return found;
	
}

/* Return non-zero if 'obj' will collide with 'other' within 'dt' milliseconds */
static int physics_check_collide(ModelInstance *obj, ModelInstance *other, double dt, CollisionSpec *coll,
					Room *room, Game *game) {

	int i;
	int found = 0;
	
	/* Check all the vertices in the moving object... */
	for ( i=0; i<obj->model->num_coll_primitives; i++ ) {
		int j;
		for ( j=0; j<obj->model->coll_primitives[i]->num_vertices; j++ ) {
		
			int a;
			const double stx = obj->model->coll_primitives[i]->vertices[3*j+0];
			const double sty = obj->model->coll_primitives[i]->vertices[3*j+1];
			const double sx =  stx*cos(obj->yaw) + sty*sin(obj->yaw);
			const double sy = -stx*sin(obj->yaw) + sty*cos(obj->yaw);
			const double sz = obj->model->coll_primitives[i]->vertices[3*j+2];
			
			/* ...against all primitives in the static object */
			for ( a=0; a<other->model->num_coll_primitives; a++ ) {
				if ( physics_check_collide_all_faces(obj, other, dt, a, sx, sy, sz,
									coll, room, game) ) {
					found = 1;
				}
			}
			
		}
	}
	
	return found;
	
}

static int physics_collision_room(ModelInstance *obj, Game *game, double dt, CollisionSpec *coll,
					int rx, int ry, int rz) {

	Room *room;
	int found = 0;
	
	room = game_find_room(game, rx, ry, rz);	
	if ( room != NULL ) {
		int j;
		for ( j=0; j<room->num_objects; j++ ) {
			if ( room->objects[j] != NULL ) {
				if ( physics_check_collide(obj, room->objects[j], dt, coll, room, game) ) {
					found = 1;
				}
			}
		}
	}
	
	return found;

}

/* Find the earliest collision for 'obj'.  Fill out 'coll' and return 1 if any */
static int physics_find_earliest_collision(ModelInstance *obj, Game *game, double dt, CollisionSpec *coll) {

	int found = 0;
	int rx, ry, rz;
	
	rx = game->cur_room_x;
	ry = game->cur_room_y;
	rz = game->cur_room_z;
	if ( physics_collision_room(obj, game, dt, coll, rx, ry, rz) ) found = 1;
	
	if ( game->lander->x > 4.0 ) {
		if ( physics_collision_room(obj, game, dt, coll, rx+1, ry, rz) ) found = 1;
	}
	if ( game->lander->x < -4.0 ) {
		if ( physics_collision_room(obj, game, dt, coll, rx-1, ry, rz) ) found = 1;
	}
	if ( game->lander->y > 4.0 ) {
		if ( physics_collision_room(obj, game, dt, coll, rx, ry+1, rz) ) found = 1;
	}
	if ( game->lander->y < -4.0 ) {
		if ( physics_collision_room(obj, game, dt, coll, rx, ry-1, rz) ) found = 1;
	}
	if ( game->lander->z > 4.0 ) {
		if ( physics_collision_room(obj, game, dt, coll, rx, ry, rz-1) ) found = 1;
	}
	if ( game->lander->z < -4.0 ) {
		if ( physics_collision_room(obj, game, dt, coll, rx, ry, rz-1) ) found = 1;
	}
	
	return found;

}

/* Called once for each object which isn't just "scenery" */
static void physics_process(ModelInstance *obj, Uint32 dt, Game *game) {
	
	int collided = 0;
	
	/* Air friction */
	if ( obj->vx > 0.0 ) {
		obj->vx -= FRICTION * dt * obj->vx;
		if ( obj->vx < 0.0 ) obj->vx = 0.0;
	} else {
		obj->vx += FRICTION * dt * -obj->vx;
		if ( obj->vx > 0.0 ) obj->vx = 0.0;
	}
	
	if ( obj->vy > 0.0 ) {
		obj->vy -= FRICTION * dt * obj->vy;
		if ( obj->vy < 0.0 ) obj->vy = 0.0;
	} else {
		obj->vy += FRICTION * dt * -obj->vy;
		if ( obj->vy > 0.0 ) obj->vy = 0.0;
	}
	
	if ( obj->vz > 0.0 ) {
		obj->vz -= FRICTION * dt * obj->vz;
		if ( obj->vz < 0.0 ) obj->vz = 0.0;
	} else {
		obj->vz += FRICTION * dt * -obj->vz;
		if ( obj->vz > 0.0 ) obj->vz = 0.0;
	}
	
	if ( obj->yawspeed > 0.0 ) {
		obj->yawspeed -= FRICTION * dt * obj->yawspeed * TORQUE;
		if ( obj->yawspeed < 0.0 ) obj->yawspeed = 0.0;
	} else {
		obj->yawspeed += FRICTION * dt * -obj->yawspeed * TORQUE;
		if ( obj->yawspeed > 0.0 ) obj->yawspeed = 0.0;
	}
	
	/* Gravity */
	if ( (obj->attribs & OBJ_GRAVITY) && (!obj->landed) ) {
		obj->vz -= GRAVITY * dt;
	}
	
	/* Take a step, allowing for collisions and landing */
	double sttc = 0.0;
	do {
	
		CollisionSpec coll;
		coll.ttc = +HUGE_VAL;
		coll.nx = 0.0;  coll.ny = 0.0;  coll.nz = 0.0;
		coll.cx = 0.0;  coll.cy = 0.0;  coll.cz = 0.0;
		coll.obj = NULL;
		collided = physics_find_earliest_collision(obj, game, dt-sttc, &coll);
		
		if ( collided ) {
		
			/* Step forward to the point of collision */
			obj->x = coll.cx;
			obj->y = coll.cy;
			obj->z = coll.cz;
			
			sttc += coll.ttc;
			
			/* Can we land here? */
			if ( (coll.nx==0) && (coll.ny==0) && (coll.nz==1.0) ) {
			
				/* Yes - land (already moved to this position */
				
				if ( strcmp(coll.obj->model->name, "platform") == 0 ) {
				
					/* Landed on a platform */
					obj->recharging = 1;
					game->platform_rel_x = coll.obj->x - obj->x;
					game->platform_rel_y = coll.obj->y - obj->y;
					game->time_of_landing_event = game->time + sttc;
					
				} else {
				
					/* Landed on something that isn't safe */
					double modv = sqrt(obj->vx*obj->vx + obj->vy*obj->vy + obj->vz*obj->vz);
					audio_play(game->audio, "clang", modv/0.01, 0);
					game->radiation = 1.0;
				
				}
				
				obj->landed = 1;
				obj->vx = 0.0;
				obj->vy = 0.0;
				obj->vz = 0.0;
				obj->yawspeed = 0.0;
				
			} else {
			
				/* No - bounce */
				double modv = sqrt(obj->vx*obj->vx + obj->vy*obj->vy + obj->vz*obj->vz);
				audio_play(game->audio, "clang", modv/0.01, 0);
				obj->vx = -obj->vx;
				obj->vy = -obj->vy;
				obj->vz = -obj->vz;
				game->fuel -= 0.2;
				
			}
		
		} else {
			
			/* No further collision - perform the 'end step' */
			obj->x += obj->vx * (dt-sttc);
			obj->y += obj->vy * (dt-sttc);
			obj->z += obj->vz * (dt-sttc);
			obj->yaw += obj->yawspeed * (dt-sttc);
			
		}
	
	} while ( collided && !obj->landed );
	
	if ( obj->yaw < -M_PI ) obj->yaw += 2*M_PI;
	if ( obj->yaw > M_PI ) obj->yaw -= 2*M_PI;

}

void physics_step(Game *game, int dt) {

	struct timeval tv;
	suseconds_t us;
	time_t sec;
	
	gettimeofday(&tv, NULL);
	us = tv.tv_usec;
	sec = tv.tv_sec;
	
	/* Handle things specific to the lander craft */
	if ( game->thrusting ) {
		if ( game->fuel > 0.0 ) {
			game->lander->vz += THRUST * dt;
			game->fuel -= 0.00002 * dt;
			game->lander->landed = 0;
			game->radiation = 0.1;
			if ( game->lander->recharging ) {
				game->time_of_landing_event = game->time + dt;
				game->lander->recharging = 0;
			}
		}
	}
	if ( (game->radiation > 0.3) && (game->fuel > 0.0) ) {
		game->fuel -= 0.001*game->radiation*dt;
	}
	if ( game->lander->recharging ) {
		game->fuel += 0.0001*dt;
	}
	
	/* Compensate for laziness elsewhere */
	if ( game->fuel > 1.0 ) game->fuel = 1.0;
	if ( game->fuel < 0.0 ) game->fuel = 0.0;
	
	if ( game->fuel == 0.0 ) {
		printf("Explode!\n");
		game->fuel = 1.0;
	}
	
	if ( game->forward && !game->lander->landed ) {
		game->lander->vx += sinf(game->lander->yaw) * FTHRUST * dt;
		game->lander->vy += cosf(game->lander->yaw) * FTHRUST * dt;
	} else if ( game->reverse && !game->lander->landed ) {
		game->lander->vx -= sinf(game->lander->yaw) * FTHRUST * dt;
		game->lander->vy -= cosf(game->lander->yaw) * FTHRUST * dt;
	}
	if ( game->turn_left && !game->lander->landed ) {
		game->lander->yawspeed -= YAWTHRUST * dt;	/* -ve yaw is "left" */
	}
	if ( game->turn_right && !game->lander->landed ) {
		game->lander->yawspeed += YAWTHRUST * dt;	/* +ve yaw is "right" */
	}
	
	physics_process(game->lander, dt, game);
	
	game_check_handoff(game);
	
	gettimeofday(&tv, NULL);
	us = tv.tv_usec - us;
	sec = tv.tv_sec - sec;
	
	game->time_physics = us + sec*1e6;
	
}