/* 
 * PlanetPenguin Racer 
 * Copyright (C) 2004-2005 Volker Stroebel <volker@planetpenguin.de>
 *
 * Copyright (C) 1999-2001 Jasmin F. Patry
 * 
 * This program 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
 * of the License, or (at your option) any later version.
 * 
 * This program 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 this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

#include "course_load.h"
#include "course_render.h"
#include "hier.h"
#include "hier_util.h"
#include "tux.h"
#include "part_sys.h"
#include "phys_sim.h"
#include "nmrcl.h"
#include "course_mgr.h"
#include "stuff.h"

#include "ppogl/base/defs.h"

#include "loop.h"
#include "game_mgr.h"
#include "elements.h"

#include <iostream>

/*
 * Constants
 */
 
extern TerrainTex terrain_texture[NUM_TERRAIN_TYPES];
extern unsigned int num_terrains;

/// Tux's mass (kg) 
#define TUX_MASS 20

/// Width of Tux (m)
#define TUX_WIDTH 0.45

/// Minimum speed (m/s)
#define MIN_TUX_SPEED 1.4

/// Minimum speed for friction to take effect (m/s)
#define MIN_FRICTION_SPEED 2.8

/// Initial Tux speed (m/s)
#define INIT_TUX_SPEED 3.0

/// Maximum frictional force
#define MAX_FRICTIONAL_FORCE 800

/// Maximum angle by which frictional force is rotated when turning
#define MAX_TURN_ANGLE 45

/// Maximum turning force perpendicular to direction of movement
#define MAX_TURN_PERPENDICULAR_FORCE 400

/// Maximum increase in friction when turning
#define MAX_TURN_PENALTY 0.15

/// Force applied by Tux when braking (N)
#define BRAKE_FORCE 200

/// Maximum roll angle (degrees)
#define MAX_ROLL_ANGLE 30

/// Roll angle while braking (degrees)
#define BRAKING_ROLL_ANGLE 55

/// Baseline friction coeff for rolling
#define IDEAL_ROLL_FRIC_COEFF 0.35

/// Baseline speed for rolling (m/s)
#define IDEAL_ROLL_SPEED 6.0

/* Tux's orientation is updated using a first-order filter; these are the 
   time constants of the filter when tux is on ground and airborne (s) */
#define TUX_ORIENTATION_TIME_CONSTANT 0.14
#define TUX_ORIENTATION_AIRBORNE_TIME_CONSTANT 0.5

/// Max particles generated by turning (particles/s)
#define MAX_TURN_PARTICLES 1500

/// Max particles generated by rolling (particles/s)
#define MAX_ROLL_PARTICLES 3000

/// Particles generated by braking (particles/s)
#define BRAKE_PARTICLES 4000

/// Number of particles is multiplied by ( speed / PARTICLES_SPEED_FACTOR )
#define PARTICLE_SPEED_FACTOR 40

/// Maximum deflection angle for particles (out to side of player)
#define MAX_PARTICLE_ANGLE 80

/// Speed at which maximum deflection occurs
#define MAX_PARTICLE_ANGLE_SPEED 50

/// Particle speed = min ( MAX_PARTICLE_SPEED, speed*PARTICLE_SPEED_MULTIPLIER )
#define PARTICLE_SPEED_MULTIPLIER 0.3

/// Max speed of particles
#define MAX_PARTICLE_SPEED 2

/// The compressiblity of the first section of the spring modelling Tux's tush (m)
#define TUX_GLUTE_STAGE_1_COMPRESSIBILITY 0.05

/// The spring coefficient of first section of aforementioned tush (N/m)
#define TUX_GLUTE_STAGE_1_SPRING_COEFF 1500

/// The damping coefficient of first section of aforementioned tush (N*s/m)
#define TUX_GLUTE_STAGE_1_DAMPING_COEFF 100

/// The compressiblity of the second section of the spring modelling Tux's tush (m)
#define TUX_GLUTE_STAGE_2_COMPRESSIBILITY 0.12

/// The spring coefficient of second section of aforementioned tush (N/m)
#define TUX_GLUTE_STAGE_2_SPRING_COEFF 3000

/// The damping coefficient of second section of aforementioned tush (N*s/m)
#define TUX_GLUTE_STAGE_2_DAMPING_COEFF 500

/// The spring coefficient of third section of aforementioned tush (N/m)
#define TUX_GLUTE_STAGE_3_SPRING_COEFF 10000

/// The damping coefficient of third section of aforementioned tush (N*s/m)
#define TUX_GLUTE_STAGE_3_DAMPING_COEFF 1000

/// The maximum force exertedd by both sections of spring (N)
#define TUX_GLUTE_MAX_SPRING_FORCE 3000

/// Maximum distance that player can penetrate into terrain (m)
#define MAX_SURFACE_PENETRATION 0.2

/// Density of air @ -3 C (kg/m^3)
#define AIR_DENSITY 1.308

/// Diameter of sphere used to model Tux for air resistance calcs (m)
#define TUX_DIAMETER TUX_WIDTH

/// Viscosity of air @ -3 C (N*s/m^2)
#define AIR_VISCOSITY 17.00e-6

/** Drag coefficient vs. Reynolds number table (from Janna, _Introduction to
   Fluid Mechanics_, 3rd Edition, PWS-Kent, p 308) 
   Values are for Re in the range of 10^-1 to 10^6, spaced logarithmically
   (all values are log base 10) */
static const double air_log_re[] = { -1,
				     0,
				     1,
				     2,
				     3,
				     4,
				     5,
				     6 };
static const double air_log_drag_coeff[] = { 2.25,
					     1.35,
					     0.6,
					     0.0,
					     -0.35,
					     -0.45,
					     -0.33,
					     -0.9 };

/// Minimum time step for ODE solver (s)
#define MIN_TIME_STEP 0.01

/// Maximum time step for ODE solver (s)
#define MAX_TIME_STEP 0.10

/// Maximum distance of step for ODE solver (m)
#define MAX_STEP_DISTANCE 0.20

/// Tolerance on error in Tux's position (m)
#define MAX_POSITION_ERROR 0.005

/// Tolerance on error in Tux's velocity (m/s)
#define MAX_VELOCITY_ERROR 0.05

/** To smooth out the terrain, the terrain normals are interpolated
   near the edges of the triangles.  This parameter controls how much
   smoothing is done, as well as the size of the triangle boundary
   over which smoothing occurs.  A value of 0 will result in no
   smoothing, while a value of 0.5 will smooth the normals over the
   entire surface of the triangle (though the smoothing will increase
   as the value approaches infinity).  (dimensionless) */
#define NORMAL_INTERPOLATION_LIMIT 0.05

/// The distance to move Tux up by so that he doesn't sit too low on the ground (m)
#define TUX_Y_CORRECTION_ON_STOMACH 0.33

/// The square of the distance that Tux must move before we recompute a collision (m^2)
#define COLLISION_TOLERANCE 0.04

/// Duraction of paddling motion (s)
#define PADDLING_DURATION 0.40

/// Force applied against ground when paddling (N)
#define MAX_PADDLING_FORCE 122.5

/// Ideal paddling friction coefficient
#define IDEAL_PADDLING_FRIC_COEFF 0.35

/// G force applied for jump with charge of 0
#define BASE_JUMP_G_FORCE 1.5

/// Maximum G force applied during jump
#define MAX_JUMP_G_FORCE 3

