//  Wheel.cc - a wheel.
//
//  Copyright (C) 2001--2004 Sam Varner
//
//  This file is part of Vamos Automotive Simulator.
//
//  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 <vamos/body/Wheel.h>
#include <vamos/geometry/Conversions.h>
//#include <vamos/geometry/Ac3d.h>
#include <iostream>

using namespace Vamos_Geometry;

//* Class Wheel

//** Constructor
Vamos_Body::
Wheel::Wheel (double mass, 
			  Three_Vector position, 
			  double tire_offset,
			  double roll_height,
			  double restitution,
			  Suspension* suspension, 
			  const Tire& tire, 
			  const Brake& brake,
			  bool steered,
			  bool driven,
			  Side side) :
  // Friction is handled by the tire.
  Contact_Point (mass, position, Material::RUBBER,
				 0.0, restitution),
  m_original_position (position),
  m_tire_offset ((side == RIGHT) ? -tire_offset : tire_offset),
  m_roll_height (roll_height),
  mp_suspension (suspension),
  m_tire (tire),
  m_brake (brake),
  m_ground_velocity (0.0, 0.0, 0.0),
  m_normal (Three_Vector (0.0, 0.0, 0.0)),
  m_ang_velocity (Three_Vector (0.0, 0.0, 0.0)),
  m_tire_torque (0.0),
  m_drive_torque (0.0),
  m_braking_torque (0.0),
  m_steered (steered),
  m_driven (driven),
  m_side (side),
/*  m_slow_wheel_list (0),
  m_fast_wheel_list (0),
  m_stator_list (0),*/
  m_transition_speed (10.0),
  m_rotation (0.0)
{
}

void Vamos_Body::
Wheel::find_forces ()
{
  if (!m_contact)
	{
	  m_force.zero ();
	  m_torque.zero ();
	  m_impulse.zero ();
	  m_position = m_original_position;
	  mp_suspension->reset ();
	}

  m_tire.input (m_ground_velocity, 
				m_ang_velocity [2],
				mp_suspension->force ().project (m_normal),
				mp_suspension->current_camber (m_normal.unit () [1]),
				m_drive_torque + m_braking_torque,
				m_brake.is_locked (), 
				m_material);

  m_tire.find_forces ();
  m_force = m_tire.force ();
  m_torque = m_tire.torque ();
  m_torque [1] *= -1.0;
}

// Advance the wheel forward in time by TIME.
void Vamos_Body::
Wheel::propagate (double time)
{
  m_tire.propagate (time);
  orient (mp_suspension->orientation ());
  m_rotation += speed () * time / m_tire.radius ();
	//cout << m_rotation << endl;
}

void Vamos_Body::
Wheel::rewind ()
{
  m_tire.rewind ();
}

// Handle collisions.  The return value is the displacement of the
// wheel as a result of the contact.
double Vamos_Body::
Wheel::contact (const Three_Vector& position,
				const Inertia_Tensor& inertia,
				const Three_Vector& velocity, 
				double distance,
				const Three_Vector& normal,
				const Three_Vector& ang_velocity,
				Material_Handle material)
{
  m_contact = true;
  if (mp_suspension->bottomed_out ())
	{
	  Particle::contact (position, 
						 inertia, 
						 rotate_in (velocity), 
						 distance, 
						 rotate_in (normal), 
						 rotate_in (ang_velocity), 
						 material);
	}

  m_impulse.zero ();
	
  m_normal = rotate_in (normal);
  Three_Vector v_perp = rotate_in (velocity).project (m_normal);
  m_ground_velocity = rotate_in (velocity) - v_perp;
  m_ang_velocity = ang_velocity;
  Three_Vector disp = 
	(m_normal * distance).back_project (Three_Vector (0.0, 0.0, -1.0));
  mp_suspension->displace (disp.abs ());
  mp_suspension->input (m_force, m_normal);
  mp_suspension->torque (m_braking_torque);
  m_material = material;
  m_position = m_original_position + disp;
  double rdisp = mp_suspension->displacement();
	return -rdisp;
}

void Vamos_Body::
Wheel::drive_torque (double torque_in)
{
  m_drive_torque = torque_in;
}

// Return the torque exerted on the wheel by the brake when a pressure
// of FACTOR * Brake::m_max_pressure is applied to the brake.
void Vamos_Body::
Wheel::brake (double factor)
{
  m_braking_torque = -m_brake.torque (factor, m_tire.rotational_speed ());
}

