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

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

	- to get-density:
		return 20.0.

	- to get-wheel-width:
		return 0.4.

	- to get-wheel-radius:
		return 0.8.
}

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.

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

	+ 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.
}

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

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

		joint set-joint-velocity to n.

	+ to get-velocity:
		% Gets the velocity of this wheel.
		
		return (joint get-joint-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).
		sensorAngle (float).
		value (float).

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

  + section "Configuring the Sensor Values"

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

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

		return value.

	+ to iterate:
		i (object).
		strength, angle (float).
		toObstacle, transDir (vector).

		transDir = (self get-rotation) * direction.

		value = 0.0.

		foreach i in (all CeldasObstacles): {
			toObstacle = (i get-location) - (self get-location).
			angle = angle(toObstacle, transDir).

			if angle < sensorAngle: {
				strength = | (self get-location) - (i get-location) |.
				strength = 100.0 / (strength * strength) .

				if strength > value: value = strength.
			}
		}

}