/// Magnitude of force before damage is incurred (N)
#define DAMAGE_RESISTANCE ( 4.0 * TUX_MASS * EARTH_GRAV )

/// Damage scaling factor (health/(N*s))
#define DAMAGE_SCALE 9e-8

/// Amount to scale tree force by (so that it does more damage than regular collisions) (dimensionless)
#define COLLISION_BASE_DAMAGE 0.02

/// Damage incurred by paddling exertion (health/s)
#define PADDLING_DAMAGE 0.02

/// Wind velocity
static ppogl::Vec3d wind_vel(250.0/3.6, 0, 0);
static double wind_scale = 0.5;

/*
 * Static variables
 */

/// Current time step in ODE solver
static double ode_time_step = -1;

class Index2d
{
public:
	Index2d(){};
	Index2d(const int i, const int j):i(i),j(j){}
	int i,j;
};	

void
set_wind_velocity(ppogl::Vec3d velocity, float scale)
{
	wind_vel = velocity;
	wind_scale = scale;							
}

void
increment_turn_fact(Player& plyr, double amt)
{
    plyr.control.turn_fact += amt;
    plyr.control.turn_fact = MIN( 1.0, MAX( plyr.control.turn_fact, -1.0 ) );
}

double
get_min_tux_speed()
{
    return MIN_TUX_SPEED;   
}

float
get_min_y_coord() 
{
    float courseWidth, courseLength;
    Course::getDimensions( &courseWidth, &courseLength );
	
    float angle = Course::getAngle();
    float minY = -courseLength * tan( ANGLES_TO_RADIANS( angle ) );
    
	return minY;
} 

void
get_indices_for_point(double x, double z, 
			    int *x0, int *y0, int *x1, int *y1)
{
    float courseWidth, courseLength;
    float xidx, yidx;
    int nx, ny;

	Course::getDimensions( &courseWidth, &courseLength );
    Course::getDivisions( &nx, &ny );
    
    xidx = x / courseWidth * ( float(nx) - 1. );
    yidx = -z / courseLength * ( float(ny) - 1. );

    if (xidx < 0) {
        xidx = 0;
    } else if ( xidx > nx-1 ) {
        xidx = nx-1;
    } 

    if (yidx < 0) {
        yidx = 0;
    } else if ( yidx > ny-1 ) {
        yidx = ny-1;
    } 

    /* I found that ceil(3) was quite slow on at least some architectures, 
       so I've replace it with an approximation */
    *x0 = int(xidx);              /* floor(xidx) */
    *x1 = int( xidx + 0.9999 );   /* ceil(xidx) */
    *y0 = int(yidx);              /* floor(yidx) */
    *y1 = int( yidx + 0.9999 );   /* ceil(yidx) */

    if(*x0 == *x1){
		if(*x1 < nx - 1){
	    	(*x1)++;
		}else{ 
	    	(*x0)--;
		}
    } 

    if(*y0 == *y1){
		if(*y1 < ny - 1){
	    	(*y1)++;
		}else{ 
	   		(*y0)--;
		}
    }

    PP_ENSURE( *x0 >= 0, "invalid x0 index" );
    PP_ENSURE( *x0 < nx, "invalid x0 index" );
    PP_ENSURE( *x1 >= 0, "invalid x1 index" );
    PP_ENSURE( *x1 < nx, "invalid x1 index" );
    PP_ENSURE( *y0 >= 0, "invalid y0 index" );
    PP_ENSURE( *y0 < ny, "invalid y0 index" );
    PP_ENSURE( *y1 >= 0, "invalid y1 index" );
    PP_ENSURE( *y1 < ny, "invalid y1 index" );
}


void
find_barycentric_coords(float x, float z, 
			      Index2d *idx0, 
			      Index2d *idx1, 
			      Index2d *idx2,
			      float *u, float *v)
{
    float xidx, yidx;
    int nx, ny;
    int x0, x1, y0, y1;
    float dx, ex, dz, ez, qx, qz, invdet; // to calc. barycentric coords 
    float courseWidth, courseLength;
    float *elevation;

    elevation = Course::getElevData();
    Course::getDimensions( &courseWidth, &courseLength );
    Course::getDivisions( &nx, &ny );

    get_indices_for_point( x, z, &x0, &y0, &x1, &y1 );
    
    xidx = x / courseWidth * ( float(nx) - 1. );
    yidx = -z / courseLength * ( float(ny) - 1. );


    /* The terrain is meshed as follows:
	      ...
	   +-+-+-+-+            x<---+
	   |\|/|\|/|                 |
        ...+-+-+-+-+...              V
	   |/|\|/|\|                 y
	   +-+-+-+-+
	      ...
       
       So there are two types of squares: those like this (x0+y0 is even):

       +-+
       |/|
       +-+

       and those like this (x0+y0 is odd).
       
       +-+
       |\|
       +-+
    */


    if ( (x0 + y0) % 2 == 0 ) {
	if ( yidx - y0 < xidx - x0 ) {
	    *idx0 = Index2d(x0, y0); 
	    *idx1 = Index2d(x1, y0); 
	    *idx2 = Index2d(x1, y1); 
	} else {
	    *idx0 = Index2d(x1, y1); 
	    *idx1 = Index2d(x0, y1); 
	    *idx2 = Index2d(x0, y0); 
	} 
    } else {
	// x0 + y0 is odd 
	if ( yidx - y0 + xidx - x0 < 1 ) {
	    *idx0 = Index2d(x0, y0); 
	    *idx1 = Index2d(x1, y0); 
	    *idx2 = Index2d(x0, y1); 
	} else {
	    *idx0 = Index2d(x1, y1); 
	    *idx1 = Index2d(x0, y1); 
	    *idx2 = Index2d(x1, y0); 
	} 
    }

    dx = idx0->i - idx2->i;
    dz = idx0->j - idx2->j;
    ex = idx1->i - idx2->i;
    ez = idx1->j - idx2->j;
    qx = xidx - idx2->i;
    qz = yidx - idx2->j;

    invdet = 1./(dx * ez - dz * ex);
    *u = (qx * ez - qz * ex) * invdet;
    *v = (qz * dx - qx * dz) * invdet;
}


ppogl::Vec3d
find_course_normal(const float x, const float z)
{
    ppogl::Vec3d *course_nmls;
    ppogl::Vec3d tri_nml;
    float xidx, yidx;
    int nx, ny;
    int x0, x1, y0, y1;
    ppogl::Vec3d p0, p1, p2;
    Index2d idx0, idx1, idx2;
    ppogl::Vec3d n0, n1, n2;
    float course_width, course_length;
    float u, v;
    float min_bary, interp_factor;
    ppogl::Vec3d smooth_nml;
    ppogl::Vec3d interp_nml;
    float *elevation;

    elevation = Course::getElevData();
    Course::getDimensions( &course_width, &course_length );
    Course::getDivisions( &nx, &ny );
    course_nmls = get_course_normals();

    get_indices_for_point( x, z, &x0, &y0, &x1, &y1 );
    
    xidx = x / course_width * ( float(nx) - 1. );
    yidx = -z / course_length * ( float(ny) - 1. );

    find_barycentric_coords( x, z, &idx0, &idx1, &idx2, &u, &v );

    n0 = course_nmls[ idx0.i + nx * idx0.j ];
    n1 = course_nmls[ idx1.i + nx * idx1.j ];
    n2 = course_nmls[ idx2.i + nx * idx2.j ];

    p0 = COURSE_VERTEX( idx0.i, idx0.j );
    p1 = COURSE_VERTEX( idx1.i, idx1.j );
    p2 = COURSE_VERTEX( idx2.i, idx2.j );
    
    smooth_nml = (u*n0)+(v*n1+( 1.-u-v)*n2);

    tri_nml = (p1-p0)^(p2-p0);
    tri_nml.normalize();

    min_bary = MIN( u, MIN( v, 1. - u - v ) );
    interp_factor = MIN( min_bary / NORMAL_INTERPOLATION_LIMIT, 1.0 );

    interp_nml = (interp_factor*tri_nml)+
	((1.-interp_factor)*smooth_nml);
    interp_nml.normalize();

    return interp_nml;
}

