/** 
 * Car
 *
 * A test car.
 * 
 *  @author Bruno Augier 
 *  @version 2.0 - 01/04/2009
 *  @seealso http://dzzd.net/
 */

import net.dzzd.DzzD;
import net.dzzd.access.*;
import java.awt.event.KeyEvent;

public class Car
{
	private double maxTurn;
	private double maxSpeed;
	private double minSpeed;
	
	private double speed;
	private double turn;
	
	
	private boolean speedUp;
	private boolean speedDown; //break
	private boolean turnLeft;
	private boolean turnRight;
	
	private IAxis3D pos;	//Car position (& rotation)
	
	ISolidSphere3D wfl;
	ISolidSphere3D wfr;
	ISolidSphere3D wbl;
	ISolidSphere3D wbr;
	
	IPoint3D fwfl;
	IPoint3D fwfr;
	IPoint3D fwbl;
	IPoint3D fwbr;
	
	IPoint3D ffwfl;
	IPoint3D ffwfr;
	IPoint3D ffwbl;
	IPoint3D ffwbr;
	
	IMesh3D car;
	IMesh3D mwfl;
	IMesh3D mwfr;
	IMesh3D mwbl;
	IMesh3D mwbr;
	
		
	public Car(IScene3D scene)
	{
		this.maxTurn=Math.PI*0.05;
		this.maxSpeed=1.0;
		this.minSpeed=-this.maxSpeed*0.3;
		this.speed=0;
		this.turn=0;
		this.speedUp=false;
		this.speedDown=false;
		this.turnLeft=false;
		this.turnRight=false;
		
		this.wfl=DzzD.newSolidSphere3D();
		this.wfr=DzzD.newSolidSphere3D();
		this.wbl=DzzD.newSolidSphere3D();
		this.wbr=DzzD.newSolidSphere3D();
		
		this.wfl.setRadius(8);
		this.wfr.setRadius(8);
		this.wbl.setRadius(8);
		this.wbr.setRadius(8);
		
		this.wfl.setScene3D(scene);
		this.wfr.setScene3D(scene);
		this.wbl.setScene3D(scene);
		this.wbr.setScene3D(scene);
		
		
		this.fwfl=DzzD.newPoint3D();
		this.fwfr=DzzD.newPoint3D();
		this.fwbl=DzzD.newPoint3D();
		this.fwbr=DzzD.newPoint3D();
		
		this.ffwfl=DzzD.newPoint3D();
		this.ffwfr=DzzD.newPoint3D();
		this.ffwbl=DzzD.newPoint3D();
		this.ffwbr=DzzD.newPoint3D();
		
		this.car=scene.getMesh3DByName("CAR");
		this.mwfl=scene.getMesh3DByName("WFL");
		this.mwfr=scene.getMesh3DByName("WFR");
		this.mwbl=scene.getMesh3DByName("WBL");
		this.mwbr=scene.getMesh3DByName("WBR");
		
		car.setSolid(false);
		mwfl.setSolid(false);
		mwfr.setSolid(false);
		mwbl.setSolid(false);
		mwbr.setSolid(false);
		
		
		this.wfl.getSource().copy(mwfl.getPosition());
		this.wfr.getSource().copy(mwfr.getPosition());
		this.wbl.getSource().copy(mwbl.getPosition());
		this.wbr.getSource().copy(mwbr.getPosition());
		
		this.wfl.getDestination().copy(this.wfl.getSource());
		this.wfr.getDestination().copy(this.wfr.getSource());
		this.wbl.getDestination().copy(this.wbl.getSource());
		this.wbr.getDestination().copy(this.wbr.getSource());

		
		gravity.set(0,-0.18,0);
		bounce=0.0;
		grip=0.98;
		
	}
	double bounce;
	double grip;
	boolean sol=false;
		
	public void doKeyboardInput(IDirectInput input)
	{
		this.speedUp=false;
		this.speedDown=false;
		this.turnRight=false;
		this.turnLeft=false;
		
		if(input.isKey(KeyEvent.VK_UP))
				this.speedUp=true; 
				
		if(input.isKey(KeyEvent.VK_DOWN))
				this.speedDown=true;
		
		if(input.isKey(KeyEvent.VK_RIGHT))
				this.turnRight=true; 
				
		if(input.isKey(KeyEvent.VK_LEFT))
				this.turnLeft=true; 
		
		
		
		this.speed*=0.95;	
		if(this.turnRight)
			this.turn+=0.03;
		else
		if(this.turnLeft)
			this.turn-=0.03;
		else
			this.turn*=0.05;	
		//if(this.sol)
		
		
		if(this.speedUp)
		{
		
			if(this.speed<=0)
				this.speed=maxSpeed*0.1;
			else
				this.speed+=maxSpeed*0.1;
			
		}
			
		if(this.speedDown)
		{
		
			if(this.speed>0)
				this.speed=0;
			else
				this.speed-=0.5;
		}
		
			
	//	else
	//	this.speed=0;
		
		
		if(this.turn>this.maxTurn)
			this.turn=this.maxTurn;		
		if(this.turn<-this.maxTurn)
			this.turn=-this.maxTurn;					
			
		if(this.speed>this.maxSpeed)
			this.speed=this.maxSpeed;		
		if(this.speed<this.minSpeed)
			this.speed=this.minSpeed;	
		
		
		
					
							
	}
	
