@use PhysicalControl.
@use Shape.
@use Stationary.
@use Link.
@use MultiBody.
@use NeuralNetwork3.
@define BRAITENBERG_MAX_VELOCITY 10.

PhysicalControl : BraitenbergControl2 {
	% This class is used for building simple Braitenberg vehicle 
	% simulations.  To create a Braitenberg vehicle simulation, 
	% subclass BraitenbergControl and use the init method to 
	% create OBJECT(BraitenbergLight) and 
	% OBJECT(BraitenbergVehicle) 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 (6, 6, 24).

		self enable-shadows.
		self enable-reflections.
                
                self enable-fast-physics.
		self set-fast-physics-iterations to 5.

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

MultiBody : BraitenbergVehicle2 (aka BraitenbergVehicles2) {
	% This object is used in conjunction with OBJECT(BraitenbergControl) to
	% create simple Braitenberg vehicles.

	+ variables:
		bodyShape (object).
		wheelShape (object).
		sensorShape (object).
                armShape(object).
                lightShape(object).
		bodyLink (object).
                
                buildPointer (object).
                
                brain (object).
                
		wheels (list).
		sensors (list).
                lights(list).
                arms(list).
                type(int).
	+ to destroy:
		free sensorShape.
		free wheelShape.
		free bodyShape.
                free armShape.
                free lightShape.
                if bodyLink: free bodyLink.
                if buildPointer: free buildPointer.
                if brain: free brain.

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

		wheelShape = new Shape.
		wheelShape init-with-polygon-disk radius .6 sides 40 height .1.

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

                armShape = new Shape.
                armShape init-with-cube size (0.1, 1.0, 0.1).
                
                lightShape = new Shape.
                lightShape init-with-sphere radius 0.3.

		self move to (0, 1.0, 0).
		self set-texture-scale to 1.5.
                super enable-self-collisions.
        # brain management
        + to init-and-start-brain of size(int):
                if brain: free brain.
                brain = new NeuralNetwork3.
                brain init-with neuron-count size.
                brain init-random-structure with-connectivity-percent 1.0 with-autapses 0.
		#self add-dependency on brain.
                brain randomize-weights between -1.0.              
                if wheels: wheels set-brain to brain.
                if arms: arms set-brain to brain.
                if sensors: sensors set-brain to brain.
                if lights: lights set-brain to brain.
                
        + to start-brain of b(object):
                if brain: free brain.
                brain = b.
                if wheels: wheels set-brain to brain.
                if sensors: sensors set-brain to brain.
                if lights: lights set-brain to brain.
                if arms: arms set-brain to brain.
        + to set-brain of b(object):
                if brain: free brain.
                brain = b.
        + to get-brain:
                return brain.
	+ section "Adding Wheels, Lights, Arms, Sensors to a Vehicle"

        + to add-body with seg(object):
 		bodyLink = seg.         #new Segment.
		#bodyLink set-shape to bodyShape.	
		bodyLink set-mu to -1.0.
		bodyLink set-eT to .8.
                bodyLink set-color-integer to 0.

		self set-root to bodyLink.
                buildPointer = bodyLink.
                
	+ to add-wheel at location (vector):
		% Adds a wheel at location on the vehicle.  This method returns
		% the wheel which is created, a OBJECT(BraitenbergWheel).  You'll
		% use the returned object to connect it to the vehicle's sensors.

		wheel, joint (object).

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

		joint = new RevoluteJoint.
                wheel set-joint to joint.
                wheel rotate around-axis (1,0,0) by 1.57 .
		joint set-relative-rotation around-axis (1, 0, 0) by 1.57.
		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).
		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) of-type t (int) with-normal norm (vector):
		% Adds a sensor at location on the vehicle.  This method returns
		% the sensor which is created, a OBJECT(BraitenbergSensor).  You'll
		% use the returned object to connect it to the vehicle's wheels.

		sensor, joint (object).

		sensor = new BraitenbergSensor2.
                sensor add-type of t.
		sensor set-shape to sensorShape.
                sensor rotate around-axis (0,0,1) by 1.57 .
		joint = new RevoluteJoint.

		joint set-relative-rotation around-axis (0, 0, 1) by -1.57.
		joint link parent bodyLink to-child sensor with-normal norm
					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 add-light at location (vector) of-type t (int):
                light, joint(object).
                
                light = new BraitenbergLight2.
                light set-shape to lightShape.
                light set-color-integer to t.
                self set-type to t.
                
                joint = new RevoluteJoint.
                
                joint link parent bodyLink to-child light with-normal (1, 0, 0)
					with-parent-point location with-child-point (0, 0, 0).
		
                self add-dependency on joint.  
                self add-dependency on light.
                
                push light onto lights.
        + to add-arm-segment of seg (object) at-parent-location location (vector) with-child-location childPoint (vector) with-normal norm (vector) with-rotation rot(vector):
		% Adds a wheel at location on the vehicle.  This method returns
		% the wheel which is created, a OBJECT(BraitenbergWheel).  You'll
		% use the returned object to connect it to the vehicle's sensors.

		joint (object).

		joint = new RevoluteJoint.
                seg set-joint to joint.
                seg rotate around-axis (1,0,0) by 1.57 .
		joint set-relative-rotation around-axis rot by 1.57.
		joint link parent buildPointer to-child seg with-normal norm
					with-parent-point location with-child-point childPoint.
                seg set-joint to joint.
		joint set-strength-limit to (joint get-strength-hard-limit).

		self add-dependency on joint.
		self add-dependency on seg.
                buildPointer = seg.

		push seg onto arms.

		return seg.               
        + to set-type to t(int):
                type = t.
                lights set-color-integer to t.
        + to get-type:
                return type.
        
}
Link : Segment (aka Segments) {
        + variables:
                joint(object).
                hull(object).
                brain(object).                  #pointer to brain
                start(int).                     #start of sensors input space or start of output vector space on brain 
                last(int).                      #signify that it is the last segment to use the brain therefor should reset states
                color(int).
        + to destroy:
                if hull: free hull.
                if joint: free joint.
                super destroy.
        + to set-brain to b(object):
                if brain: free brain.
                brain = b.
        + to set-start to d(int):
                start = d.
        + to set-shape to s(object):
                hull = s.
                super set-shape to s.
        + to scale by scale (vector):
                hull scale by scale.
        + to set-joint to j(object):
                joint = j.
        + to set-last:
                last = 1.
        + to get-point-on-shape on-vector theVector (vector):
    #This method is experimental.
    #Starting from inside the shape at the center, this function goes in the direction of theVector until it hits the edge of the shape. The resulting point is returned.
    #This allows you to compute link points for arbitrary shapes. For example, if you want to compute a link point for the "left-most" point on the shape, you can call this method with (-1, 0, 0).
    #Returns (0, 0, 0) if the shape is not initialized or if an error occurs.                 
                return (hull get-point-on-shape on-vector theVector).
        + to set-color-integer to col(int):
                color = col.                
                if col == 0: self set-color to (0,1,0).
                if col == 1: self set-color to (1,0,0).
                if col == 2: self set-color to (0,0,1).
                if col == 3: self set-color to (0,0,0).
                if col == 4: self set-color to (1,1,1).
        + to get-color-integer:
                return color. 
	+ to add-sensor at location (vector) of-type t (int) with-normal norm (vector):
		% Adds a sensor at location on the vehicle.  This method returns
		% the sensor which is created, a OBJECT(BraitenbergSensor).  You'll
		% use the returned object to connect it to the vehicle's wheels.

		sensor, joint (object).
                sensorShape (object).
		sensorShape = new Shape.
		sensorShape init-with-polygon-cone radius .1 sides 10 height .25.
		sensor = new BraitenbergSensor2.
                sensor add-type of t.
		sensor set-shape to sensorShape.
                sensor rotate around-axis (0,0,1) by 1.57 .
		joint = new RevoluteJoint.
		joint set-relative-rotation around-axis (0, 0, 1) by -1.57.
		joint link parent self to-child sensor with-normal norm
					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).

		return sensor.
}
Segment : BraitenbergLight2 (aka BraitenbergLights2) {
	% A BraitenbergLight is used in conjunction with OBJECT(BraitenbergControl)
	% and OBJECT(BraitenbergVehicle).  It is what the OBJECT(BraitenbergSensor)
	% objects on the BraitenbergVehicle detect.
	% <p>
	% There are no special behaviors associated with the lights--they're 
	% basically just plain OBJECT(Mobile) objects.

	+ to init:
		self set-shape to (new Shape init-with-sphere radius .3).
                super set-color-integer to 0.                             # Until we change it's flavor.
}
Segment : BraitenbergWheel2 (aka BraitenbergWheels2) {
	% A BraitenbergWheel is used in conjunction with OBJECT(BraitenbergVehicle)
	% to build Braitenberg vehicles.  This class is typically not instantiated
	% manually, since OBJECT(BraitenbergVehicle) creates one for you when you
	% add a wheel to the vehicle.
	% <p>
	% <b>NOTE: this class is included as part of the file "Braitenberg.tz".</b>

	+ variables:
		joint (object).
		naturalVelocity (float).
		newVelocity, oldVelocity (float).

	+ to init:
		naturalVelocity = 0.
		newVelocity = 0.
        + to destroy:
                super destroy.
	- to set-joint to j (object):
		% Used internally.

		joint = j.
        + to add-sensor at location (vector) of-type t (int) with-normal norm (vector):
                super add-sensor at location of-type t with-normal norm.
	+ section "Configuring the Wheel's Natural Velocity"

	+ to set-natural-velocity to n (float):
		% Sets the "natural" velocity of this wheel.  The natural velocity
		% is the speed at which the wheel turns in the absence of sensor
		% input.  

		naturalVelocity = n.

	+ to activate with-input n (float):
		% Used internally.

		newVelocity = n.

	+ to iterate:
                if brain: newVelocity = (brain get-neuron-state at start).
		if newVelocity > BRAITENBERG_MAX_VELOCITY: newVelocity = BRAITENBERG_MAX_VELOCITY.
		if newVelocity < -BRAITENBERG_MAX_VELOCITY: newVelocity = -BRAITENBERG_MAX_VELOCITY.
                
		# this oldVelocity stuff is used to limit the deceleration so that 

		if newVelocity == 0: {
			joint set-joint-velocity to oldVelocity.
			oldVelocity *= .95.
		} else {
			joint set-joint-velocity to newVelocity.
			oldVelocity = newVelocity.
		}

		newVelocity = naturalVelocity.
                #if last: brain re-states.
}