float
find_y_coord(float x, float z)
{
    float ycoord;
    ppogl::Vec3d p0, p1, p2;
    Index2d idx0, idx1, idx2;
    float u, v;
    int nx, ny;
    float *elevation;
    float course_width, course_length;

    /* cache last request */
    static float last_x, last_z, last_y;
    static bool cache_full = false;

    if ( cache_full && last_x == x && last_z == z ) {
	return last_y;
    }

	Course::getDivisions( &nx, &ny );
    Course::getDimensions( &course_width, &course_length );
    elevation = Course::getElevData();

    find_barycentric_coords( x, z, &idx0, &idx1, &idx2, &u, &v );

    p0 = COURSE_VERTEX( idx0.i, idx0.j );
    p1 = COURSE_VERTEX( idx1.i, idx1.j );
    p2 = COURSE_VERTEX( idx2.i, idx2.j );

    ycoord = u * p0.y() + v * p1.y() + ( 1. - u - v ) * p2.y();

    last_x = x;
    last_z = z;
    last_y = ycoord;
    cache_full = true;

    return ycoord;
} 

void
get_surface_type(float x, float z, float weights[])
{
    int *terrain;
    float courseWidth, courseLength;
    int nx, ny;
    Index2d idx0, idx1, idx2;
    float u, v;
    unsigned int i;

    find_barycentric_coords( x, z, &idx0, &idx1, &idx2, &u, &v );

    terrain = Course::getTerrainData();
    Course::getDimensions( &courseWidth, &courseLength );
    Course::getDivisions( &nx, &ny );

    for (i=0; i<num_terrains; i++) {
	weights[i] = 0;
	if ( terrain[ idx0.i + nx*idx0.j ] == int(i) ) {
	    weights[i] += u;
	}
	if ( terrain[ idx1.i + nx*idx1.j ] == int(i) ) {
	    weights[i] += v;
	}
	if ( terrain[ idx2.i + nx*idx2.j ] == int(i) ) {
	    weights[i] += 1.0 - u - v;
	}
    }
} 

pp::Plane
get_local_course_plane(ppogl::Vec3d pt)
{
    pp::Plane plane;

    pt.y() = find_y_coord( pt.x(), pt.z() );

    plane.nml = find_course_normal( pt.x(), pt.z() );
    plane.d = -( plane.nml.x() * pt.x() + 
		 plane.nml.y() * pt.y() +
		 plane.nml.z() * pt.z() );

    return plane;
}

static void
update_paddling(Player& plyr)
{
    if ( plyr.control.is_paddling ) {
		if ( GameMgr::getInstance().time - plyr.control.paddle_time >= PADDLING_DURATION ) {
		    PP_LOG( DEBUG_CONTROL, "paddling off" );
		    plyr.control.is_paddling = false;
		}
    }
}

void
set_tux_pos(Player& plyr, ppogl::Vec3d new_pos)
{
    float playWidth, playLength;
    float courseWidth, courseLength;
    float boundaryWidth;
    float disp_y;

    Course::getPlayDimensions( &playWidth, &playLength );
    Course::getDimensions( &courseWidth, &courseLength );
    boundaryWidth = ( courseWidth - playWidth ) / 2; 


    if ( new_pos.x() < boundaryWidth ) {
        new_pos.x() = boundaryWidth;
    } else if ( new_pos.x() > courseWidth - boundaryWidth ) {
        new_pos.x() = courseWidth - boundaryWidth;
    } 

    if ( new_pos.z() > 0 ) {
        new_pos.z() = 0;
    } else if ( -new_pos.z() >= playLength ) {
        new_pos.z() = -playLength;
        GameMode::setMode(GameMode::GAME_OVER);
    } 

    plyr.pos = new_pos;

    disp_y = new_pos.y() + TUX_Y_CORRECTION_ON_STOMACH; 

    const std::string& tuxRoot = tux[plyr.num].getRootNode();
    reset_scene_node( tuxRoot );
    translate_scene_node( tuxRoot, 
			  ppogl::Vec3d( new_pos.x(), disp_y, new_pos.z() ) );
} 

bool
check_model_collisions(Player& plyr, ppogl::Vec3d pos, 
			      ppogl::Vec3d *tree_loc, float *tree_diam)
{
    if(GameConfig::disableCollisionDetection){
		return false;
	}	
	
	if(modelLocs.empty()){
		//no models? what a boring course...
		return false;
	}
		
	ppogl::Polyhedron *ph, ph2;
    pp::Matrix mat;
    bool hit = false;
    ppogl::Vec3d distvec;
    float squared_dist;
        
    /* These variables are used to cache collision detection results */
    static bool last_collision = false;
    static ppogl::Vec3d last_collision_tree_loc( -999, -999, -999 );
    static double last_collision_tree_diam = 0;
    static ppogl::Vec3d last_collision_pos( -999, -999, -999 );

    /* If we haven't moved very much since the last call, we re-use
       the results of the last call (significant speed-up) */
	   
	  
    ppogl::Vec3d dist_vec = pos - last_collision_pos;
    if ( dist_vec.length2() < COLLISION_TOLERANCE ) {
	if ( last_collision ) {
	    if ( tree_loc != NULL ) {
		*tree_loc = last_collision_tree_loc;
	    }
	    if ( tree_diam != NULL ) {
		*tree_diam = last_collision_tree_diam;
	    }
	    return true;
	} else {
	    return false;
	}
    }

	ppogl::RefPtr<ModelType> model_type = (*modelLocs.begin()).getType();
    ph = model_type->ph;

	float diam=0.0;
    float height;
	ppogl::Vec3d loc;

	std::list<Model>::iterator it;

    for(it=modelLocs.begin();it!=modelLocs.end();it++) {
		diam = (*it).getDiameter(); 
    	height = (*it).getHeight();
		loc = (*it).getPosition();

        distvec = ppogl::Vec3d( loc.x() - pos.x(), 0.0, loc.z() - pos.z() );

	/* check distance from tree; .6 is the radius of a bounding
           sphere around tux (approximate) */
	squared_dist = ( diam/2. + 0.6 );
	squared_dist *= squared_dist;
        if ( distvec.length2() > squared_dist ) {
            continue;
        } 

	/* have to look at polyhedron - switch to correct one if necessary */
	if ( model_type != ((*it).getType()) )  {
	    model_type = (*it).getType();
	    ph = model_type->ph;
	}
        ph2 = copy_polyhedron( *ph );
	
        //mat.makeScaling( diam, height, diam );
		mat.makeScaling( diam, diam, diam );
	
        trans_polyhedron( mat, ph2 );
        mat.makeTranslation( loc.x(), loc.y(), loc.z() );
        trans_polyhedron( mat, ph2 );

	const std::string& tux_root = tux[plyr.num].getRootNode();
	reset_scene_node( tux_root );
	translate_scene_node( tux_root, 
			      ppogl::Vec3d( pos.x(), pos.y(), pos.z() ) );
        hit = collide( tux_root, ph2 );
		
        free_polyhedron( ph2 );

        if ( hit == true ) {
	    if ( tree_loc != NULL ) {
		*tree_loc = loc;
	    }
	    if ( tree_diam != NULL ) {
		*tree_diam = diam;
	    }


            break;
        } 
    } 

    last_collision_tree_loc = loc;
    last_collision_tree_diam = diam;
    last_collision_pos = pos;

    if ( hit ) {
	last_collision = true;

	/* Record collision in player data so that health can be adjusted */
	plyr.collision = true;
    } else {
	last_collision = false;
    }

    return hit;
} 