	IPoint3D p=DzzD.newPoint3D();
	IPoint3D p2=DzzD.newPoint3D();
	
	private void setDistance(IPoint3D s1,IPoint3D s2,IPoint3D fs1,IPoint3D fs2,double d)
	{
		p2.copy(s1);
		p2.sub(s2);
		double l=p2.norm();

		p2.mul(d/l-1);
		p2.mul(0.3);
		fs1.add(p2);
		fs2.sub(p2);
		
	}

	IPoint3D gravity=DzzD.newPoint3D();
	IAxis3D cara=DzzD.newAxis3D();
	double tt=0;
	IPoint3D cax=DzzD.newPoint3D();
	IPoint3D cay=DzzD.newPoint3D();
	IPoint3D caz=DzzD.newPoint3D();
	
	public void move(IScene3D s)
	{
		this.displace(s);
		this.constraints(s);
		
		//Inertia : must be <1.0 to be stable
		double inertia=1.0;
		this.fwfl.copy(wfl.getSource()).sub(mwfl.getPosition()).mul(inertia);
		this.fwfr.copy(wfr.getSource()).sub(mwfr.getPosition()).mul(inertia);
		this.fwbl.copy(wbl.getSource()).sub(mwbl.getPosition()).mul(inertia);
		this.fwbr.copy(wbr.getSource()).sub(mwbr.getPosition()).mul(inertia);
		
		  
		
		//Set new pos on scene object
		mwfl.getPosition().copy(this.wfl.getSource());
		mwfr.getPosition().copy(this.wfr.getSource());
		mwbl.getPosition().copy(this.wbl.getSource());
		mwbr.getPosition().copy(this.wbr.getSource());

		p.copy(this.wfl.getSource());
		p.add(this.wfr.getSource());
		p.div(2.0);
		caz.copy(p);
		
		p.copy(this.wbl.getSource());
		p.add(this.wbr.getSource());
		p.div(2.0);
		caz.sub(p);		
		
		p.copy(this.wfr.getSource());
		p.add(this.wbr.getSource());
		p.div(2.0);
		cax.copy(p);
		
		p.copy(this.wfl.getSource());
		p.add(this.wbl.getSource());
		p.div(2.0);
		cax.sub(p);
		
		(cay.copy(caz)).cross(cax);
		
		
		(caz.copy(cax)).cross(cay);
		
		
		cax.normalize();
		cay.normalize();
		caz.normalize();		
		
		
		
		double axx=cax.getX();		
		double axy=cax.getY();
		double axz=cax.getZ();
		double ayx=cay.getX();		
		double ayy=cay.getY();
		double ayz=cay.getZ();		
		double azx=caz.getX();	
		double azy=caz.getY();
		double azz=caz.getZ();

		double den=Math.sqrt((axx*axx)+(axy*axy)+(axz*axz));
		double sz=-axy/den;
		
				
		double rz=0.0;
		if(sz>1.0) sz=1.0;
		if(sz<-1.0) sz=-1.0;
			rz=Math.asin(sz);
		
		//if(ayy<0)
			//rz=Math.PI-rz;
		 
		den=Math.sqrt((axx*axx)+(axz*axz));
		if(den>1.0) den=1.0;
		double rx=0.0;
		double ry=0.0;
		if(Math.abs(den)>0.0);//Double.MIN_VALUE*10)
		{
			double cx=ayy/den;			
			if(cx>1.0) cx=1.0;
			if(cx<-1.0) cx=-1.0;
			rx=Math.acos(cx);
			if(azy<0.0)
			 rx=-rx;
			 			
	
			double cy=axx/den;			
			if(cy>1.0) cy=1.0;
			if(cy<-1.0) cy=-1.0;					
			ry=Math.acos(cy);
			if(axz<0.0)
			 ry=-ry;
		}
		
		if(Math.abs(rx)>Math.PI*0.5 && Math.abs(ry)>Math.PI*0.5)
		{
			rx=-Math.PI+rx;
			ry=-Math.PI+ry;
			rz=Math.PI-rz;
		}
				
		car.getRotation().setX(rx);
		car.getRotation().setY(ry);
		car.getRotation().setZ(rz);
		
		p.copy(this.wfl.getSource());
		p.add(this.wfr.getSource());
		p.add(this.wbl.getSource());
		p.add(this.wbr.getSource());
		p.div(4);
		
		car.getPosition().copy(p);

		
		mwfl.getRotation().copy(car.getRotation());
		mwfr.getRotation().copy(car.getRotation());
		mwbl.getRotation().copy(car.getRotation());
		mwbr.getRotation().copy(car.getRotation());
		
		mwfl.getRotation().setY(mwfl.getRotation().getY()-turn);
		mwfr.getRotation().setY(mwfr.getRotation().getY()-turn);
		
		speedr-=speed*0.1;
		mwfl.getRotation().setX(speedr);
		mwfr.getRotation().setX(speedr);
		mwbl.getRotation().setX(speedr);
		mwbr.getRotation().setX(speedr);

		
		cara.copy(car.getAxis3D());
		
		


	}
	
