//  Tire.cc - the tire for 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/Tire.h>
#include <vamos/geometry/Conversions.h>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <iostream>

using namespace Vamos_Geometry;

#define MINSLIP 1.0e-4
//#define MINSLIP 1.0e-2

//* Class Tire_Friction

//** Constructor
Vamos_Body::
Tire_Friction::Tire_Friction (const std::vector <double>& long_parameters,
							  const std::vector <double>& trans_parameters,
							  const std::vector <double>& align_parameters) :
  m_longitudital_parameters (long_parameters),
  m_transverse_parameters (trans_parameters),
  m_aligning_parameters (align_parameters),
  m_slide (0.0)
{
  assert (m_longitudital_parameters.size () == 11);
  assert (m_transverse_parameters.size () == 15);
  assert (m_aligning_parameters.size () == 18);
}

double Vamos_Body::Tire_Friction::Pacejka_Fx(double sigma, double Fz, double friction_factor)
{
	//double Fz_squared = Fz * Fz;
	
	// Evaluate the longitudinal parameters.
	const std::vector <double>& b = m_longitudital_parameters;
	/*double Cx = b [0];
	double Dx = friction_factor * (b [1] * Fz_squared + b [2] * Fz);
	double Bx = (b [3] * Fz_squared + b [4] * Fz) * exp (-b [5] * Fz) 
	/ (Cx * Dx);
	double Ex = b [6] * Fz_squared + b [7] * Fz + b [8];
	double Shx = b [9] * Fz + b [10];*/
	
	double D = (b[1]*Fz + b[2])*Fz*friction_factor;
	double B = (b[3]*Fz+b[4])*exp(-b[5]*Fz)/(b[0]*(b[1]*Fz+b[2]));
	double E = (b[6]*Fz*Fz+b[7]*Fz+b[8]);
	double S = (100*sigma + b[9]*Fz+b[10]);
	double Fx = D*sin(b[0] * atan(S*B+E*(atan(S*B)-S*B)));
	return Fx;
}

double Vamos_Body::Tire_Friction::Pacejka_Fy(double alpha, double Fz, double gamma, double friction_factor)
{
	//cout << alpha << "," << Fz << "," << gamma << endl;
	
	const std::vector <double>& a = m_transverse_parameters;
	
	/*double Cy = a [0];
  double Dy = (a [1] * Fz_squared + a [2] * Fz);
  double By = a [3] * sin (2.0 * atan (Fz / a [4])) 
	* (1.0 - a [5] * std::abs (gamma)) / (Cy * Dy);
  double Ey = a [6] * Fz + a [7];
  double Shy = a [8] * gamma + a [9] * Fz + a [10];
  double Svy = (a [11] * Fz + a [12]) * gamma * Fz 
	+ a [13] * Fz + a [14];*/
	
	double D = (a[1]*Fz+a[2])*Fz*friction_factor;
	double B = a[3]*sin(2.0*atan(Fz/a[4]))*(1.0-a[5]*std::abs(gamma))/(a[0]*(a[1]*Fz+a[2])*Fz);
	double E = a[6]*Fz+a[7];
	double S = alpha + a[8]*gamma+a[9]*Fz+a[10];
//	double Sv = ((a[11]*Fz+a[12])*gamma + a[13])*Fz+a[14];
	
	double Fy = D*sin(a[0]*atan(S*B+E*(atan(S*B)-S*B)));//+Sv;
	return Fy;
}

double Vamos_Body::Tire_Friction::Pacejka_Mz(double sigma, double alpha, double Fz, double gamma, double friction_factor)
{
	const std::vector <double>& c = m_aligning_parameters;
	
	double D = (c[1]*Fz+c[2])*Fz*friction_factor;
	double B = (c[3]*Fz*Fz+c[4]*Fz)*(1.0-c[6]*std::abs(gamma))*exp(-c[5]*Fz)/(c[0]*D);
	double E = (c[7]*Fz*Fz+c[8]*Fz+c[9])*(1.0-c[10]*std::abs(gamma));
	double S = alpha + c[11]*gamma+c[12]*Fz+c[13];
//	double Sv = (c[14]*Fz*Fz+c[15]*Fz)*gamma+c[16]*Fz + c[17];
	
	double Mz = D*sin(c[0]*atan(S*B+E*(atan(S*B)-S*B)));//+Sv;
	return Mz;
}