void
check_item_collection(Player& plyr, ppogl::Vec3d pos)
{
    float diam = 0.0; 
    float height;
    ppogl::Vec3d loc;
    ppogl::Vec3d distvec;
    float squared_dist;
    //int item_type;

    /* These variables are used to cache collision detection results */
    static ppogl::Vec3d last_collision_pos( -999, -999, -999 );

    /* If we haven't moved very much since the last call, we re-use
       the results of the last call (significant speed-up) */
    ppogl::Vec3d dist_vec = pos - last_collision_pos;
    if ( dist_vec.length2() < COLLISION_TOLERANCE ) {
		return ;
    }

    //items = get_item_locs();
    //num_items = get_num_items();
    
	//ItemType& item_type = itemLocs.type.begin();
	
	std::list<Item>::iterator it;	
    for(it=itemLocs.begin();it!=itemLocs.end();it++){
		if(!(*it).isCollectable() || (*it).isCollected()){
		    continue;
		}

        diam = (*it).getDiameter();
        height = (*it).getHeight();
        loc = (*it).getPosition();

        distvec = ppogl::Vec3d( loc.x() - pos.x(), 0.0, loc.z() - pos.z() );

		// check distance from tree; .6 is the radius of a bounding
        // sphere around tux (approximate)
		squared_dist = ( diam/2.0 + 0.6 );
		squared_dist *= squared_dist;
        if( distvec.length2() > squared_dist ){
            continue;
        } 

		if( (pos.y() - 0.6 >= loc.y() && pos.y() - 0.6 <= loc.y() + height) ||
			(pos.y() + 0.6 >= loc.y() && pos.y() + 0.6 <= loc.y() + height) ||
			(pos.y() - 0.6 <= loc.y() && pos.y() + 0.6 >= loc.y() + height) )
		{
	    	// close enough to hitting the flag
	    	// play a noise? 
	    	(*it).setCollected();
			(*it).setDrawable(false);	 
			 
			if((*it).getType()==ItemType::HERRING){
				plyr.herring += (*it).getScore();
				if (plyr.herring <0) plyr.herring=0;				
			}else{
				plyr.incLives();			
			}

			ppogl::AudioMgr::getInstance().playSound("item_collect",0);
		}
    }
} 

float
get_compression_depth(const int terrain) 
{
	return terrain_texture[terrain].compression;
}

/*
 * Check for tree collisions and adjust position and velocity appropriately.
 */
static void
adjust_for_model_collision( Player& plyr, 
				       ppogl::Vec3d pos, ppogl::Vec3d *vel )
{
    ppogl::Vec3d modelNml;
    ppogl::Vec3d modelLoc;
    bool modelHit;
    float speed;
    float costheta;
    float modelDiam;

    modelHit = check_model_collisions( plyr, pos, &modelLoc, &modelDiam );
    if (modelHit) {
	/*
	 * Calculate the normal vector to the tree; here we model the tree
	 * as a vertical cylinder.
	 */
        modelNml.x() = pos.x() - modelLoc.x();
        modelNml.y() = 0;
        modelNml.z() = pos.z() - modelLoc.z(); 
        modelNml.normalize();

	/* Reduce speed by a minimum of 30% */
        speed = vel->normalize();
        speed *= 0.7;

	/* 
	 * If Tux is moving into the tree, reduce the speed further, 
	 * and reflect the velocity vector off of the tree
	 */
        costheta = *vel*modelNml;
        if (costheta < 0 ) {
	    /* Reduce speed */
	    
            speed *= 1 + costheta;
            speed *= 1 + costheta; 

	    /* Do the reflection */
            *vel = ((-2. * ( *vel* modelNml ))*modelNml)+*vel;

	    vel->normalize();

        } 

	speed = MAX( speed, get_min_tux_speed() );
        *vel = speed*(*vel);
    } 
}

/*
 * Adjusts velocity so that his speed doesn't drop below the minimum 
 * speed
 */
double
adjust_velocity(ppogl::Vec3d *vel, pp::Plane surf_plane)
{
    ppogl::Vec3d surf_nml;
    float speed;

    surf_nml = surf_plane.nml;

    speed = vel->normalize();

    if ( speed < EPS )
    {
	if ( fabs( surf_nml.x() ) + fabs( surf_nml.z() ) > EPS ) {
	    *vel = projectIntoPlane( 
		surf_nml, ppogl::Vec3d( 0.0, -1.0, 0.0 ) );
	    vel->normalize();
	} else {
	    *vel = ppogl::Vec3d( 0.0, 0.0, -1.0 );
	}
    }

    speed = MAX( get_min_tux_speed(), speed );

    *vel = speed*(*vel);
    return speed;
}

void
adjust_position(ppogl::Vec3d *pos, pp::Plane surf_plane, 
		      float dist_from_surface)
{
    if(dist_from_surface < -MAX_SURFACE_PENETRATION){
		*pos = ( *pos+ (-MAX_SURFACE_PENETRATION - dist_from_surface)* surf_plane.nml);
    }
    return;
}

static ppogl::Vec3d
adjust_tux_zvec_for_roll(Player& plyr, 
					  ppogl::Vec3d vel, ppogl::Vec3d zvec)
{
    pp::Matrix rot_mat; 

    vel = projectIntoPlane( zvec, vel );

    vel.normalize();

    if ( plyr.control.is_braking ) {
	rot_mat.makeRotationAboutVector( vel, 
					   plyr.control.turn_fact *
					   BRAKING_ROLL_ANGLE );
    } else {
	rot_mat.makeRotationAboutVector( vel, 
					   plyr.control.turn_fact *
					   MAX_ROLL_ANGLE );
    }

    return rot_mat.transformVector(zvec);
}

static ppogl::Vec3d
adjust_surf_nml_for_roll(Player& plyr, 
					  ppogl::Vec3d vel, 
					  float fric_coeff,
					  ppogl::Vec3d nml)
{
    pp::Matrix rot_mat; 
    float angle;
    float speed;
    float roll_angle;

    speed = vel.normalize();

    vel = projectIntoPlane( nml, vel );

    vel.normalize();
    if ( plyr.control.is_braking ) {
	roll_angle = BRAKING_ROLL_ANGLE;
    } else {
	roll_angle = MAX_ROLL_ANGLE;
    }

	angle = plyr.control.turn_fact *
	roll_angle *
	MIN( 1.0, MAX(0.0, fric_coeff)/IDEAL_ROLL_FRIC_COEFF )
	* MIN(1.0, MAX(0.0,speed-MIN_TUX_SPEED)/
	      (IDEAL_ROLL_SPEED-MIN_TUX_SPEED));
    
    rot_mat.makeRotationAboutVector( vel, angle );

    return rot_mat.transformVector(nml);
}