// Return the position relative to the body where the wheel exerts its
// force.
Three_Vector Vamos_Body::
Wheel::force_position () const
{
  return m_position + m_tire.contact_position () 
	+ Three_Vector (0.0, m_tire_offset, m_roll_height);
}

// Return the position of the wheel relative to the body for the
// purpose of detecting collisions.
Three_Vector Vamos_Body::
Wheel::contact_position () const
{
  return m_original_position + m_tire.contact_position ()
	+ Three_Vector (0.0, m_tire_offset, 0.0);
}

// Return the position of the wheel relative to the body
Three_Vector Vamos_Body::
Wheel::position () const
{
  return m_position;
}

// Retrun the slip ratio for the wheel.
double Vamos_Body::
Wheel::slip () const
{
  if (m_ground_velocity [0] == 0.0) return 0.0;

  return (speed () - m_ground_velocity [0]) / std::abs (m_ground_velocity [0]);
}

// Return the wheel to its initial conditions.
void Vamos_Body::
Wheel::reset ()
{
  Particle::reset ();
  m_tire.reset ();
}

/*GLuint Vamos_Body::
Wheel::make_model (std::string file, double scale, 
				   const Three_Vector& translation, 
				   const Three_Vector& rotation)
{
//  Ac3d* model = new Ac3d (file, scale, translation, rotation);
  //GLuint display_list = model->build ();
  //delete model;
  //return display_list;
}*/

void Vamos_Body::
Wheel::set_models (std::string slow_file, 
				   std::string fast_file, 
				   double transition_speed,
				   std::string stator_file,
				   double stator_offset,
				   double scale,
				   const Three_Vector& translation,
				   const Three_Vector& rotation)
{
  Three_Vector offset;
  if (stator_file != "")
	{
	  offset [1] += (m_side == RIGHT) ? stator_offset : -stator_offset;
	}
	
	//printf("%s, %s\n", slow_file.c_str(), fast_file.c_str());
	
	model.Load(slow_file,true);	
	
  /*if (m_slow_wheel_list != 0)
	{
	  glDeleteLists (m_slow_wheel_list, 1);
	}
  m_slow_wheel_list = make_model (slow_file, scale, translation + offset, rotation);

  if (m_fast_wheel_list != 0)
	{
	  glDeleteLists (m_fast_wheel_list, 1);
	}
  m_fast_wheel_list = 
	make_model (fast_file, scale, translation + offset, rotation);

  if (stator_file != "")
	{
	  if (m_stator_list != 0)
		{
		  glDeleteLists (m_stator_list, 1);
		}
	  m_stator_list = make_model (stator_file, scale, translation, rotation);
	}*/
	
	m_transition_speed = transition_speed;
}

extern GLfloat LightPosition[4];

void Vamos_Body::
Wheel::transform ()
{
  glTranslatef (m_position [0], m_position [1], m_position [2]);
  double angle;
  Three_Vector axis = axis_angle (&angle);
	glRotatef (angle, axis [0], axis [1], axis [2]);
	glRotatef(m_rotation*(180/3.141593), 0, 1, 0);
	
	float lp[4];
	lp[0] = -LightPosition[0];
	lp[1] = LightPosition[2];
	lp[2] = LightPosition[1];
	lp[3] = 0;
	//glLightfv( GL_LIGHT1, GL_POSITION, lp );
}

// Draw the wheel.
void Vamos_Body::
Wheel::draw ()
{
  glPushMatrix ();
  transform ();
	//model.Draw(0,0);
	if (m_side == RIGHT)
		glScalef(1,-1,1);
	model.DrawStatic();
/*  glCallList (m_stator_list);
  if (speed () < m_transition_speed)
	{
	  glRotatef (rad_to_deg (m_rotation), 0.0, 1.0, 0.0);
	  glCallList (m_slow_wheel_list);
	}
  else
	{
	  glCallList (m_fast_wheel_list);
	}*/
  glPopMatrix ();
glPushMatrix();
  mp_suspension->draw ();
	glPopMatrix();
}

Vamos_Body::Suspension * Vamos_Body::
Wheel::suspension()
{
	return mp_suspension;
}

Vamos_Geometry::Three_Vector Vamos_Body::
Wheel::ang_velocity()
{
	return m_ang_velocity;
}
