using System;
using System.Threading;
using System.Diagnostics;

namespace VehicleDynamics
{
	public class CarDynamics : IDisposable
	{
		public enum IgnitionState { IgnitionOff, IgnitionOn, IgnitionStart };
		public enum GearState { Park=0, Reverse=1, Neutral=2, Drive=3, FirstGear=4, SecondGear=5, ThirdGear=6 };

	#region Attributes
		private	Euler			attitude = new Euler();
		private	Euler			attitude_rate = new Euler();
		private	Euler			ground_slope = new Euler();
		private	Vector			position = new Vector();
		private	Vector			velocity = new Vector();			// body velocity
		private	Vector			earth_velocity = new Vector();		// earth velocity
		private	Vector			acceleration = new Vector();		// body accelerations
		private	Vector			body_gravity = new Vector();
		private	Wheel[]			wheel = new Wheel[4];
		private	double			weight;
		private	double			cg_height;
		private	double			wheel_base;
		private	double			wheel_track;
		private	double			wheel_angle;
		private	double			wheel_max_angle; 
		private	double			weight_distribution;
		private	double			front_weight;
		private	double			back_weight;
		private	double			wheel_pos;
		private	double			throttle;
		private	double			brake;
		private	double			engine_rpm;
		private	double			wheel_rpm;
		private	double			wheel_force;
		private	double			net_force;
		private	double			drag;
		private	double			rolling_resistance;
		private	double			eng_torque;
		private	double			mph;
		private	double			brake_torque;
		private	double			engine_loss;
		private	double			idle_rpm;
		private	double			max_rpm;
		private	double			target_rpm;
		private	double			engine_horsepower;
		private	double			max_engine_torque;
		private	double			air_density;
		private	double			drag_coeff;
		private	double			frontal_area;
		private	double			wheel_diameter;
		private	double			mass;		// in slugs
		private	double			inverse_mass;		// in slugs
		private	float			centripedal_accel;
		private	bool			running;
		private	bool			front_wheel_drive = false;
		private	GearState		gear;
		private	GearState		auto_gear = GearState.Drive;
		private	double[]		gear_ratio = new double[7];
		private	LFI				torque_curve = new LFI();      // percent max torque - index by % max RPM * 10
		private	IgnitionState	ignition_state = IgnitionState.IgnitionOn;
		private bool            was_sliding = false;
		private Thread          m_process_thread;
		private bool            thread_active = true;
		private Mutex           mutex = new Mutex();

	#endregion

	#region Properties
		public double SteeringWheel	{ get { return wheel_pos; } set { wheel_pos = value; } }

		public bool EngineRunning { get { return running; } }

		public double EngineRPM { get { return engine_rpm; } }

		public double Throttle { get { return throttle; } set { throttle = value; } }

		public double Brake { get { return brake; } set { brake = value; } }

		public GearState Gear { get { return gear; } set { gear = value; } }

		public IgnitionState Ignition { get { return ignition_state; }
			set 
			{
				ignition_state = value;
				if ( ignition_state == IgnitionState.IgnitionStart && 
					( gear == GearState.Park || gear == GearState.Neutral ) ) 
				{
					running = true;
				} 
				else if ( ignition_state == IgnitionState.IgnitionOff ) 
				{
					running = false;
				}
			}
		}

		public double Roll { get { return attitude.Phi; } set { attitude.Phi = value; } }
		public double Pitch { get { return attitude.Theta; } set { attitude.Theta = value; } }
		public double Heading { get { return attitude.Psi; } set { attitude.Psi = value; } }

		public double North { get { return position.X; } set { position.X = value; } }
		public double East { get { return position.Y; } set { position.Y = value; } }
		public double Height { get { return position.Z; } set { position.Z = value; } }

		public double NorthVelocity { get { return velocity.X; } set { velocity.X = value; } }
		public double EastVelocity { get { return velocity.Y; } set { velocity.Y = value; } }
		public double VerticalVelocity { get { return velocity.Z; } set { velocity.Z = value; } }

		public double ForwardVelocity { get { return velocity.X; } }
		public double SidewaysVelocity { get { return velocity.Y; } }
		public double WheelRadius { get { return wheel[(int)WhichWheel.LeftFront].radius; }
			set 
			{
				wheel_diameter = value * 2.0;
				wheel[(int)WhichWheel.LeftFront].radius = value;
				wheel[(int)WhichWheel.LeftRear].radius = value; 
				wheel[(int)WhichWheel.RightFront].radius = value; 
				wheel[(int)WhichWheel.RightRear].radius = value; } 
		}