void
adjust_orientation(Player& plyr, float dtime, ppogl::Vec3d vel,
			 float dist_from_surface, ppogl::Vec3d surf_nml)
{
    ppogl::Vec3d new_x, new_y, new_z; 
    pp::Matrix inv_cob_mat;
    pp::Matrix rot_mat;
    pp::Quat new_orient;
    float time_constant;
    static ppogl::Vec3d minus_z_vec(0., 0., -1.);
    static ppogl::Vec3d y_vec(0., 1., 0.);

    if ( dist_from_surface > 0 ) {
	new_y = 1.*vel;
	new_y.normalize();
	new_z = projectIntoPlane( new_y, ppogl::Vec3d(0., -1., 0.) );
	new_z.normalize();
	new_z = adjust_tux_zvec_for_roll( plyr, vel, new_z );
    } else { 
	new_z = -1.*surf_nml;
	new_z = adjust_tux_zvec_for_roll( plyr, vel, new_z );
	new_y = projectIntoPlane( surf_nml,1.*vel);
	new_y.normalize();
    }

    new_x = new_y^new_z;

	{
		pp::Matrix cob_mat;    
		pp::Matrix::makeChangeOfBasisMatrix( cob_mat, inv_cob_mat, new_x, new_y, new_z );
	    new_orient = pp::Quat(cob_mat);
	}
    if ( !plyr.orientation_initialized ) {
	plyr.orientation_initialized = true;
	plyr.orientation = new_orient;
    }

    time_constant = dist_from_surface > 0
	? TUX_ORIENTATION_AIRBORNE_TIME_CONSTANT
	: TUX_ORIENTATION_TIME_CONSTANT;

    plyr.orientation = pp::Quat::interpolate( 
	plyr.orientation, new_orient, 
	MIN( dtime / time_constant, 1.0 ) );

    plyr.plane_nml = plyr.orientation.rotate( minus_z_vec );
    plyr.direction = plyr.orientation.rotate( y_vec );

    pp::Matrix cob_mat( plyr.orientation );


    /* Trick rotations */
    new_y = ppogl::Vec3d( cob_mat.data[1][0], cob_mat.data[1][1], cob_mat.data[1][2] ); 
    rot_mat.makeRotationAboutVector( new_y, 
				       ( plyr.control.barrel_roll_factor * 360 ) );
    cob_mat=rot_mat*cob_mat;
    new_x = ppogl::Vec3d( cob_mat.data[0][0], cob_mat.data[0][1], cob_mat.data[0][2] ); 
    rot_mat.makeRotationAboutVector( new_x, 
				       plyr.control.flip_factor * 360 );
    cob_mat=rot_mat*cob_mat;



    inv_cob_mat.transpose(cob_mat);

    const std::string& tux_root = tux[plyr.num].getRootNode();
    transform_scene_node( tux_root, cob_mat, inv_cob_mat ); 
}

float
adjust_particle_count(float particles) 
{
    if ( particles < 1 ) {
	if ( ( float(rand()) ) / RAND_MAX < particles ) {
	    return 1.0;
	} else {
	    return 0.0;
	}
    } else {
	return particles;
    }
}

void
generate_particles(Player& plyr, float dtime, 
			 ppogl::Vec3d pos, float speed) 
{
    ppogl::Vec3d left_part_pt, right_part_pt;
    float brake_particles;
    float turn_particles;
    float roll_particles;
    float surf_weights[NUM_TERRAIN_TYPES]; 
    float surf_y;
    float left_particles, right_particles;
    ppogl::Vec3d left_part_vel, right_part_vel;
    pp::Matrix rot_mat;
    ppogl::Vec3d xvec;
	bool particles_type;
	GLuint particle_binding = 0;
	
	unsigned int i;
	
    get_surface_type( pos.x(), pos.z(), surf_weights );
    surf_y = find_y_coord( pos.x(), pos.z() );

	
	particles_type=false;
	for (i=0;i<num_terrains;i++){
		if(terrain_texture[i].particles){	
			if(surf_weights[i] > 0.5){
				particles_type=true;
				particle_binding = terrain_texture[i].particles->getID();
    		}
		}
	}
	
	
	
	
    if ( particles_type== true && pos.y() < surf_y ) {
	xvec =plyr.direction^plyr.plane_nml;

        right_part_pt = left_part_pt = pos;

	right_part_pt = right_part_pt + ((TUX_WIDTH/2.0)*xvec );

	left_part_pt = left_part_pt + ((-TUX_WIDTH/2.0)* xvec);

        right_part_pt.y() = left_part_pt.y()  = surf_y;

	brake_particles = dtime *
	    BRAKE_PARTICLES * ( plyr.control.is_braking ? 1.0 : 0.0 )
	    * MIN( speed / PARTICLE_SPEED_FACTOR, 1.0 );
	turn_particles = dtime * MAX_TURN_PARTICLES 
	    * MIN( speed / PARTICLE_SPEED_FACTOR, 1.0 );
	roll_particles = dtime * MAX_ROLL_PARTICLES 
	    * MIN( speed / PARTICLE_SPEED_FACTOR, 1.0 );

	left_particles = turn_particles * 
	    fabs( MIN(plyr.control.turn_fact, 0.) ) + 
	    brake_particles +
	    roll_particles * fabs( MIN(plyr.control.turn_animation, 0.) );

	right_particles = turn_particles * 
	    fabs( MAX(plyr.control.turn_fact, 0.) ) + 
	    brake_particles +
	    roll_particles * fabs( MAX(plyr.control.turn_animation, 0.) );

	left_particles = adjust_particle_count( left_particles );
	right_particles = adjust_particle_count( right_particles );

	/* Create particle velocitites */
	rot_mat.makeRotationAboutVector( plyr.direction,
	    MAX( -MAX_PARTICLE_ANGLE, 
		 -MAX_PARTICLE_ANGLE * speed / MAX_PARTICLE_ANGLE_SPEED ) );
	left_part_vel = rot_mat.transformVector(plyr.plane_nml);
	left_part_vel = MIN( MAX_PARTICLE_SPEED, 
					   speed * PARTICLE_SPEED_MULTIPLIER )*
				      left_part_vel;
	rot_mat.makeRotationAboutVector( plyr.direction,
	    MIN( MAX_PARTICLE_ANGLE, 
		 MAX_PARTICLE_ANGLE * speed / MAX_PARTICLE_ANGLE_SPEED ) );
	right_part_vel = rot_mat.transformVector( plyr.plane_nml );
	right_part_vel = MIN( MAX_PARTICLE_SPEED,
					    speed * PARTICLE_SPEED_MULTIPLIER )*
				       right_part_vel;

        create_new_particles( left_part_pt, left_part_vel, 
			      int(left_particles), particle_binding );
        create_new_particles( right_part_pt, right_part_vel, 
			      int(right_particles), particle_binding );
    } 
}

/*
 * Calculate the magnitude of force due to air resistance (wind)
 */
