@use PhysicalControl.
@use Shape.
@use Stationary.
@use Link.
@use MultiBody.
@use Drawing.

@define CELDAS_MAX_VELOCITY 30.

PhysicalControl : CeldasControl {
	% This class is used for building simple vehicle 
	% simulations.  To create a vehicle simulation, 
	% subclass CeldasControl and use the init method to 
	% create OBJECT(CeldasObstacle) and 
	% OBJECT(CeldasVehicle) objects.

	+ variables:
		floor (object).
		floorShape (object).
		cloudTexture (object).


	+ to init:
		self enable-lighting.
                #self enable-smooth-drawing.

		floorShape = new Shape.
		floorShape init-with-cube size (200, .2, 200).

		floor = new Stationary.
		floor register with-shape floorShape at-location (0, 0, 0).
		#floor catch-shadows.

		self point-camera at (0, 0, 0) from (3, 3, 24).

		#self enable-shadows.
		#self enable-reflections.

		cloudTexture = (new Image load from "images/clouds.png"). 
		self set-background-color to (.4, .6, .9).
		self set-background-texture-image to cloudTexture.

}

MultiBody : CeldasLightVehicle (aka CeldasLightVehicles) {
	% This object is used in conjunction with OBJECT(CeldasControl) to
	% create simple vehicles.

	+ variables:
		bodyShape (object).
		wheelShape (object).
		sensorShape (object).
		bodyLink (object).

		wheels (list).
		sensors (list).

	+ to init:
		bodyShape = new Shape.
		bodyShape init-with-cube size (4.0, .75, 3.0).	

		wheelShape = new Shape.
		wheelShape init-with-polygon-disk radius ( self get-wheel-radius ) sides 20 height ( self get-wheel-width ).
		# 40

		sensorShape = new Shape.
		sensorShape init-with-polygon-cone radius .2 sides 5 height .5.
		# 10

		bodyShape set-density to ( self get-density ).
		bodyLink = new Link.
		bodyLink set-shape to bodyShape.	
		bodyLink set-mu to -1.0.
		bodyLink set-eT to .8.

		self set-root to bodyLink.

		self move to (0, 0.9, 0).
		self set-texture-scale to 1.5.

	- to get-density:
		return 1.0.

	- to get-wheel-width:
		return 0.1.

	- to get-wheel-radius:
		return 0.6.

	+ section "Adding Wheels and Sensors to a Vehicle"

	+ to add-wheel at location (vector):
		% Adds a wheel at location on the vehicle.  This method returns
		% the wheel which is created, a OBJECT(CeldasWheel).

		wheel, joint (object).

		wheel = new CeldasWheel.
		wheel set-shape to wheelShape.

		joint = new RevoluteJoint.

		joint set-relative-rotation around-axis (1, 0, 0) by 1.5708.
		joint link parent bodyLink to-child wheel with-normal (0, 0, 1)
					with-parent-point location with-child-point (0, 0, 0).

		wheel set-eT to .8.
		wheel set-texture to 0.
		wheel set-joint to joint.
		joint set-strength-limit to (joint get-strength-hard-limit) / 2.
		wheel set-color to (.6, .6, .6).
		wheel set-mu to 100000.

		self add-dependency on joint.
		self add-dependency on wheel.

		push wheel onto wheels.

		return wheel.

	+ to add-sensor at location (vector) with-direction direction = (0,1,0)(vector) :
		% Adds a sensor at location on the vehicle.  This method returns
		% the sensor which is created, a OBJECT(CeldasSensor).

		sensor, joint (object).

		sensor = new CeldasSensor.
                sensor set-direction to direction.
                
		sensor set-shape to sensorShape.

		joint = new RevoluteJoint.

		joint set-relative-rotation around-axis (0, 0, 1) by -1.57.
		joint link parent bodyLink to-child sensor with-normal (1, 0, 0)
					with-parent-point location with-child-point (0, 0, 0).

		joint set-double-spring with-strength 300 with-max 0.01 with-min -0.01.

		self add-dependency on joint.
		self add-dependency on sensor.

		sensor set-color to (0, 0, 0).

		#push sensor onto sensors.

		return sensor.

	+ to destroy:
		free sensorShape.
		free wheelShape.
		free bodyShape.

		super destroy.
}

CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
	% A heavy duty version of OBJECT(CeldasLightVehicle), this
	% vehicle is heavier and harder to control, but more stable
	% at higher speeds.
        +variables:
            lSensor, rSensor, fSensor, bSensor (object).
            lWheel,rWheel (object).        
        
	- to get-density:
		return 20.0.

	- to get-wheel-width:
		return 0.4.

	- to get-wheel-radius:
		return 0.8.

        + to set-global-velocity to velocity (float):
		rWheel set-velocity to velocity.
		lWheel set-velocity to velocity.

	+ to get-global-velocity:
		return ((rWheel get-velocity) + (lWheel get-velocity)) / 2.

	+ to turn-right with-velocity velocity (float):		
		lWheel set-velocity to velocity.
                rWheel set-velocity to -velocity.

	+ to turn-left with-velocity velocity (float):
#                vehicle rotate around-axis (0,1,0) by 1. 
		lWheel set-velocity to -velocity.	
		rWheel set-velocity to velocity.

        + to get-sensor-value:
		return (fSensor get-sensor-value).

        +to init:
            fSensor = (self add-sensor at (2.0, .4, 0)).            
            fSensor set-direction to (1,0,0).
            fSensor set-id at 1.
	    fSensor set-body at self.
            bSensor = (self add-sensor at (-2.0, .4, 0)).
            bSensor set-direction to (-1,0,0).
            bSensor set-id at 2.
	    bSensor set-body at self.
            lSensor = (self add-sensor at (0, .4, 1.5)).
            lSensor set-direction to (0,0,1).
            lSensor set-id at 3.
	    lSensor set-body at self.


            rSensor = (self add-sensor at (0, .4, -1.5)).
            rSensor set-direction to (0,0,-1).
            rSensor set-id at 4.
	    rSensor set-body at self.

            lWheel  = (self add-wheel at (0, 0, -1.5)).
            rWheel  = (self add-wheel at (0, 0, 1.5)).
                        
        +to iterate:            
        #+ to post-iterate:
		valuef,valueb,valuer,valuel (float).
		fl, fr(float).
            	
                valuef=fSensor get-data.
		valueb=bSensor get-data.
                valuel=lSensor get-data.
                valuer=rSensor get-data.

		self turn-left with-velocity(20).
                self set-global-velocity to (15).
		if valuef >7: 		
                    self set-global-velocity to (15).
                else if (valuef <=7) && (valuef > 0):
                {   
                    self set-global-velocity to (0).
                    #self turn-left with-velocity(2).
                    #self turn-right with-velocity(2).
                    #self set-global-velocity to (0).
		}
                #print "sensor valuef: $valuef  valueb: $valueb".
                
                #else if value < 0.1: self turn-left with-velocity CELDAS_MAX_TURN_VELOCITY.
		#else if value > 10: self set-global-velocity to ((self get-global-velocity) - 1).

		#fl = (flWheel get-velocity).
		#fr = (frWheel get-velocity).
		#print " sensorf: $value  sensorb $valueb, fr: $fr, fl: $fl".            
            
}

Stationary : CeldasObstacle (aka CeldasObstacles) {
	% A CeldasObstacle is used in conjunction with OBJECT(CeldasControl)
	% and OBJECT(CeldasVehicle).  It is what the OBJECT(CeldasSensor)
	% objects on the CeldasVehicle detect.
	% <p>
	% There are no special behaviors associated with the walls--they're 
	% basically just plain OBJECT(Stationary) objects.
   
        +variables:
            large (float).
	    direction (vector).	


	+ to init with-size theSize = (10, 3, .1) (vector) with-color theColor = (1, 0, 0) (vector) at-location theLocation = (0, 0, 0) (vector) with-rotation theRotation = [ ( 0, 0, 1 ), ( 0, 1, 0 ), ( 1, 0, 0 ) ] (matrix):                    
		self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation.
                large=20.

	+ to init-with-shape shape theShape (object) color theColor = (1, 0, 0) (vector) at-location theLocation = (0, 0, 0) (vector) with-rotation theRotation = [ ( 1, 0, 0 ), ( 0, 1, 0 ), ( 0, 0, 1 ) ] (matrix):
		self register with-shape theShape at-location theLocation with-rotation theRotation.
		self set-color to theColor.
                
        + to get-large:
            return large.

	+ to set-direction at theDirection (vector):
	    direction=theDirection.

	+ to get-direction:
	    return direction.
}

Link : CeldasWheel (aka CeldasWheels) {
	% A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle)
	% to build Celdas vehicles.  This class is typically not instantiated
	% manually, since OBJECT(CeldasVehicle) creates one for you when you
	% add a wheel to the vehicle.

	+ variables:
		joint (object).
		velocity (float).

	+ to init:
		velocity = 0.

	- to set-joint to j (object):
		% Used internally.

		joint = j.

	+ section "Configuring the Wheel's Velocity"

	+ to set-velocity to n (float):
		% Sets the velocity of this wheel.

		if n > CELDAS_MAX_VELOCITY: n = CELDAS_MAX_VELOCITY.
		velocity = n.

		joint set-joint-velocity to velocity.

	+ to get-velocity:
		% Gets the velocity of this wheel.
		
		return velocity.

}