// Return the friction vector calculated from the magic formula.
// HUB_VELOCITY is the velocity vector of the wheel's reference
// frame.  PATCH_SPEED is the rearward speed of the contact pacth with
// respect to the wheel's frame.
Three_Vector Vamos_Body::
Tire_Friction::friction_forces (double normal_force, double friction_factor,
								const Three_Vector& hub_velocity, 
								double patch_speed, double current_camber,
								double sigma_hat, double alpha_hat)
{
	double Fz = normal_force / 1000.0;
	
	// Calculate the slip parameters sigma and tan_alpha.  We have to
	// play some games to keep them reasonable in extreme conditions.
	
	// Leave the slip parameters at zero if the difference between the
	// patch speed and the hub velocity is very small.
	double sigma = 0.0;
	double tan_alpha = 0.0;
	double alpha = 0.0;
	
	double V = hub_velocity[0];
	//if (std::abs(V) > MINSLIP)
	{
		double denom = std::max (std::abs (hub_velocity [0]), 1.0);
		
		//sigma = (patch_speed/V - 1.0);
		sigma = (patch_speed - V)/denom;
		//sigma = patch_speed/V;
		//sigma = -sigma;
		
		tan_alpha = hub_velocity [1] / denom;
		//alpha = rad_to_deg(atan2(hub_velocity[1],hub_velocity[0]));
		alpha = -rad_to_deg(atan2(hub_velocity[1],denom));
	}
	
	/*if (std::abs (hub_velocity [0] - patch_speed) > MINSLIP)
	{
	  // Put a lower limit on the denominator to keep sigma and
	  // tan_alpha from getting out of hand at low speeds.
		double denom = std::max (std::abs (hub_velocity [0]), 3.0);
		sigma = (patch_speed - hub_velocity [0]) / denom;
		tan_alpha = hub_velocity [1] / denom;
		alpha = atan(tan_alpha);
	}*/
	
	double gamma = rad_to_deg (current_camber);
	
	//double Fx = Pacejka_Fx(sigma, Fz);
	//double Fy = Pacejka_Fy(alpha, Fz, gamma);
	//double Mz = Pacejka_Mz(sigma, alpha, Fz, gamma);
	
	//combine the grip
	//double sigma_hat = 0.08;
	//double alpha_hat = 8.0;
	//double s = std::abs(sigma) / sigma_hat;
	//double a = std::abs(alpha) / alpha_hat;
	double s = sigma / sigma_hat;
	double a = alpha / alpha_hat;
	double rho = sqrt(s*s+a*a);
	
	double Fx = (s / rho)*Pacejka_Fx(rho*sigma_hat, Fz, friction_factor);
	double Fy = (a / rho)*Pacejka_Fy(rho*alpha_hat, Fz, gamma, friction_factor);
	double Mz = Pacejka_Mz(sigma, alpha, Fz, gamma, friction_factor);
	//Fx = (s / rho)*Pacejka_Fx(sigma, Fz);
	//Fy = (a / rho)*Pacejka_Fy(alpha, Fz, gamma);
	//cout << sigma << "," << sigma_hat << ": " << endl;
	//cout << s << "," << a << ": " << rho << endl;
	//Mz = (a / rho)*Pacejka_Mz(rho*sigma_hat, rho*alpha_hat, Fz, gamma);
	//cout << Fx << "," << Fy << "," << Mz << endl;
	
	//cout << V << "," << patch_speed << "," << sigma << ": " << Fx << endl;
	//cout << hub_velocity[0] << "," << hub_velocity[1] << "," << alpha << ": " << Fy << endl;
	//cout << "sigma=0: " << Pacejka_Fx(0, 2.5) << endl;
	//cout << "Fy,alpha=0: " << Pacejka_Fy(0, 2.5, 0) << endl;
	//cout << "alpha=0: " << Pacejka_Mz(0, 0, 2.5, 0) << endl;
	
	//double slip_x = sigma;
	double slip_x = -sigma / (1.0 + std::abs (sigma));
	//double slip_x = patch_speed/V;
	//double d_alpha = 
	//deg_to_rad (-Shy - Svy / (By * Cy * Dy * (1.0 + (180.0 / pi - 1.0) * Ey)));
	// Genta lists d_alpha as -Shy - Svy / (By * Cy * Dy).  This is what
	// you get from the magic formula for Fy(alpha) using the small
	// angle approximation.  However, the constants calculated above are
	// for alpha in degrees.  The small-angle formulas atan(x) =
	// 180*x/pi and sin(x) = pi*x/180 for x << 180/pi give the formula
	// d_alpha shown above.  Ey, in general, is not << 1.  After we get
	// d_alpha, we have to convert it to radians.
	double slip_y = tan_alpha / (1.0+std::abs (sigma-1.0));
	//double slip_y = hub_velocity[1];
	double total_slip = std::sqrt (slip_x * slip_x + slip_y * slip_y);
	/*if (total_slip > 1.0e-6)
	{
		Fx *= slip_x / total_slip;
		Fy *= slip_y / total_slip;
		Mz *= slip_y / total_slip;
	}*/
	
	//cout << slip_x << "," << slip_y << "," << total_slip << endl;
	
	// Set the volume of the tire squeal sound. 
	//m_slide = 0.0;
	const std::vector <double>& b = m_longitudital_parameters;
	double maxforce = b[2] * 7.0; //(b[1]*(normal_force / 1000.0)+b[2])*4.0;
	double combforce = sqrt(Fx*Fx + Fy*Fy);
	//double overforce = maxforce - combforce;
	if (combforce > maxforce && combforce > 1.0e-3)
	{
		//cout << "max: " << maxforce << ", " << combforce << " -- " << Fx << "," << Fy << " -- " << total_slip << endl;
		Fx *= maxforce / combforce;
		Fy *= maxforce / combforce;
	}
	/*if (!(std::abs(Fx) < maxforce))
	{
		
		Fx = maxforce;
	}
	if (!(std::abs(Fy) < maxforce))
	{
		Fy = maxforce;
	}*/
	
	if (hub_velocity.abs () < 0.1)
	{
	  m_slide = 0.0;
	}
  else
	{
	  m_slide = total_slip;
	  if (m_slide > 1.0)
		m_slide = 1.0;
	}
	
	//cout << Fx << endl;
	
	// Construct the friction vector.  The z-component is the aligning
	// torque.
	return Three_Vector (Fx, Fy, Mz);
}