ppogl::Vec3d
calc_wind_force(ppogl::Vec3d player_vel)
{
    ppogl::Vec3d total_vel;
    float wind_speed;
    float re;         /* Reynolds number */
    float df_mag;     /* magnitude of drag force */
    float drag_coeff; /* drag coefficient */
    int table_size;      /* size of drag coeff table */

    static float last_time_called = -1;

    total_vel = -1*player_vel;
    
    if ( GameMgr::getInstance().getCurrentRace().windy ) {
	/* adjust wind_scale with a random walk */
	if ( last_time_called != GameMgr::getInstance().time ) {
	    wind_scale = wind_scale + 
		(rand()/double(RAND_MAX)-0.50) * 0.15;
	    wind_scale = MIN( 1.0, MAX( 0.0, wind_scale ) );
	}

	total_vel = total_vel+(wind_scale*wind_vel);
    }

    wind_speed = total_vel.normalize();

    re = AIR_DENSITY * wind_speed * TUX_DIAMETER / AIR_VISCOSITY;

    table_size = sizeof(air_log_drag_coeff) / sizeof(air_log_drag_coeff[0]);

    drag_coeff = pow( 10.0, 
		      lin_interp( air_log_re, air_log_drag_coeff, 
				  log10(re), table_size ) );

    df_mag = 0.5 * drag_coeff * AIR_DENSITY * ( wind_speed * wind_speed )
	* ( M_PI * ( TUX_DIAMETER * TUX_DIAMETER ) * 0.25 );

    PP_ENSURE( df_mag > 0, "Negative wind force" );

    last_time_called = GameMgr::getInstance().time;

    return df_mag*total_vel;
}

static ppogl::Vec3d
calc_spring_force(float compression, ppogl::Vec3d vel, 
				  ppogl::Vec3d surf_nml, ppogl::Vec3d *unclamped_f)
{
    float spring_vel; /* velocity perp. to surface (for damping) */
    float spring_f_mag; /* magnitude of force */

    PP_REQUIRE( compression >= 0, 
		    "spring can't have negative compression" );
    
    spring_vel =vel*surf_nml;

    /* Stage 1 */
    spring_f_mag = 
	MIN( compression, TUX_GLUTE_STAGE_1_COMPRESSIBILITY ) 
	* TUX_GLUTE_STAGE_1_SPRING_COEFF;

    /* Stage 2 */
    spring_f_mag += 
	MAX( 0, MIN( compression - TUX_GLUTE_STAGE_1_COMPRESSIBILITY, 
		     TUX_GLUTE_STAGE_2_COMPRESSIBILITY ) )
	* TUX_GLUTE_STAGE_2_SPRING_COEFF;

    /* Stage 3 */
    spring_f_mag += 
	MAX( 0., compression - TUX_GLUTE_STAGE_2_COMPRESSIBILITY - 
	     TUX_GLUTE_STAGE_1_COMPRESSIBILITY ) 
	* TUX_GLUTE_STAGE_3_SPRING_COEFF;

    /* Damping */
    spring_f_mag -= spring_vel * ( 
	compression <= TUX_GLUTE_STAGE_1_COMPRESSIBILITY 
	? TUX_GLUTE_STAGE_1_SPRING_COEFF : 
	( compression <= TUX_GLUTE_STAGE_2_COMPRESSIBILITY 
	  ? TUX_GLUTE_STAGE_2_DAMPING_COEFF
	  : TUX_GLUTE_STAGE_3_DAMPING_COEFF ) );

    /* Clamp to >= 0.0 */
    spring_f_mag = MAX( spring_f_mag, 0.0 );

    if ( unclamped_f != NULL ) {
	*unclamped_f = spring_f_mag*surf_nml;
    }

    /* Clamp to <= TUX_GLUTE_MAX_SPRING_FORCE */
    spring_f_mag = MIN( spring_f_mag, TUX_GLUTE_MAX_SPRING_FORCE );

    return spring_f_mag*surf_nml;
}