		public double HorsePower { get { return engine_horsepower; } set { engine_horsepower = value; } }

		public double LFGroundHeight { set { wheel[(int)WhichWheel.LeftFront].ground_height = value; } }
		public double RFGroundHeight { set { wheel[(int)WhichWheel.RightFront].ground_height = value; } }
		public double LRGroundHeight { set { wheel[(int)WhichWheel.LeftRear].ground_height = value; } }
		public double RRGroundHeight { set { wheel[(int)WhichWheel.RightRear].ground_height = value; } }

		public double MPH { get { return mph; } }
		public bool   Running { get { return running; } }

	#endregion

		public CarDynamics()
		{
			engine_horsepower = 470.0f;
			torque_curve.SetDataPoint(  0, 0.0f );
			torque_curve.SetDataPoint(  10, 0.00f );
			torque_curve.SetDataPoint(  20, 0.22f );
			torque_curve.SetDataPoint(  30, 0.5f );
			torque_curve.SetDataPoint(  40, 0.72f );
			torque_curve.SetDataPoint(  50, 0.9f );
			torque_curve.SetDataPoint(  60, 1.0f );
			torque_curve.SetDataPoint(  70, 0.98f );
			torque_curve.SetDataPoint(  80, 0.89f );
			torque_curve.SetDataPoint(  90, 0.5f );
			torque_curve.SetDataPoint( 100, 0.13f );
			wheel[(int)WhichWheel.LeftFront]  = new Wheel(WhichWheel.LeftFront);
			wheel[(int)WhichWheel.RightFront] = new Wheel(WhichWheel.RightFront);
			wheel[(int)WhichWheel.LeftRear]   = new Wheel(WhichWheel.LeftRear);
			wheel[(int)WhichWheel.RightRear]  = new Wheel(WhichWheel.RightRear);
			Reset();
			m_process_thread = new Thread(new ThreadStart(Process));
			m_process_thread.Start();
		}

		public void Process()
		{
			float delta_t = 0.01f;
               
			Debug.WriteLine("car physics thread started");
			while ( thread_active )
			{
				Process( delta_t );
				Thread.Sleep(10);
			}
			Debug.WriteLine("car physics thread terminated");
		}

