//Focus on the car used by the script owner
rcar $car=get scriptowner car
//Temoins
rcarmodel $temoin
rcarmodel $temoin2

//Capteurs ESP
float $slipratio0
float $slipratio1
float $slipratio2
float $slipratio3
float $slipangle0
float $slipangle1
float $slipangle2
float $slipangle3
float $wheel0_vel
float $wheel1_vel
float $wheel2_vel
float $wheel3_vel
float $gforce[3]
float $gforce_lat
float $lacet
float $brake
float $speed
float $steerangle
float $throttle
int $rpm

//																		Seuils 
//Quand l'ESP augmente son action (derepage trop long)
int $urgence = 1000
//Seuil de glissement des roues avant intervention de l'ESP
float $wheelfront = 0.21
float $wheelrear = 0.21
//Seuil de declanchement de l'ABS
float $spin = 0.25
float $spin_rear = -0.25
//Seuil de patinage
float $ratio = 1.10
//Seuil de declanchement de l'ESP (inactif en ligne droite..)
float $seuil_lateral = 0.01

//																		Valeurs statiques

//Valeur de freinage pour chaque roue
float $brake0
float $brake1
float $brake2
float $brake3
//Valeur initiales de freinage pour chaque roue
float $brake0stat = 6500
float $brake1stat = 6500
float $brake2stat = 2800
float $brake3stat = 2800
float $unbrake_front = 0.01
float $unbrake_rear = 0.01


//Variables
shared int $right_oversteer = 0
shared int $right_understeer = 0
shared int $left_oversteer = 0
shared int $left_understeer = 0
shared float $esp
float $frequence = 0.20
float $finalbrake
float $finalthrottle
float $unthrottle
float $compteur = 0
int $mode = 1
int $action
int $patinage = 0
int $csv = 1
float $wait = 0.005