Segment : BraitenbergSensor2 (aka BraitenbergSensors2) {
	% A BraitenbergSensor is used in conjunction with OBJECT(BraitenbergVehicle)
	% to build Braitenberg vehicles.  This class is typically not instantiated
	% manually, since OBJECT(BraitenbergVehicle) creates one for you when you
	% add a sensor to the vehicle.
	% <p>
	% <b>NOTE: this class is included as part of the file "Braitenberg.tz".</b>

	+ variables:
		wheels (list).
		bias (float).
		direction (vector).
		sensorAngle (float).
		activationObject (object).
		activationMethod (string).
                types(list).
                strengths(list).
                cnt(int).
                
        + to destroy:
                if activationObject: free activationObject.
                super destroy.

	+ to init:
		bias = 1.0.
		direction = (0, 1, 0).
		sensorAngle = 1.0.

  + section "Linking the Sensor to a Wheel"

	+ to link to w (object):
		% Associates this sensor with wheel w.

		push w onto wheels.

  + section "Configuring the Sensor Values"
        + to add-type of d (int):
                push d onto types.
                push 0 onto strengths.
        + to get-types:
                return types.
        + to get-strengths:
                return strengths.           
        + to set-direction to d(vector):
                direction = d.
	+ to set-bias to d (float):
		% Sets the "bias" of this sensor.  The default bias is 1, meaning
		% that the sensor has a positive influence on associated wheels
		% with strength 1.  You can change this to any magnitude, positive
		% or negative.

		bias = d.
	
	+ to set-sensor-angle to n (float):
		% Sets the angle in which this sensor can detect light.  The default
		% value of 1.5 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-activation-method to m (string) in-object o (object):
		% This method specifies an activation method for the sensor.  An
		% activation method is a method which takes as input the strength
		% read by the sensor, and as output returns the strength of the 
		% signal which will travel on to the motor.
		% <p>
		% Your activation function should be defined as:
		% <pre>
		%     + to <i>activation-function-name</i> with-sensor-strength s (float):
		% </pre>
		% <p>
		% The default activation method is linear, but more complex vehicles
		% may require non-linear activation functions.
		%

		activationMethod = m.
		activationObject = o.

	+ to iterate:
		i (object).
		lights (int).
		total, strength, angle (float).
		toLight, transDir (vector).
                n(int).

		transDir = (self get-rotation) * direction.

		for n=0, n<|types|, n++:{
                    strengths{n}=0.
                }
                for n=0, n<|types|, n++:{
                    foreach i in (all Segments): {
                        if ((i get-color-integer) == (types{n}) ):{
                            toLight = (i get-location) - (self get-location).
                            angle = angle(toLight, transDir).

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

				if activationMethod && activationObject: {
					strength = (activationObject call-method named activationMethod with-arguments { strength }).
				} 

                                if strength > 10: strength = 10.

				total += strength.

				lights++.
                            }
                        }
                        strengths{n} = strengths{n} + total.
                        total = 0.
                    }
                }

		if lights != 0: total /= lights.

		total = 50 * total * bias.
                for n = start, n < (|strengths| + start), n++:{
                    if brain: brain set-neuron-state at n to strengths{n - start}.
                    strengths{n - start} = 0.
                }
		#wheels activate with-input total.
                #if last: brain update-state.
                if last  &&  (cnt % 10 == 1): {
                    if brain: brain update-state.
                    #cnt++.
                }
                else{
                        cnt++.
                }
                #if (cnt % 100 == 1 && last == 1): 
                if(cnt % 10 == 9)&& brain: brain re-states.
                if cnt == 11: cnt = 0.
                #print "here is the damn counter $cnt".
}