static
ppogl::Vec3d calc_net_force(Player& plyr, ppogl::Vec3d pos, 
				ppogl::Vec3d vel)
{
    ppogl::Vec3d nml_f;      /* normal force */
    ppogl::Vec3d unclamped_nml_f; /* unclamped normal force (for damage calc) */
    ppogl::Vec3d fric_f;     /* frictional force */
    float fric_f_mag; /* frictional force magnitude */
    ppogl::Vec3d fric_dir;   /* direction of frictional force */
    ppogl::Vec3d grav_f;     /* gravitational force */
    ppogl::Vec3d air_f;      /* air resistance force */
    ppogl::Vec3d brake_f;    /* braking force */
    ppogl::Vec3d paddling_f; /* paddling force */
    ppogl::Vec3d net_force;  /* the net force (sum of all other forces) */
    float comp_depth; /* depth to which the terrain can be compressed */
    float speed;      /* speed (m/s) */
    ppogl::Vec3d orig_surf_nml; /* normal to terrain at current position */
    ppogl::Vec3d surf_nml;   /* normal to terrain w/ roll effect */
    float glute_compression; /* amt that Tux's tush has been compressed */
    float steer_angle; /* Angle to rotate fricitonal force for turning */
    pp::Matrix fric_rot_mat; /* Matrix to rotate frictional force */
    ppogl::Vec3d jump_f;
    pp::Plane surf_plane;
    float dist_from_surface;
    float surf_weights[NUM_TERRAIN_TYPES];
    float surf_fric_coeff;
    unsigned int i;

    get_surface_type( pos.x(), pos.z(), surf_weights );
    surf_plane = get_local_course_plane( pos );
    orig_surf_nml = surf_plane.nml;

    surf_fric_coeff = 0;
    for (i=0; i<num_terrains; i++) {
	/*
	surf_fric_coeff += surf_weights[i] * fricCoeff[terrain_texture[i].type];
	*/
	surf_fric_coeff += surf_weights[i] * terrain_texture[i].friction;
    }
    surf_nml = adjust_surf_nml_for_roll( plyr, vel, surf_fric_coeff,
					 orig_surf_nml );
    
    comp_depth = 0;
    for (i=0; i<num_terrains; i++) {
	comp_depth += surf_weights[i] * get_compression_depth(i);
    }


    grav_f = ppogl::Vec3d( 0, -EARTH_GRAV * TUX_MASS, 0 );

    dist_from_surface = surf_plane.distance( pos );

    if ( dist_from_surface <= 0 ) {
	plyr.airborne = false;
    } else {
	plyr.airborne = true;
    }

    /*
     * Calculate normal force
     */
    nml_f = ppogl::Vec3d( 0., 0., 0. );
    if ( dist_from_surface <= -comp_depth ) {
	/*
	 * Tux ended up below the surface.
	 * Calculate the spring force exterted by his rear end. :-)
	 */
	glute_compression = -dist_from_surface - comp_depth;
	PP_ASSERT( glute_compression >= 0, 
			 "unexpected negative compression" );

	nml_f = calc_spring_force( glute_compression, vel, surf_nml,
				   &unclamped_nml_f );
    }

    /* Check if player is trying to jump */
    if ( plyr.control.begin_jump == true ) {
	plyr.control.begin_jump = false;
	if ( dist_from_surface <= 0 ) {
	    plyr.control.jumping = true;
	    plyr.control.jump_start_time = GameMgr::getInstance().time;
	} else {
	    plyr.control.jumping = false;
	}
    }


    /* Apply jump force in up direction for JUMP_FORCE_DURATION */
    if ( ( plyr.control.jumping ) &&
	 ( GameMgr::getInstance().time - plyr.control.jump_start_time < 
	   JUMP_FORCE_DURATION ) ) 
    {
	jump_f = ppogl::Vec3d( 
	    0, 
	    BASE_JUMP_G_FORCE * TUX_MASS * EARTH_GRAV + 
	    plyr.control.jump_amt * 
	    (MAX_JUMP_G_FORCE-BASE_JUMP_G_FORCE) * TUX_MASS * EARTH_GRAV, 
	    0 );

    } else {
	jump_f = ppogl::Vec3d( 0, 0, 0 );
	plyr.control.jumping = false;
    }

    /* Use the unclamped normal force for damage calculation purposes */
    plyr.normal_force = unclamped_nml_f;

    /* 
     * Calculate frictional force
     */
    fric_dir = vel;
    speed = fric_dir.normalize();
    fric_dir = -1.0*fric_dir;
    
    if ( dist_from_surface < 0 && speed > MIN_FRICTION_SPEED ) {
	ppogl::Vec3d tmp_nml_f = nml_f;

	fric_f_mag = tmp_nml_f.normalize() * surf_fric_coeff;

	fric_f_mag = MIN( MAX_FRICTIONAL_FORCE, fric_f_mag );

	fric_f = fric_f_mag*fric_dir;


	/*
	 * Adjust friction for steering
	 */
	steer_angle = plyr.control.turn_fact * MAX_TURN_ANGLE;

	if ( fabs( fric_f_mag * sin( ANGLES_TO_RADIANS( steer_angle ) ) ) >
	     MAX_TURN_PERPENDICULAR_FORCE ) 
	{
	    //check_assertion( fabs( plyr->control.turn_fact ) > 0,
		//	     "steer angle is non-zero when player is not "
		//	     "turning" );
	    steer_angle = RADIANS_TO_ANGLES( 
		asin( MAX_TURN_PERPENDICULAR_FORCE / fric_f_mag ) ) * 
		plyr.control.turn_fact / fabs( plyr.control.turn_fact );
	}
	fric_rot_mat.makeRotationAboutVector( orig_surf_nml, steer_angle );
	fric_f = fric_rot_mat.transformVector( fric_f );
	fric_f = (1.0 + MAX_TURN_PENALTY)*fric_f;


	/*
	 * Calculate braking force
	 */
	if ( speed > get_min_tux_speed() && plyr.control.is_braking ) {
	    brake_f = (surf_fric_coeff * BRAKE_FORCE)*fric_dir; 
	} else {
	    brake_f = ppogl::Vec3d( 0., 0., 0. );
	}
	
    } else {
	fric_f = brake_f = ppogl::Vec3d( 0., 0., 0. );
    }

    /*
     * Calculate air resistance
     */
    air_f = calc_wind_force( vel );


    /*
     * Calculate force from paddling
     */
    update_paddling( plyr );
    if ( plyr.control.is_paddling ) {
	if ( plyr.airborne ) {
	    paddling_f = ppogl::Vec3d( 0, 0, -TUX_MASS * EARTH_GRAV / 4.0 );
	    paddling_f = plyr.orientation.rotate( paddling_f );
	} else {
	    paddling_f = ( 
		-1 * MIN( MAX_PADDLING_FORCE,  
			  MAX_PADDLING_FORCE * 
			  ( MAX_PADDLING_SPEED - speed ) / MAX_PADDLING_SPEED * 
			  MIN(1.0, 
			      surf_fric_coeff/IDEAL_PADDLING_FRIC_COEFF)))*
		fric_dir;
	}
    } else {
	paddling_f = ppogl::Vec3d( 0., 0., 0. );
    }

    
    /*
     * Add all the forces 
     */
    net_force = jump_f+grav_f+nml_f+fric_f+air_f+brake_f+paddling_f;

    return net_force;
}

static float
adjust_time_step_size(float h, ppogl::Vec3d vel)
{
    float speed;

    speed = vel.normalize();

    h = MAX( h, MIN_TIME_STEP );
    h = MIN( h, MAX_STEP_DISTANCE / speed );
    h = MIN( h, MAX_TIME_STEP );

    return h;
}

/**
 * Solves the system of ordinary differential equations governing Tux's 
 * movement.  Based on Matlab's ode23.m, (c) The MathWorks, Inc.
 */