	double speedr=0;
	boolean solfl=false;
	boolean solfr=false;
	boolean solbl=false;
	boolean solbr=false;
	
	public void displace(IScene3D s)
	{
		p.copy(cara.getAZ()).sub(cara.getOrigin());
	
		this.fwfl.add(gravity); 
		this.fwfr.add(gravity);
		this.fwbl.add(gravity); 
		this.fwbr.add(gravity); 
		
		
		//add force
		double g1=1;
	//	if(speed<0)
	//	g1=0.95;
		
		
		if(solfl)
			this.fwfl.mul(grip*g1); 
		if(solfr)
			this.fwfr.mul(grip*g1); 
			
			
		double g2=1.0009;	
	
			
		if(solbl)
			this.fwbl.mul(grip*g2); 
		if(solbr)
			this.fwbr.mul(grip*g2); 
						
		if(sol)
		{
			p.mul(speed);	
			p.rotateY(-turn);
			if(solfl)
				this.fwfl.add(p); 
			if(solfr)
				this.fwfr.add(p); 
		}
		 solfl=false;
		 solfr=false;
		 solbl=false;
		 solbr=false;

		
		
		this.wfl.getDestination().add(this.fwfl);
		this.wfr.getDestination().add(this.fwfr);
		this.wbl.getDestination().add(this.fwbl);
		this.wbr.getDestination().add(this.fwbr);
		/*
		if(!sol)
		bounce=1.0;
		else
		bounce=0;
	*/
		sol=false;

		for(int x=0;x<4;x++)
		{
		
			if(this.wfl.moveBounce(1,bounce).isImpact())
			{
				this.solfl=true;;
				sol=true;
			}
			
			if(this.wfr.moveBounce(1,bounce).isImpact())
			{
				this.solfr=true;
				sol=true;
			}
			if(this.wbl.moveBounce(1,bounce).isImpact())
			{
				this.solbl=true;
				sol=true;
			}
				
			if(this.wbr.moveBounce(1,bounce).isImpact())
			{
				this.solbr=true;
				sol=true;
			}
			if(!sol)
				break;
		}
		

		
		
	}
	
	public void constraints(IScene3D s)
	{
		
		this.setDistance(wfl.getSource(),wfr.getSource(),this.wfl.getDestination(),this.wfr.getDestination(),30);
		this.setDistance(wbl.getSource(),wbr.getSource(),this.wbl.getDestination(),this.wbr.getDestination(),30);
		this.setDistance(wbl.getSource(),wfl.getSource(),this.wbl.getDestination(),this.wfl.getDestination(),30);
		this.setDistance(wbr.getSource(),wfr.getSource(),this.wbr.getDestination(),this.wfr.getDestination(),30);	
		this.setDistance(wfl.getSource(),wbr.getSource(),this.wfl.getDestination(),this.wbr.getDestination(),42.43);
		this.setDistance(wfr.getSource(),wbl.getSource(),this.wfr.getDestination(),this.wbl.getDestination(),42.43);


		//Apply constraints
		for(int x=0;x<2;x++)
		{		
			if(this.wfl.moveSlide(1).isImpact())
				sol=true;
			if(this.wfr.moveSlide(1).isImpact())
				sol=true;
			if(this.wbl.moveSlide(1).isImpact())
				sol=true;
			if(this.wbr.moveSlide(1).isImpact())
				sol=true;
			
			if(!sol)
				break;
		}

		
		
	}

}
