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

@define CELDAS_MAX_VELOCITY 5.
@define CELDAS_TURNO 30.
@define CELDAS_SENSOR_THRESHOLD 10.

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).

	+ 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).
		lfWheel,rfWheel,lbWheel,rbWheel (object).
		tleft,tright (int).	    
		avanzando,retrocediendo,girando_izq,girando_der(int).	    
		iterate(int).
		teorias (list).
		sa (object).
		teoria (object).
		entorno (hash).
		datos-finales (hash).
		plan-finished (int).
		posicion-inicial (vector).
		posicion-final (vector).
        
	- to get-density:
		return 20.0.

	- to get-wheel-width:
		return 0.4.

	- to get-wheel-radius:
		return 0.8.

	- to near position thePosition (vector) with-error error (float):
		vectorAux(vector).
		vectorAux = (self get-location) - thePosition.

		#print "-----> (pos, other_pos, diff, error): ", (self get-location), thePosition, vectorAux, error.

		if ((|vectorAux::x| < error) && (|vectorAux::z| < error)):
			return 1.

		return 0.

        + to set-global-velocity to velocity (float):
		rfWheel set-velocity to velocity.
		lfWheel set-velocity to velocity.
		rbWheel set-velocity to velocity.
		lbWheel set-velocity to velocity.

	+ to get-global-velocity:
		return ((rfWheel get-velocity) + (lfWheel get-velocity)) / 2.

	+ to turn-right:		
		#tright++.
		#self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. 
		#if (tright == CELDAS_TURNO): tright=0.
		if (tright == 0): self set-global-velocity to 0.
		self rotate around-axis (0,1,0) by (-1.5709/CELDAS_TURNO)*tright. 
		if (tright == CELDAS_TURNO): tright=0.
		else tright++.


	+ to turn-left:
		#tleft++.
		#self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft. 
		#if (tleft == CELDAS_TURNO): tleft=0.
		if (tleft == 0): self set-global-velocity to 0.
		self rotate around-axis (0,1,0) by (1.5709/CELDAS_TURNO)*tleft.
		if (tleft == CELDAS_TURNO): tleft=0.
		else tleft++.


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



	+to update-entorno:
		entorno{"sensor_f"} = (fSensor get-sensor-value).
		entorno{"sensor_b"} = (bSensor get-sensor-value).
		entorno{"sensor_r"} = (rSensor get-sensor-value).
		entorno{"sensor_l"} = (lSensor get-sensor-value).
		sa update-entorno with entorno.            

        +to init:
		# Configuracion de robot
		fSensor = (self add-sensor at (2.0, .4, 0)).            
		fSensor set-direction to (1,0,0).
		#fSensor set-direction to (0,0,1).
		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-direction to (0,0,1).
		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-direction to (1,0,0).
		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-direction to (-1,0,0).
		rSensor set-id at 4.
		rSensor set-body at self.

		lfWheel = (self add-wheel at (2, 0, -1.5)).
		lbWheel = (self add-wheel at (-2, 0, -1.5)).
		rfWheel = (self add-wheel at (2, 0, 1.5)).
		rbWheel = (self add-wheel at (-2, 0, 1.5)).

		tleft=tright=0.
		avanzando=0.
		retrocediendo=0.
		girando_izq=0.            
		girando_der=0.            

		posicion-inicial = (self get-location).
		posicion-final = (0, 0, 0).

		# Configuracion de sistema autonomo
		sa = new SistemaAutonomo.
		sa init with-max-pasos 4 with-max-teorias 15.
		iterate = 0.
		plan-finished = 1. # así planificamos apenas empezamos

		teorias = 4 new Teorias.
		teorias{0} init named "Avanzar" with-action "adelante".
		teorias{0} set-dato-inicial name "sensor_f" value 0.
		teorias{0} set-dato-inicial name "sensor_b" value ANY.
		teorias{0} set-dato-inicial name "sensor_r" value ANY.
		teorias{0} set-dato-inicial name "sensor_l" value ANY.
		teorias{0} set-dato-inicial name "movido" value ANY.
		teorias{0} set-dato-final name "sensor_f" value ANY.
		teorias{0} set-dato-final name "sensor_b" value ANY.
		teorias{0} set-dato-final name "sensor_r" value ANY.
		teorias{0} set-dato-final name "sensor_l" value ANY.
		teorias{0} set-dato-final name "movido" value 1.

		teorias{1} init named "Retroceder" with-action "atras" executed 2.
		teorias{1} set-dato-inicial name "sensor_f" value 1.
		teorias{1} set-dato-inicial name "sensor_b" value ANY.
		teorias{1} set-dato-inicial name "sensor_r" value ANY.
		teorias{1} set-dato-inicial name "sensor_l" value ANY.
		teorias{1} set-dato-inicial name "movido" value ANY.
		teorias{1} set-dato-final name "sensor_f" value 0.
		teorias{1} set-dato-final name "sensor_b" value ANY.
		teorias{1} set-dato-final name "sensor_r" value ANY.
		teorias{1} set-dato-final name "sensor_l" value ANY.
		teorias{1} set-dato-final name "movido" value 1.

		teorias{2} init named "Rotar a derecha" with-action "derecha".
		teorias{2} set-dato-inicial name "sensor_f" value 1.
		teorias{2} set-dato-inicial name "sensor_b" value ANY.
		teorias{2} set-dato-inicial name "sensor_r" value ANY.
		teorias{2} set-dato-inicial name "sensor_l" value ANY.
		teorias{2} set-dato-inicial name "movido" value ANY.
		teorias{2} set-dato-final name "sensor_f" value 0.
		teorias{2} set-dato-final name "sensor_b" value ANY.
		teorias{2} set-dato-final name "sensor_r" value ANY.
		teorias{2} set-dato-final name "sensor_l" value 1.
		teorias{2} set-dato-final name "movido" value 0.

		teorias{3} init named "Rotar a izquierda" with-action "izquierda" executed 2.
		teorias{3} set-dato-inicial name "sensor_f" value 1.
		teorias{3} set-dato-inicial name "sensor_b" value ANY.
		teorias{3} set-dato-inicial name "sensor_r" value ANY.
		teorias{3} set-dato-inicial name "sensor_l" value ANY.
		teorias{3} set-dato-inicial name "movido" value ANY.
		teorias{3} set-dato-final name "sensor_f" value 0.
		teorias{3} set-dato-final name "sensor_b" value ANY.
		teorias{3} set-dato-final name "sensor_r" value 1.
		teorias{3} set-dato-final name "sensor_l" value ANY.
		teorias{3} set-dato-final name "movido" value 0.

		sa add teoria teorias{0}.
		sa add teoria teorias{1}.
		sa add teoria teorias{2}.
		sa add teoria teorias{3}.

		datos-finales{"movido"} = 1.
		sa update-datos-finales with datos-finales.

        +to iterate:

		# Actualiza entorno
		self update-entorno.

		# Chequeo de objetivo
		if (self near position posicion-final with-error 5.0):
		{
			print "Llegamos al FINAL!!!".
			self set-global-velocity to 0.
			return.
		}

		# Planificación
		if (plan-finished):
		{
			# Actualiza entorno indicando que no se movió para que
			# el planificador actue
			sa set-entorno value 0 with-name "movido".
			sa plan. # Si no tenemos plan, lo hacemos
			plan-finished = 0.
			iterate = 0.
			if (! (sa has-next-theory)):
			{
				plan-finished = 1.
				print "El planificador no encuentra PLAN!!!".
				return.
			}
		}

		# Ejecución de teoría
		if (iterate == 0):
		{
			posicion-inicial = (self get-location).
			if (sa has-next-theory):
			{
				teoria = sa get-next-theory.
				if ((teoria get-accion) == "adelante"):
				{
					avanzando = 1.
					retrocediendo = 0.
					girando_izq = 0.
					girando_der = 0.
				}
				if ((teoria get-accion) == "atras"):
				{
					avanzando = 0.
					retrocediendo = 1.
					girando_izq = 0.
					girando_der = 0.
				}
				if ((teoria get-accion) == "izquierda"):
				{
					avanzando = 0.
					retrocediendo = 0.
					girando_izq = 1.
					girando_der = 0.
				}
				if ((teoria get-accion) == "derecha"):
				{
					avanzando = 0.
					retrocediendo = 0.
					girando_izq = 0.
					girando_der = 1.
				}
			}
			else
			{
				plan-finished = 1.
			}
		}

		# Validación de teoría
		if (iterate == CELDAS_TURNO):
		{
			# Actualiza entorno segun si se movio o no
			if (self near position posicion-inicial with-error 2.0):
			{
				sa set-entorno value 0 with-name "movido".
			}
			else
			{
				sa set-entorno value 1 with-name "movido".
			}
			print iterate.
			if (!(sa validate theory teoria)):
			{
				plan-finished = 1.
			}
			iterate = 0.
		}
		else
		{
			iterate++.
		}

		# Movimiento del robot
		if (avanzando):
			self set-global-velocity to (CELDAS_MAX_VELOCITY).
		if (retrocediendo):
			self set-global-velocity to (-CELDAS_MAX_VELOCITY).
		if (girando_izq):
			self turn-left.
		if (girando_der):
			self turn-right.

}

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.
		val (float).

		val = self get-data.
		if (val > CELDAS_SENSOR_THRESHOLD): return 0.
		else           return 1.
        
        #+ to iterate:
        
	+ to get-data:
		i (object).
		min,dist (float).
                v,obs(vector).
		j (int).
		des2,des3(int).
		wallBegin,wallEnd,wallCenter (float).
                obsLoc (vector).                
                posObstacle,destiny,yo(vector).
                                             
           	draw clear.
		value = 0.0.
		j=0.
		min=0.
		foreach i in (all CeldasObstacles): 
			{
			 posObstacle=i get-location.
			 v = (body get-location) - (self get-location ).
			 obsLoc::y=posObstacle::y.
			 
			 if (dot((i get-direction),(1,0,0))):
			  {
	        	   obsLoc::x=((self get-location)::x + ((posObstacle::z - (self get-location)::z)*v::x/v::z)).
                           obsLoc::z=posObstacle::z.
			  }				
			  else
			  {
        	           obsLoc::z=((self get-location)::z + ((posObstacle::x - (self get-location)::x)*v::z/v::x)).
                           obsLoc::x=posObstacle::x.
			  } 
                                                		
			#!
			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(obsLoc,positiveDirection))):
					des2=1.      
                        }
			else
			{
                            if((dot((self get-location),positiveDirection))<(dot(obsLoc,positiveDirection))):
					des2=1.         
			}			


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

			
			yo=self get-location.
			destiny=i get-direction.

														

			if (dot((self get-location),(i get-direction)) > wallBegin) && (dot((self get-location),(i get-direction)) < wallEnd):
			    	des3=1.
			else
			{
				 des3=0.
                        	 
			}				
                       
			if ((des2) && (des3)):
                         {                                    
                        	draw clear.

				dist=|obsLoc - (self get-location)|.
				if( (j==0) || (min>dist) ):
				 {
 				 	min=dist.
				 	obs=obsLoc.
				 	j++.
					#print "sensor: $id obstaculo: $posObstacle direP: $destiny direS: $direction yo: $yo ".	
				 }

			 }						

                        
		} #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 min.
		 	}
		

                value = -1.
                return value.


}