		public void Reset()
		{
			wheel_max_angle = 0.69813170079773183076947630739545; // 40 degrees
			idle_rpm = 700.0f;
			max_rpm = 7000.0f;
			engine_rpm = 0.0f;
			attitude.Theta = 0.0f;
			attitude.Phi = 0.0f;
			attitude.Psi = 0.0f;
			attitude_rate.Theta = 0.0;
			attitude_rate.Phi = 0.0;
			attitude_rate.Psi = 0.0;
			position.X = 0.0;
			position.Y = 0.0;
			position.Z = 0.0;
			velocity.X = 0.0;			// body velocity
			velocity.Y = 0.0;			// body velocity
			velocity.Z = 0.0;			// body velocity
			acceleration.X = 0.0;		// body accelerations
			acceleration.Y = 0.0;		// body accelerations
			acceleration.Z = 0.0;		// body accelerations
			cg_height             = -1.0;
			wheel_base            = 5.0;
			wheel_track           = 8.0;
			weight                = 2000.0;
			WheelRadius = 0.25;
			wheel[(int)WhichWheel.LeftFront ].offset.Set( wheel_track / 2.0f, -wheel_base / 2.0f, +cg_height + wheel[(int)WhichWheel.LeftFront].radius );
			wheel[(int)WhichWheel.RightFront].offset.Set( wheel_track / 2.0f,  wheel_base / 2.0f, +cg_height + wheel[(int)WhichWheel.LeftFront].radius );
			wheel[(int)WhichWheel.LeftRear  ].offset.Set(-wheel_track / 2.0f, -wheel_base / 2.0f, +cg_height + wheel[(int)WhichWheel.LeftFront].radius );
			wheel[(int)WhichWheel.RightRear ].offset.Set(-wheel_track / 2.0f,  wheel_base / 2.0f, +cg_height + wheel[(int)WhichWheel.LeftFront].radius );
			for ( int i=0; i<4; i++ ) 
			{
				wheel[i].StaticWeightOverWheel = weight / 4.0;
			}
			weight_distribution   = 0.5f;
			front_weight          = weight * ( 1.0 - weight_distribution);
			back_weight           = weight * weight_distribution;
			wheel_pos             = 0.0f;
			throttle              = 0.0f;
			brake                 = 0.0f;
			engine_rpm            = 0.0f;
			wheel_rpm             = 0.0f;
			wheel_force           = 0.0f;
			net_force             = 0.0f;
			mph                   = 0.0f;
			drag                  = 0.0f;
			rolling_resistance    = 0.0f;
			eng_torque            = 0.0f;
			brake_torque          = 0.0f;
			engine_loss           = 0.0f;
			air_density           = 0.068;
			drag_coeff            = 0.004f;
			frontal_area          = 20.0f;
			mass                  = weight * 0.031080950172;		// in slugs
			inverse_mass          = 1.0 / mass;
			running = true;
			front_wheel_drive = false;
			gear = GearState.Drive;
			gear_ratio[(int)GearState.Park]			= 0.0f;		
			gear_ratio[(int)GearState.Reverse]		= -80.0f;
			gear_ratio[(int)GearState.Neutral]		= 0.0f;
			gear_ratio[(int)GearState.Drive]		= 45.0f;
			gear_ratio[(int)GearState.FirstGear]	= 70.0f;
			gear_ratio[(int)GearState.SecondGear]	= 50.0f;
			gear_ratio[(int)GearState.ThirdGear]	= 30.0f;
			ignition_state = IgnitionState.IgnitionOn;
			max_engine_torque = engine_horsepower * 255.00f;

			if ( front_wheel_drive ) 
			{
				wheel[(int)WhichWheel.LeftFront ].drive_wheel = true;
				wheel[(int)WhichWheel.RightFront].drive_wheel = true;
				wheel[(int)WhichWheel.LeftRear  ].drive_wheel = false;
				wheel[(int)WhichWheel.RightRear ].drive_wheel = false;
			} 
			else 
			{
				wheel[(int)WhichWheel.LeftFront ].drive_wheel = false;
				wheel[(int)WhichWheel.RightFront].drive_wheel = false;
				wheel[(int)WhichWheel.LeftRear  ].drive_wheel = true;
				wheel[(int)WhichWheel.RightRear ].drive_wheel = true;
			}
		}

		private void IntegratePosition( float delta_t )
		{
   
			Vector	earth_velocity;            

			velocity.IncrementX( delta_t * acceleration.X );
			velocity.IncrementY( delta_t * acceleration.Y );
			velocity.IncrementZ( delta_t * acceleration.Z );

			if ( velocity.X > 48.0 ) 
			{
				velocity.X = 48.0;
				acceleration.X = -1.0;
			}
			if ( velocity.X < 0.0 ) 
			{
				velocity.X = 0.0;
				acceleration.X = 0.5;
			}

			attitude = attitude + (attitude_rate * delta_t);
			attitude.Limits();

			earth_velocity = new Vector( velocity );
			attitude.RotateAtoE( earth_velocity );

			position.IncrementX( delta_t * earth_velocity.X );
			position.IncrementY( delta_t * earth_velocity.Y );
			position.IncrementZ( delta_t * earth_velocity.Z );
//			Debug.WriteLine("accelZ = " + acceleration.Z + " velz = " + velocity.Z + " posz = " + position.Z);

			mph = (float)velocity.X * 3600.0f / 5280.0f;
			
		}

		private void CalcWeightTransfer()
		{
			front_weight = (1.0f - weight_distribution) * weight + (float)acceleration.X * cg_height / wheel_base;
			back_weight = weight - front_weight;

			wheel[(int)WhichWheel.LeftFront].WeightOverWheel  = 0.5f * front_weight - (float)acceleration.Y * cg_height / wheel_track;
			wheel[(int)WhichWheel.RightFront].WeightOverWheel = front_weight - wheel[(int)WhichWheel.LeftFront].WeightOverWheel;
			wheel[(int)WhichWheel.LeftRear].WeightOverWheel   = 0.5f * back_weight - (float)acceleration.Y * cg_height / wheel_track;
			wheel[(int)WhichWheel.RightRear].WeightOverWheel  = back_weight - wheel[(int)WhichWheel.LeftRear].WeightOverWheel;
		}