//double Vamos_Body::Tire_Friction::Pacejka_Fy(double slip_y, double Fz, double gamma);

// Return the friction vector calculated from the magic formula.
// HUB_VELOCITY is the velocity vector of the wheel's reference
// frame.  PATCH_SPEED is the rearward speed of the contact pacth with
// respect to the wheel's frame.
/*Three_Vector Vamos_Body::
Tire_Friction::friction_forces (double normal_force, double friction_factor,
								const Three_Vector& hub_velocity, 
								double patch_speed, double current_camber)
{
  double Fz = normal_force / 1000.0;
  double Fz_squared = Fz * Fz;
	
  // Evaluate the longitudinal parameters.
  const std::vector <double>& b = m_longitudital_parameters;
  double Cx = b [0];
  double Dx = friction_factor * (b [1] * Fz_squared + b [2] * Fz);
  double Bx = (b [3] * Fz_squared + b [4] * Fz) * exp (-b [5] * Fz) 
	/ (Cx * Dx);
  double Ex = b [6] * Fz_squared + b [7] * Fz + b [8];
  double Shx = b [9] * Fz + b [10];

  // Evaluate the transverse parameters.
  const std::vector <double>& a = m_transverse_parameters;
  double gamma = rad_to_deg (current_camber);

  double Cy = a [0];
  double Dy = friction_factor * (a [1] * Fz_squared + a [2] * Fz);
  double By = a [3] * sin (2.0 * atan (Fz / a [4])) 
	* (1.0 - a [5] * std::abs (gamma)) / (Cy * Dy);
  double Ey = a [6] * Fz + a [7];
  double Shy = a [8] * gamma + a [9] * Fz + a [10];
  double Svy = (a [11] * Fz + a [12]) * gamma * Fz 
	+ a [13] * Fz + a [14];

  // Evaluate the formula for aligning torque.
  const std::vector <double>& c = m_aligning_parameters;
  double Cz = c [0];
  double Dz = friction_factor * (c [1] * Fz_squared + c [2] * Fz);
  double Bz = (c [3] * Fz_squared + c [4] * Fz) 
	* (1.0 - c [6] * std::abs (gamma))
	* exp (-c [5] * Fz) / (Cz * Dz);
  double Ez = (c [7] * Fz_squared + c [8] * Fz + c [9]) 
	* (1.0 - c [10] * std::abs (gamma));
  double Shz = c [11] * gamma + c [12] * Fz + c [13];
  double Svz = (c [14] * Fz_squared + c [15] * Fz) * gamma + c [16] * Fz 
	+ c [17];

  // Calculate the slip parameters sigma and tan_alpha.  We have to
  // play some games to keep them reasonable in extreme conditions.

  // Leave the slip parameters at zero if the difference between the
  // patch speed and the hub velocity is very small.
  double sigma = 0.0;
  double tan_alpha = 0.0;
  if (std::abs (hub_velocity [0] - patch_speed) > MINSLIP)
	{
	  // Put a lower limit on the denominator to keep sigma and
	  // tan_alpha from getting out of hand at low speeds.
	  double denom = std::max (std::abs (hub_velocity [0]), 3.0);
	  sigma = (patch_speed - hub_velocity [0]) / denom;
	  tan_alpha = hub_velocity [1] / denom;
	}

  double d_sigma = -Shx;
  double slip_x = -sigma / (1.0 + std::abs (sigma)) - d_sigma;

  double d_alpha = 
	deg_to_rad (-Shy - Svy / (By * Cy * Dy * (1.0 + (180.0 / pi - 1.0) * Ey)));
  // Genta lists d_alpha as -Shy - Svy / (By * Cy * Dy).  This is what
  // you get from the magic formula for Fy(alpha) using the small
  // angle approximation.  However, the constants calculated above are
  // for alpha in degrees.  The small-angle formulas atan(x) =
  // 180*x/pi and sin(x) = pi*x/180 for x << 180/pi give the formula
  // d_alpha shown above.  Ey, in general, is not << 1.  After we get
  // d_alpha, we have to convert it to radians.

  double slip_y = tan_alpha / (1.0 + std::abs (sigma)) + d_alpha;

  double total_slip = std::sqrt (slip_x * slip_x + slip_y * slip_y);
  double slip_percent = total_slip * 100.0;

  // Find the longitudinal force.
  double Fx = -Dx * sin (Cx * atan (Bx * (1.0 - Ex) * (slip_percent + Shx)
									+ Ex * atan (Bx * (slip_percent + Shx))));

  // Find the transverse force.
  double Fy = 
	-Dy * sin (Cy * atan (By * (1.0 - Ey) * (slip_percent + Shy)
						  + Ey * atan (By * (slip_percent + Shy)))) + Svy;

  // Genta describes a more complicated method of combining Fx and Fy,
  // however I can't make sense of it.  No matter what I do I can't
  // get the Fx vs. Fy curves to look like anything reasonable.

  // Find the aligning torque.
  double Mz = 
	-Dz * sin (Cz * atan (Bz * (1.0 - Ez) * (slip_percent + Shz)
						  + Ez * atan (Bz * (slip_percent + Shz)))) + Svz;

  if (total_slip > 1.0e-6)
	{
	  Fx *= slip_x / total_slip;
	  Fy *= slip_y / total_slip;
	  Mz *= slip_y / total_slip;
	}
	
	//- Fy is the resulting combined slip lateral force
	//- Fy0 is the lateral force as calculated using the normal Fy formula
 	//- Fx is the longitudinal force as calculated using the normal Fx formula
 	//- Fx0 is the MAXIMUM longitudinal force possible (calculated as D+Sv in the Pacejka Fx formula).
	//double D = (b [1] * Fz_squared + b [2] * Fz);
	//double maxforce = Fy*sqrt(1.0-(Fx/D)*(Fx/D));
	double maxforce = b[2] * 4.0; //(b[1]*(normal_force / 1000.0)+b[2])*4.0;
	double combforce = sqrt(Fx*Fx + Fy*Fy);
	//double overforce = maxforce - combforce;
	if (combforce > maxforce && combforce > 1.0e-3)
	{
		//cout << "max: " << maxforce << ", " << combforce << " -- " << Fx << "," << Fy << " -- " << total_slip << endl;
		Fx *= maxforce / combforce;
		Fy *= maxforce / combforce;
	}
	
	//cout << slip_x << "," << total_slip << endl;

  // Set the volume of the tire squeal sound. 
  if (hub_velocity.abs () < 0.1)
	{
	  m_slide = 0.0;
	}
  else
	{
	  m_slide = total_slip;
	  if (m_slide > 1.0)
		m_slide = 1.0;
	}

	//cout << Fx << endl;
	
  // Construct the friction vector.  The z-component is the aligning
  // torque.
  return Three_Vector (Fx, Fy, Mz);
}*/