while 1
{
	$brake = get $car brake
	$throttle = get $car throttle
	$rpm = get $car rpm
	$temoin2 = get generic model 5 of $car
	$temoin = get generic model 9 of $car
	set $temoin scriptcontrolled 1
	//set $temoin2 scriptcontrolled 1
	$steerangle = get $car steerangle // y<0 means right direction, > 0 means left, same for lateral gforce
	$gforce = get $car gforce
	$gforce_lat=$gforce.x
	//$lacet = get $car rotvelocity.x
	$slipangle0 = get $car wheel 0 slipangle
	$slipangle1 = get $car wheel 1 slipangle
	$slipangle2 = get $car wheel 2 slipangle
	$slipangle3 = get $car wheel 3 slipangle
	$slipangle0 = abs($slipangle0)
	$slipangle1 = abs($slipangle1)
	$slipangle2 = abs($slipangle2)
	$slipangle3 = abs($slipangle3)
	$slipratio0 = get $car wheel 0 slipratio
	$slipratio1 = get $car wheel 1 slipratio
	$slipratio2 = get $car wheel 2 slipratio
	$slipratio3 = get $car wheel 3 slipratio
	$wheel0_vel = get system "car0.wheel0.rotvel"
	$wheel1_vel = get system "car0.wheel1.rotvel"
	$wheel2_vel = get system "car0.wheel2.rotvel"
	$wheel3_vel = get system "car0.wheel3.rotvel"
	$speed=($wheel0_vel+$wheel1_vel+$wheel2_vel+$wheel3_vel)/4
	float $maxsteerangle = get $car max steerangle
	int $derapage = 0
	$derapage = $left_oversteer+$left_understeer+$right_oversteer+$right_understeer + $patinage
	$ratio = 1.17 + abs(0.20*$steerangle/$maxsteerangle)
	float $essieu_avant = ($wheel0_vel+$wheel1_vel)/2
	float $essieu_arriere = ($wheel1_vel+$wheel2_vel)/2
	float $diff_essieux = abs($wheel0_vel+$wheel1_vel)/abs($wheel2_vel+$wheel3_vel)
	float $diagonale1 = ($wheel0_vel+$wheel3_vel)/2
	float $diagonale2 = ($wheel1_vel+$wheel2_vel)/2
	if $diagonale1 > $diagonale2
	{
		$speed = abs($diagonale1)
	}
	if $diagonale1 < $diagonale2
	{
		$speed = abs($diagonale2)
	}
	if $essieu_arriere > $essieu_avant
	{
		$speed = abs($essieu_arriere)
	}
	if $essieu_avant > $essieu_arriere
	{
		$speed = abs($essieu_avant)
	}
	
	
	$brake0=$brake0stat
	$brake1=$brake1stat
	$brake2=$brake2stat
	$brake3=$brake3stat
	$finalthrottle=$throttle
	$right_oversteer = 0
	$right_understeer = 0
	$left_oversteer = 0
	$left_understeer = 0
	set $temoin position float[3]{0.0,-10.0,0.000}
	//activation
	if is key (65) pressed //Key A
	{
		if $mode <1
		{
		$mode += 1
		}
		else
		{
		$mode = 0
		}
		wait(300)
  	}
	if $mode == 0
	{
		$compteur += 1
		$esp = 0
		$finalbrake = $brake
		$finalthrottle = $throttle
		set $temoin position float[3]{0.0,0.0,0.0}
		set $temoin2 position float[3]{0.0,0.0,0.0}
		if $compteur > 4000
		{
			set $temoin2 position float[3]{0,-10,0}
		}
	}
	if $mode == 1
	{
		//$compteur = 0
		set $temoin2 position float[3]{0,-10,0}
	}
	if $speed > 42
	{
		$mode = 1
	}

	// ESP part
 	if $esp > 1
	{
		$esp = 1
	}	
	if abs($speed) < 12 //disable system under 14km/h
	{
		$esp = 0
		$finalthrottle = $throttle
		$unthrottle = 0
		$derapage = 0
		$action = 0
	}
	if $rpm < 900
	{
		$finalthrottle = $throttle
		$unthrottle = 0
	}
	
	
	
	
	
	
	
	
	
	//front axle
	if $slipangle0 > $wheelfront or $slipangle1 > $wheelfront and $brake < 0.05 and $mode > 0 and $csv != 0
	{
		$action++
		//echo "ESP, sous virage"
		if $gforce_lat > $seuil_lateral and $steerangle > 0.05 //wheel2 is braked, left oversteer
		{
			$left_understeer = 1
			$esp=$esp+$frequence
			$brake0=0
			$brake1=0
			$brake2=$brake2stat
			$brake3=0
			$unthrottle-=$frequence
		}
		if $gforce_lat > $seuil_lateral and $steerangle < 0.05 //wheel3 is braked, right oversteer
		{
			$left_oversteer = 1
			$esp=$esp+$frequence
			$brake0=0
			$brake1=$brake1stat
			$brake2=0
			$brake3=0
			$unthrottle-=$frequence	
		}
		if $gforce_lat < -1.0*$seuil_lateral and $steerangle < -0.05 //wheel3 is braked, right oversteer
		{
			$right_understeer = 1
			$esp=$esp+$frequence
			$brake0=0
			$brake1=0
			$brake2=0
			$brake3=$brake3stat
			$unthrottle-=$frequence
		}
		if $gforce_lat < -1.0*$seuil_lateral and $steerangle > 0.05 //wheel2 is braked, right oversteer
		{
			$right_oversteer=1
			$esp=$esp+$frequence
			$brake0=0
			$brake1=0
			$brake2=$brake2stat
			$brake3=0
			$unthrottle-=$frequence
		}
		if $action > $urgence
		{
			//echo "derapage trop long !"
			$unthrottle-=$frequence
			$brake0=0.25*$brake0stat
			$brake1=0.25*$brake1stat
			if $left_understeer == 1
			{
				$brake2=$brake2stat
				$brake3=0
			}
			if $right_understeer
			{
				$brake2=0
				$brake3=$brake3stat
			}
		}
	}
	//rear axle
	if $slipangle2 > $wheelrear or $slipangle3 > $wheelrear and $brake < 0.05 and $mode > 0
	{
		//echo "ESP, survirage"
		if $gforce_lat > $seuil_lateral //wheel1 is braked, left oversteer
		{
			$left_oversteer = 1
			$esp=$esp+$frequence
			$brake0=0
			$brake1=$brake1stat
			$brake2=0
			$brake3=0
			$unthrottle-=$frequence
		}
		if $gforce_lat < -1.0*$seuil_lateral //wheel0 is braked, right oversteer
		{
			$right_oversteer = 1
			$esp=$esp+$frequence
			$brake0=$brake0stat
			$brake1=0
			$brake2=0
			$brake3=0
			$unthrottle-=$frequence
		}
	}


	
	
	
	
	
	//TC part
	if $mode > 0 and abs($speed) > 12.0 and $throttle > 0.0
	{ 
		if abs($speed) > 12.0
		{
			$patinage = 0
			if abs($wheel0_vel) > abs($ratio*$wheel1_vel)
			{
				$patinage = 1
				$esp+=1.0*$frequence
				$brake0/=$unbrake_front
				$brake1=0
				$brake2=0
				$brake3=0
			}
			if abs($wheel1_vel) > abs($ratio*$wheel0_vel)
			{
				$patinage = 1
				$esp+=1.0*$frequence
				$brake0=0
				$brake1/=$unbrake_front
				$brake2=0
				$brake3=0
			}
			if $diff_essieux > $ratio or $diff_essieux < 1.0-($ratio-1.0)
			{
				$patinage = 1
				$esp+=1.0*$frequence
				$brake0/=$unbrake_front
				$brake1/=$unbrake_front
				$brake2=0
				$brake3=0
				//$unthrottle-=0.01*$frequence
			}
			if $patinage == 1
			{
				$compteur++
			}
			if $compteur > 500
			{
				$unthrottle-=0.1*$frequence
			}
		}
	}
	if abs($speed) < 12.0
	{
		$derapage = 0
		$patinage = 0
		$esp = 0.0
	}
	
	
	
	


	//Abs part
	if ($brake+$esp) > 0.0 and abs($speed) > 5.0
	{
		if $slipratio0 < -$spin
		{
			$brake0*=$unbrake_front
			if $brake0 < 0
			{
				$brake0=0
			}
		}
		else
		{
			$brake0/=$unbrake_front
			if $brake0 > $brake0stat
			{
				$brake0=$brake0stat
			}
		}
		if $slipratio1 < -$spin
		{
			$brake1*=$unbrake_front
			if $brake1 < 0
			{
				$brake1=0
			}
		}
		else
		{
			$brake1/=$unbrake_front
			if $brake1 > $brake1stat
			{
				$brake1=$brake1stat
			}
		}
		if $slipratio2 < $spin_rear
		{
			$brake2*=$unbrake_rear
			if $brake2 < 0
			{
				$brake2=0
			}
		}
		else
		{
			$brake2/=$unbrake_rear
			if $brake2 > $brake2stat
			{
				$brake2=$brake2stat
			}
		}
		if $slipratio3 < $spin_rear
		{
			$brake3*=$unbrake_rear
			if $brake3 < 0
			{
				$brake3=0
			}
		}
		else
		{
			$brake3/=$unbrake_rear
			if $brake3 > $brake3stat
			{
				$brake3=$brake3stat
			}
		}
	}
	if $derapage == 0
	{
		$esp=0.0
		$unthrottle = 0.0
		$action = 0
	}
	if $unthrottle < -1.0
	{
		$unthrottle = -1.0
	}
	$finalbrake=$brake+$esp
	if $finalbrake > 1
	{
		$finalbrake = 1
	}
	if $finalthrottle < 0
	{
		$finalthrottle = 0
	}
	
	
	//Display the light
	if $esp > 0 and $mode > 0
	{
		set $temoin position float[3]{0.0,0.0,0.00}
		$compteur++
		if $compteur %2 == 0
		{
			set $temoin position float[3]{0.0,-10.0,0.00}	
		}
	}
	set system "car0.wheel0.max_braking" to $brake0;
	set system "car0.wheel1.max_braking" to $brake1;
	set system "car0.wheel2.max_braking" to $brake2;
	set system "car0.wheel3.max_braking" to $brake3;
	set $car brake $finalbrake
	set $car throttle $finalthrottle+$unthrottle
	wait ($wait)
	interrupt
}