		public void SetFriction(WhichWheel the_wheel, float friction)
		{
			wheel[(int)the_wheel].friction = friction;
		}

		public void SetGearRatio(GearState state, float ratio)
		{
			gear_ratio[(int)state] = ratio;
		}

		public float WheelNorth(WhichWheel the_wheel)
		{
			return (float)wheel[(int)the_wheel].earth_location.X;
		}

		public float WheelEast(WhichWheel the_wheel)
		{
			return (float)wheel[(int)the_wheel].earth_location.Y;
		}

		public float WheelHeight(WhichWheel the_wheel)
		{
			return (float)wheel[(int)the_wheel].earth_location.Z;
		}

		public void SetWheelAltitude(WhichWheel the_wheel, float altitude)
		{
			wheel[(int)the_wheel].ground_height = altitude;
		}

		public void SetWheelOffset(WhichWheel the_wheel, float forward, float right, float up)
		{
			wheel[(int)the_wheel].offset.X = forward;
			wheel[(int)the_wheel].offset.Y = right;
			wheel[(int)the_wheel].offset.Z = up;
		}

		public void Dispose()
		{
			Debug.WriteLine("car physics Dispose");
			thread_active = false;
			Thread.Sleep(20);
		}

		void Process(float delta_t)
		{
			double temp;  
			double delta_rpm;
			double current_gear_ratio = 0.0;
			double brake_force;
			double percent_rpm;
			double turn_rate;
			bool  shift_up;
			bool  shift_down;
			double delta_psi;
			Vector	gravity = new Vector(0.0f, 0.0f, 32.0f);        

			wheel_angle = wheel_pos * wheel_max_angle;

			wheel[(int)WhichWheel.LeftFront].RelHeading = wheel_angle;
			wheel[(int)WhichWheel.RightFront].RelHeading = wheel_angle;

			double sine_wheel = Math.Sin(wheel_angle);

			turn_rate = (sine_wheel * velocity.X / wheel_track) / 10.0f;
//			Debug.WriteLine( "sine_wheel=" + sine_wheel + " turn rate=" + turn_rate);

			if ( wheel[(int)WhichWheel.LeftFront].sliding && wheel[(int)WhichWheel.RightFront].sliding ) 
			{
//				turn_rate = 0.0f;
			}

			attitude_rate.Psi = turn_rate;

			delta_psi = turn_rate * delta_t;

			centripedal_accel = (float)(2.0 * velocity.X * Math.Sin(delta_psi) ) / delta_t;

			wheel_rpm = 60.0f * velocity.X / (Math.PI * wheel_diameter);

			rolling_resistance = 0.00696f * (float)Math.Abs(velocity.X);

			drag = 0.5f * drag_coeff * frontal_area * air_density * Math.Abs(velocity.X * velocity.X);

			brake_force = brake * 32.0;  // max braking 1G

			if ( mph < 0.0 ) // braking force opposes movement
			{
				brake_force *= -1.0;
			}

			if ( wheel[(int)WhichWheel.LeftFront].sliding && wheel[(int)WhichWheel.RightFront].sliding && 
				wheel[(int)WhichWheel.RightRear].sliding && wheel[(int)WhichWheel.RightRear].sliding ) 
			{
				brake_force = 0.0f;
			}

			percent_rpm = engine_rpm / max_rpm;

			switch ( gear ) 
			{
				case GearState.Park:
					if ( mph > 1.0  || mph < -1.0 ) 
					{
						brake_force = 32.0;
					} 
					else 
					{
						velocity.SetX(0.0f);
					}
					auto_gear = GearState.Park;
					break;
				case GearState.Reverse:
					auto_gear = GearState.Reverse;
					break;
				case GearState.Neutral:
					auto_gear = GearState.Neutral;
					break;
				case GearState.Drive:
					shift_up = false;
					shift_down = false;
					if ( ( percent_rpm > 0.8 && auto_gear < GearState.Drive ) ||
						( percent_rpm > 0.1 && auto_gear == GearState.Neutral ) ) 
					{
						shift_up = true;
					}
					if ( percent_rpm < 0.4 && auto_gear >= GearState.FirstGear ) 
					{
						shift_down = true;
					}
				switch ( auto_gear ) 
				{
					case GearState.Neutral:
						if ( shift_up ) 
						{
							auto_gear = GearState.FirstGear;