//* Class Tire

//** Constructor
Vamos_Body::
Tire::Tire (double radius, double rolling_resistance_1,
			double rolling_resistance_2, const Tire_Friction& friction, 
			double inertia, double tread) :
  m_radius (radius),
  m_rolling_resistance_1 (rolling_resistance_1),
  m_rolling_resistance_2 (rolling_resistance_2),
  m_tire_friction (friction),
  m_inertia (inertia),
  m_rotational_speed (0.0),
  m_last_rotational_speed (0.0),
  m_second_last_rotational_speed (0.0),
  m_slide (0.0),
  m_velocity (0.0, 0.0, 0.0),
  m_normal_ang_velocity (0.0),
  m_normal_force (0.0),
  m_camber (0.0),
  m_applied_torque (0.0),
  m_is_locked (false),
  m_material (0),
  m_feedback (0),
  m_tread (tread),
  m_friction1 (1.0),
  m_friction2 (1.0)
{
}

// Called by the wheel to update the tire.
void Vamos_Body::
Tire::input (const Three_Vector& velocity, 
			 double normal_ang_velocity,
			 const Three_Vector& normal_force,
			 double camber,
			 double torque,
			 bool is_locked,
			 Material_Handle material)
{
  orient_frame_with_unit_vector (normal_force.unit ());

  m_velocity = rotate_in (velocity);
  m_normal_ang_velocity = normal_ang_velocity;
  m_normal_force = normal_force.abs ();
  m_camber = camber;
  m_applied_torque = torque;
  m_is_locked = is_locked;
  m_material = material;
}