void
solve_ode_system(Player& plyr, float timestep) 
{
    float t0, t, tfinal;
    ODESolver x, y, z, vx, vy, vz; // estimates of derivs

    float h;
    bool done = false;
    bool failed = false;
    ppogl::Vec3d new_pos;
    ppogl::Vec3d new_vel, tmp_vel;
    float speed;
    ppogl::Vec3d new_f;
    ppogl::Vec3d saved_pos;
    ppogl::Vec3d saved_vel, saved_f;
    float pos_err[3], vel_err[3], tot_pos_err, tot_vel_err;
    float err=0, tol=0;
    int i;
	
    // Select an initial time step
    h = ode_time_step;

    if(h < 0){
		h = adjust_time_step_size( timestep, plyr.vel );
    }

    t0 = 0;
    tfinal = timestep;

    t = t0;

	// initialize state
    new_pos = plyr.pos;
    new_vel = plyr.vel;
    new_f   = plyr.net_force;

    // loop until we've integrated from t0 to tfinal
    while (!done) {

	if ( t >= tfinal ) {
	    PP_WARNING( "t >= tfinal in solve_ode_system()" );
	    break;
	}

	// extend h by up to 10% to reach tfinal
	if ( 1.1 * h > tfinal - t ) {
	    h = tfinal-t;
	    PP_ASSERT( h >= 0., "integrated past tfinal" );
	    done = true;
	}

    PP_LOG( DEBUG_ODE, "h: " << h );

	saved_pos = new_pos;
	saved_vel = new_vel;
	saved_f = new_f;

	// Loop until error is acceptable
	failed = false;

	for(;;){
	    // Store initial conditions
	    x.initODEData(new_pos.x(), h);
	    y.initODEData(new_pos.y(), h);
	    z.initODEData(new_pos.z(), h);
	    vx.initODEData(new_vel.x(), h);
	    vy.initODEData(new_vel.y(), h);
	    vz.initODEData(new_vel.z(), h);


	    // We assume that the first estimate in all ODE solvers is equal 
	    // to the final value of the last iteration
		x.updateEstimate(0, new_vel.x());
	    y.updateEstimate(0, new_vel.y());
	    z.updateEstimate(0, new_vel.z());
	    vx.updateEstimate(0, new_f.x() / TUX_MASS);
	    vy.updateEstimate(0, new_f.y() / TUX_MASS);
	    vz.updateEstimate(0, new_f.z() / TUX_MASS);

	    // Update remaining estimates
	    for ( i=1; i < ODESolver::numEstimates(); i++ ) {
		new_pos.x() = x.nextVal(i);
		new_pos.y() = y.nextVal(i);
		new_pos.z() = z.nextVal(i);
		new_vel.x() = vx.nextVal(i);
		new_vel.y() = vy.nextVal(i);
		new_vel.z() = vz.nextVal(i);

		x.updateEstimate(i, new_vel.x());
		y.updateEstimate(i, new_vel.y());
		z.updateEstimate(i, new_vel.z());

		new_f = calc_net_force( plyr, new_pos, new_vel );

		vx.updateEstimate(i, new_f.x() / TUX_MASS);
		vy.updateEstimate(i, new_f.y() / TUX_MASS);
		vz.updateEstimate(i, new_f.z() / TUX_MASS);
	    }

	    // Get final values
	    new_pos.x() = x.finalEstimate();
	    new_pos.y() = y.finalEstimate();
	    new_pos.z() = z.finalEstimate();

	    new_vel.x() = vx.finalEstimate();
	    new_vel.y() = vy.finalEstimate();
	    new_vel.z() = vz.finalEstimate();

		// Calculate the error
		pos_err[0] = x.estimateError();
		pos_err[1] = y.estimateError();
		pos_err[2] = z.estimateError();

		vel_err[0] = vx.estimateError();
		vel_err[1] = vy.estimateError();
		vel_err[2] = vz.estimateError();

		tot_pos_err = 0.;
		tot_vel_err = 0.;
		for ( i=0; i<3; i++ ) {
		    pos_err[i] *= pos_err[i];
		    tot_pos_err += pos_err[i];
		    vel_err[i] *= vel_err[i];
		    tot_vel_err += vel_err[i];
		}
		tot_pos_err = sqrt( tot_pos_err );
		tot_vel_err = sqrt( tot_vel_err );

        PP_LOG(DEBUG_ODE, "pos_err: " << tot_pos_err << ", vel_err: " << tot_vel_err );

		if ( tot_pos_err / MAX_POSITION_ERROR >
		     tot_vel_err / MAX_VELOCITY_ERROR )
		{
		    err = tot_pos_err;
		    tol = MAX_POSITION_ERROR;
		} else {
		    err = tot_vel_err;
		    tol = MAX_VELOCITY_ERROR;
		}

		// Update h based on error
		if (  err > tol  && h > MIN_TIME_STEP + EPS  )
		{
		    done = false;
		    if ( !failed ) {
			failed = true;
			h *=  MAX( 0.5, 0.8 *
				   pow( tol/err, 
					float(ODESolver::timeStepExponent()) ) );
		    } else {
			h *= 0.5;
		    }

		    h = adjust_time_step_size( h, saved_vel );

		    new_pos = saved_pos;
		    new_vel = saved_vel;
		    new_f = saved_f;
		
		} else {
		    // Error is acceptable or h is at its minimum; stop
		    break;
		}
	}

	// Update time
	t = t + h;

	tmp_vel = new_vel;
	speed = tmp_vel.normalize();

	// only generate particles if we're drawing them
	if(GameConfig::drawParticles){
	    generate_particles( plyr, h, new_pos, speed );
	}

	// Calculate the final net force
	new_f = calc_net_force( plyr, new_pos, new_vel );

	// If no failures, compute a new h
	if(!failed){
		double temp = 1.25 * pow(double(err) / tol,double(ODESolver::timeStepExponent()));
		if ( temp > 0.2 ) {
			h = h / temp;
	    } else {
			h = 5.0 * h;
	    }
	}

	// Clamp h to constraints
	h = adjust_time_step_size( h, new_vel );

	// Important: to make trees "solid", we must manipulate the 
	// velocity here; if we don't and Tux is moving very quickly,
	// he can pass through trees
	adjust_for_model_collision( plyr, new_pos, &new_vel );

	// Try to collect items here
	check_item_collection( plyr, new_pos );

    }

    // Save time step for next time
    ode_time_step = h;

    plyr.vel = new_vel;
    plyr.pos = new_pos;
    plyr.net_force = new_f;
}


/**
 * Updates Tux's position taking into account gravity, friction, tree 
 * collisions, etc.  This is the main physics function.
 */
void
update_player_pos(float timestep)
{
    ppogl::Vec3d surf_nml;   // normal vector of terrain
    ppogl::Vec3d tmp_vel;
    float speed;
    float paddling_factor; 
    ppogl::Vec3d local_force;
    float flap_factor;
    pp::Plane surf_plane;
    float dist_from_surface;

    if(timestep > 2.0*EPS){
		for(int i=0; i<GameMgr::getInstance().numPlayers; i++){
			solve_ode_system(players[i], timestep);
		}
    }
	
	for(int i=0; i<GameMgr::getInstance().numPlayers; i++){
		Player& plyr=players[i];
		
		tmp_vel = plyr.vel;
	
		//Set position, orientation, generate particles
    	surf_plane = get_local_course_plane( plyr.pos );
    	surf_nml = surf_plane.nml;
		dist_from_surface = surf_plane.distance( plyr.pos );
		adjust_velocity( &plyr.vel, surf_plane );
		adjust_position( &plyr.pos, surf_plane, dist_from_surface );

		speed = tmp_vel.normalize();

		set_tux_pos( plyr, plyr.pos );
		adjust_orientation( plyr, timestep, plyr.vel, 
				dist_from_surface, surf_nml );

    	flap_factor = 0;

		if(plyr.control.is_paddling){
			double factor;
			factor = (GameMgr::getInstance().time - plyr.control.paddle_time) / PADDLING_DURATION;
			if(plyr.airborne){
	    		paddling_factor = 0;
	    		flap_factor = factor;
			}else{
	   			paddling_factor = factor;
	    		flap_factor = 0;
			}
    	}else{
			paddling_factor = 0.0;
    	}

		// calculate force in Tux's local coordinate system
    	local_force = plyr.orientation.conjugate().rotate(plyr.net_force);

    	if(plyr.control.jumping){
			flap_factor = (GameMgr::getInstance().time - plyr.control.jump_start_time) / JUMP_FORCE_DURATION;
    	} 

		tux[i].adjustJoints( plyr.control.turn_animation, plyr.control.is_braking,
		       	paddling_factor, speed, local_force, flap_factor );
	}
}

void
init_physical_simulation()
{
    ppogl::Vec3d nml;
    pp::Matrix rotMat;
    float ycoord;
    ppogl::Vec3d init_vel;
    ppogl::Vec3d init_f;
    
    for(int i=0; i<GameMgr::getInstance().numPlayers; i++){
		Player& plyr = players[i];

		ycoord = find_y_coord( plyr.pos.x(), plyr.pos.z() );
		nml = find_course_normal(plyr.pos.x(), plyr.pos.z() );
		rotMat.makeRotation( -90., 'x' );
		init_vel = rotMat.transformVector( nml );
		init_vel = INIT_TUX_SPEED*init_vel;
		init_f = ppogl::Vec3d( 0., 0., 0. );

		plyr.pos.y() = ycoord;
		plyr.vel = init_vel;
		plyr.net_force = init_f;
		plyr.control.turn_fact = 0.0;
		plyr.control.turn_animation = 0.0;
		plyr.control.barrel_roll_factor = 0.0;
		plyr.control.flip_factor = 0.0;
		plyr.control.is_braking = false;
		plyr.orientation_initialized = false;
		plyr.plane_nml = nml;
		plyr.direction = init_vel;
		plyr.normal_force = ppogl::Vec3d(0,0,0);
		plyr.airborne = false;
		plyr.collision = false;
		plyr.control.jump_amt = 0;
		plyr.control.is_paddling = false;
		plyr.control.jumping = false;
		plyr.control.jump_charging = false;
		plyr.control.barrel_roll_left = false;
		plyr.control.barrel_roll_right = false;
		plyr.control.barrel_roll_factor = 0;
		plyr.control.front_flip = false;
		plyr.control.back_flip = false;
    }
    ode_time_step = -1;
}