Link : CeldasSensor (aka CeldasSensors) {
	% A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle)
	% to build Celdas vehicles.  This class is typically not instantiated
	% manually, since OBJECT(CeldasVehicle) creates one for you when you
	% add a sensor to the vehicle.

	+ variables:
		direction (vector).
                positiveDirection(vector).
		sensorAngle (float).
		value (float).
                draw (object).
		body(object).
                id(int).

	+ to init :
		direction = (1,0,1).
                positiveDirection= (1,0,1).
		sensorAngle = 1.6.
		value = 0.0.
                draw = new Drawing.
                		

  + section "Configuring the Sensor Values"
        + to set-id at n (int):
            id=n.

	+ to set-body at robotBody(object):
		body=robotBody.
	        
	+ to set-sensor-angle to n (float):
		% Sets the angle in which this sensor can detect obstacles.  The default
		% value of 1.6 means that the sensor can see most of everything in
		% front of it.  Setting the value to be any higher leads to general
		% wackiness, so I don't suggest it.

		sensorAngle = n.

        + to set-direction to n (vector):
                direction = n.
                positiveDirection::x=|n::x|.
                positiveDirection::y=|n::y|.
                positiveDirection::z=|n::z|.

  + section "Getting the Sensor Values"

	+ to get-sensor-value:
		% Gets the sensor value. This should be used from post-iterate,
		% if not, the sensor reading correspond to the previous
		% iteration.
        
        #+ to iterate:
        
	+ to get-data:
		i (object).
                x,y,z (float).
		min,dist (float).
                v,obs(vector).
		aux(float).
		j (int).
		des1,des2,des3(int).
		wallBegin,wallEnd (float).
                
		aux1,aux2,aux3,aux4 (float).
		yo,toObstacle, transDir (vector).
                largeWall (float).
                source,destiny (vector).
                obsLoc (vector).                
                location (vector).
                posObstacle,posSensor (vector).
                angulo(double).
                                             
           	draw clear.
		value = 0.0.
		j=0.
		min=0.
		foreach i in (all CeldasObstacles): 
			{
			 posObstacle=i get-location.
			 
                                                		
			
			if(dot((i get-direction),direction)==0):
				des1=1.
			else
				des1=0.


			des2=0.
			if(dot(direction,(1,1,1))<0):
			{                        
                            if((dot((self get-location),positiveDirection))>(dot(posObstacle,positiveDirection))):
					des2=1.      
                        }
			else
			{
                            if((dot((self get-location),positiveDirection))<(dot(posObstacle,positiveDirection))):
					des2=1.         
			}			


			#Compruebo que el robot este frente a la pared
			wallBegin=dot((i get-location),(i get-direction) )- (i get-large)/2.
			wallEnd=dot((i get-location),(i get-direction) )+ (i get-large)/2.		


			#print "begin: $wallBegin end: $wallEnd".
			yo=self get-location.
			destiny=i get-direction.

			v = (body get-location) - (self get-location ).
			obsLoc::y=y=posObstacle::y.								
				
			if (dot((i get-direction),(1,0,0))):
			{
	        	 obsLoc::x=x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).
                         obsLoc::z=z=posObstacle::z.
			}				
			else
			{
        	         obsLoc::z=z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).
                         obsLoc::x=x=posObstacle::x.
			}                       				


			if (dot((obsLoc),(i get-direction)) > wallBegin) && (dot((obsLoc),(i get-direction)) < wallEnd):
			    	des3=1.
			else
			{
				 des3=0.
                        	 
			}				

 			aux1=dot((self get-location),(i get-direction)).

			#print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".
			#print "dist: $aux1 begin: $wallBegin end: $wallEnd".

                       
			print "sensor: $id , des1: $des1, des2: $des2, des3: $des3".
			if ((des1) && (des2) && (des3)):
                         {                                    
                        	draw clear.        	                
				#print " posObstacle: $posObstacle".                        				
							
				dist=|obsLoc - (self get-location)|.
				if( (j==0) || (min>dist) ):
				 {
 				 	min=dist.
				 	obs=obsLoc.
				 	j++.	
				 }

			 }						

                        
		} #end for

		if(j!=0):
 			{
		  	  #Dibujo el laser
       		  	  draw set-color to (1, 0, 0).
        	  	  draw draw-line from (self get-location) to (obs).
			  return dist.
		 	}
		

                value = -1.
                return value.


}