// Orient the tire's z-axis with the normal force.
void Vamos_Body::
Tire::orient_frame_with_unit_vector (const Three_Vector& normal_unit_vector)
{
  Three_Vector rotation_axis = 
	Three_Vector (-normal_unit_vector [1], normal_unit_vector [0], 0.0);

  double length = sqrt (normal_unit_vector [0] * normal_unit_vector [0]
						+ normal_unit_vector [1] * normal_unit_vector [1]);
  double rotation_angle = asin (length);

  orient (Three_Matrix (1.0));
  rotate (rotation_axis.unit () * rotation_angle);
}

void Vamos_Body::
Tire::find_forces ()
{
  // m_force is the force of the road on the tire.  The force of the
  // tire on the body must be calculated.  The transverse component
  // won't change, but the longitudial component will be affected by
  // the tire's ability to move in that direction, and by applied
  // foces (acceleration and braking).

  // Skip this step if we don't have a surface yet.
  if (m_material.null ())
	return;

  m_slide = 0.0;

  if (m_normal_force <= 0.0)
	{
	  m_force.zero ();
	  m_torque.zero ();
	  return;
	}

	double sh, ah;
	LookupSigmaHatAlphaHat(m_normal_force, sh, ah);
	
	
  // Get the friction force (components 0 and 1) and the aligning
  // torque (component 2).
	double surface_friction_factor = (1.0-m_tread)*m_friction1 + m_tread*m_friction2;
	//cout << m_tread << ": " << m_friction1 << "," << m_friction2 << endl;
	//if (surface_friction_factor < 1.0) cout << surface_friction_factor << endl;
  Three_Vector friction_force = m_tire_friction.
	//friction_forces (m_normal_force, m_material->friction_factor (), m_velocity, speed(), m_camber, sh, ah);
	friction_forces (m_normal_force, surface_friction_factor, m_velocity, speed(), m_camber, sh, ah);
	
	//cout << m_normal_force << endl;
	
	//if (!firstprint)
	if (0)
	{
		ofstream of("/home/joe/temp/y.txt");
		double x;
		double load = 2.500;
		of << "Fx = c(";
		for (x = -2; x < 2; x += 0.01)
		{
			/*float load = 2500;
			float fspeed = 1.0;
			Three_Vector fakevel(fspeed/(x+1),0,0);
			cout << m_tire_friction.
		friction_forces (load, m_material->friction_factor (),
						 fakevel, fspeed, m_camber)[0] << ", ";*/
			//cout << m_tire_friction.Pacejka_Fx(x, load) << ",";
			//of << m_tire_friction.Pacejka_Mz(0, x, load, 0) << ",";
			of << m_tire_friction.Pacejka_Fx(x, load, 1.0) << ",";
		}
		of << "0)" << endl;
		//of.close();
		
		//ofstream of("/home/joe/temp/y3.txt");
		//double x;
		of << "Fy = c(";
		for (x = -20; x < 20; x += 0.1)
		{
			/*float load = 2500;
			float fspeed = 1.0;
			Three_Vector fakevel(fspeed/(x+1),0,0);
			cout << m_tire_friction.
		friction_forces (load, m_material->friction_factor (),
						 fakevel, fspeed, m_camber)[0] << ", ";*/

			//cout << m_tire_friction.Pacejka_Fx(x, load) << ",";
			//of << m_tire_friction.Pacejka_Mz(0, x, load, 0) << ",";
			of << m_tire_friction.Pacejka_Fy(x, load, 0, 1.0) << ",";
		}
		of << "0)" << endl;
		//of.close();
		
		//ofstream of("/home/joe/temp/y4.txt");
		//double x;
		of << "Mz = c(";
		for (x = -20; x < 20; x += 0.1)
		{
			/*float load = 2500;
			float fspeed = 1.0;
			Three_Vector fakevel(fspeed/(x+1),0,0);
			cout << m_tire_friction.
		friction_forces (load, m_material->friction_factor (),
						 fakevel, fspeed, m_camber)[0] << ", ";*/
			
			
			//cout << m_tire_friction.Pacejka_Fx(x, load) << ",";
			//of << m_tire_friction.Pacejka_Mz(0, x, load, 0) << ",";
			of << m_tire_friction.Pacejka_Mz(0, x, load, 0, 1.0) << ",";
		}
		of << "0)" << endl;
		
		of.close();
		
//		firstprint = true;
	}

  // the frictional force opposing the motion of the contact patch.
  m_force = Three_Vector (friction_force [0], friction_force [1], 0.0);

  // Apply the reaction torque from acceleration or braking.  In
  // normal conditions this torque comes from friction.  However, the
  // frictional force can sometimes be large when no acceleration or
  // braking is applied.  For instance, when you run into a gravel
  // trap.  In any case, the reaction torque should never be larger
  // than the applied accelerating or braking torque. 
  double reaction = m_force [0] * m_radius;
  if (((m_applied_torque > 0.0) && (reaction > m_applied_torque))
	  || ((m_applied_torque < 0.0) && (reaction < m_applied_torque)))
	{
	  reaction = m_applied_torque;
	}
	
	//cout << m_force[0] << endl;
	
	//cout << friction_force [2] << endl;

  m_torque = Three_Vector (0.0, reaction, friction_force [2]);
	
  double feedbackcoeff = 0.05;
  m_feedback = m_feedback*(1.0-feedbackcoeff) + friction_force[2]*feedbackcoeff;

  if (!m_is_locked)
	{
	  double rolling_1 = m_rolling_resistance_1;
	  if (speed () < 0.0)
		rolling_1 *= -1.0;
	  // Include constant and quadratic rolling resistance.
	  double rolling = m_rolling_resistance_factor  
		* (rolling_1 + m_rolling_resistance_2 * speed () * speed ());
	  m_applied_torque -= (m_force [0] + rolling) * m_radius;
	}

  // Add the suface drag.
  m_force [0] -= m_rolling_drag * m_velocity [0];
  m_force [1] -= m_rolling_drag * m_velocity [1];

  m_slide = m_tire_friction.slide ();
	
	bool nan = false;
	if (!(m_force[0] < 0 || m_force[0] >= 0))
	{
		m_force[0] = 0;
		nan = true;
	}
	if (!(m_force[1] < 0 || m_force[1] >= 0))
	{
		m_force[1] = 0;
		nan = true;
	}
	if (!(m_force[2] < 0 || m_force[2] >= 0))
	{
		m_force[2] = 0;
		nan = true;
	}
	
	if (nan)	
		m_force.zero();
	
	nan = false;
	
	if (!(m_torque[0] < 0 || m_torque[0] >= 0))
	{
		m_torque[0] = 0;
		nan = true;
	}
	if (!(m_torque[1] < 0 || m_torque[1] >= 0))
	{
		m_torque[1] = 0;
		nan = true;
	}
	if (!(m_torque[2] < 0 || m_torque[2] >= 0))
	{
		m_torque[2] = 0;
		nan = true;
	}
	
	if (nan)
		m_torque.zero();
}

// Advance this suspension component forward in time by TIME.
void Vamos_Body::
Tire::propagate (double time) 
{
	m_second_last_rotational_speed = m_last_rotational_speed;
  m_last_rotational_speed = m_rotational_speed;
  if (m_is_locked)
	{
	  // This causes speed() to return 0.0.
	  m_rotational_speed = 0.0;
	}
  else
	{
	  m_rotational_speed += m_applied_torque * time / m_inertia;
	}
}

void Vamos_Body::
Tire::rewind ()
{
  m_rotational_speed = m_last_rotational_speed;
	m_last_rotational_speed = m_second_last_rotational_speed;
}

// Return the position of the contact patch in the wheel's coordinate
// system.
Three_Vector Vamos_Body::
Tire::contact_position () const
{
  return Three_Vector (0.0, 0.0, -m_radius);
}

// Set the tire to its initial conditions.
void Vamos_Body::
Tire::reset ()
{
	//firstprint = false;
  m_rotational_speed = 0.0;
  m_force.zero ();
  m_torque.zero ();
	
	int i;
	for (i = 0; i < HAT_ITERATIONS; i++)
	{
		FindSigmaHatAlphaHat((double)(i+1)*HAT_LOAD, sigma_hat[i], alpha_hat[i]);
		//cout << i << ": " << sigma_hat[i] << "," << alpha_hat[i] << endl;
	}
}

void Vamos_Body::
Tire::FindSigmaHatAlphaHat(double load, double & sh, double & ah)
{
	double x, y, ymax;
	ymax = 0;
	for (x = -2; x < 2; x += 0.01)
	{
		y = m_tire_friction.Pacejka_Fx(x, load, 1.0);
		if (y > ymax)
		{
			sh = x;
			ymax = y;
		}
	}
	
	ymax = 0;
	for (x = -20; x < 20; x += 0.1)
	{
		y = m_tire_friction.Pacejka_Fy(x, load, 0, 1.0);
		if (y > ymax)
		{
			ah = x;
			ymax = y;
		}
	}
}

void Vamos_Body::
Tire::LookupSigmaHatAlphaHat(double normalforce, double & sh, double & ah)
{
	double nf = (normalforce / 1000.0);
	if (nf < HAT_LOAD)
	{
		sh = sigma_hat[0];
		ah = alpha_hat[0];
	}
	else if (nf > HAT_LOAD*HAT_ITERATIONS)
	{
		sh = sigma_hat[HAT_ITERATIONS-1];
		ah = alpha_hat[HAT_ITERATIONS-1];
	}
	else
	{
		int lbound;
		double blend;
		lbound = (int)(nf/HAT_LOAD);
		lbound--;
		if (lbound < 0)
			lbound = 0;
		blend = (nf-HAT_LOAD*(lbound+1))/HAT_LOAD;
		sh = sigma_hat[lbound]*(1.0-blend)+sigma_hat[lbound+1]*blend;
		ah = alpha_hat[lbound]*(1.0-blend)+alpha_hat[lbound+1]*blend;
	}
}
