#For meta-layer hosing expiriment
@use Braitenberg7.
@use NeuralNetwork5.
@use NetworkServer.

@define PORT_NUM 2323.
@define MASTERHOST "192.168.0.100".
@define POP 4.                            #Maximum population
@define STATIC 20.                         #Maximum number of static blocks.
@define NCOUNT 30.                         #Brain size
@define AT_HOME 1.
@define LOAD 0.
@define SAVE 1.
@define TIME_C 10.
@define META 1.
@define MUTATE 50.			#Number of weights to alter
@define KINDS 2.			#Cardinal number of sensor types for setting start

Controller ManipulatorController.

BraitenbergControl2 : ManipulatorController {
	+ variables:

		vehicle (object).               #currently instantiated vehicle
                vehicles (list).                #for memtest
                brains (list).                  #POP of NeuralNetwork3's
                current (int).
                
                trialtime(int).
                trials, totaltrials, run(int).
                results(object).

		sns(int).			#ptr for sensors
                aft(int).			#ptr for affectors
                n(int).
                m(int).
                Size (vector).
                tempShape(object).
                temp(object).
                
                isthumus (object).
                homeurl(string).
                migrants (list).
                currentmigrant (int).
                currentslot (int).              #roller
                limb(int).
                
 		locked (int).
		lockMenu(object).               
	+ to init:
               super init.
                current = 0.
 		locked = 0.
                totaltrials = 0.
                trials = 0.
                limb = 0.
                sns = 0.
                aft = (NCOUNT - 1).
                trialtime = TIME_C.
		lockMenu = (self add-menu named "Lock Driver" for-method "toggle-driver-lock").
                self add-menu-separator.
                self add-menu named "Save Simulation" for-method "save-drivers".
                self add-menu named "Load Simulation" for-method "load-drivers".
                
                if !AT_HOME:{
                    isthumus = new NetworkServer.
                    isthumus listen on-port PORT_NUM.
                    homeurl = isthumus get-url.
                }
                else{
                    homeurl = "At_Home".
                }
                if !LOAD: {
                    self build-brains.
                    results = new Results.
                } else {
                    self load-drivers.
                    #self load-results.
                    results = new Results.#xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
                }
                #self start-new-manipulators number-of POP.
                #self schedule method-call "memtest" at-time (self get-time) + 1.0.               
                self init-statics with STATIC.
                vehicle = (self start-vehicle).
                vehicle set-all-kinds to 0.
                vehicle set-all-parent.
                vehicle center.
                #self set-positions.
                self schedule method-call "set-positions" at-time ((self get-time) + .99).                
                self schedule method-call "change-drivers" at-time ((self get-time) + trialtime).
                self watch item vehicle.
        + to change-drivers:
                fit(double).
                if locked: return.
                #evaluate current
                fit = (vehicle get-fitness).
                (brains{current}) set-fitness to fit.
                results add-fitness of fit at-trial totaltrials of-run run with-data-string ((brains{current}) get-homeland).
                #print "$current fitness $fit".
                #start next 
                current++.
                trials++.
                totaltrials++.
                if current == 4:{
                    self print-averages.
                    current = 0.
                    self breed-brains.
                    self pick-drivers.
                    #trialtime += 1.
                }
                
                #self reset-positions.
                #self schedule method-call "reset-positions" at-time ((self get-time) + .2).
                #vehicle center.
                vehicle free-all-connected-objects.
                free vehicle.
                print "Made it past freeing vehicle".
                free all Segments.
                self init-statics with STATIC.
                vehicle = (self start-vehicle).
                vehicle set-all-kinds to 0.
                vehicle set-all-parent.                
                vehicle = vehicle start-brain of (brains{current}).
                self schedule method-call "change-drivers" at-time ((self get-time) + trialtime).
 		self watch item vehicle.
 	+ to reset-positions:
 		vec(vector).
 		brains{current} re-states.
 		all BraitenbergSensor2 re-states.
 		all Segments return-to-start.
 		all Segments set-velocity to (0, 0, 0).
 		all Segments set-acceleration to (0, 0, 0).
 		all Segments set-force to (0, 0, 0).
 		all Link set-velocity to (0, 0, 0).
 		all Link set-acceleration to (0, 0, 0).
 		all Link set-force to (0, 0, 0).
 		#all MultiBody set-acceleration to (0, 0, 0).
 		all BraitenbergWheel2 reset-velocity.
 	+ to set-positions:
 		all Segments set-start-position.
 	+ to print-averages:
 		val(double).
 		x(int).
 		for x=2, x < totaltrials, x *= 2:{
 			val = (results get-average-fitness over-last x).
 			print "average over last $x was $val".
 		}
 	+ to display-current-driver:
                homeland(string).
                fit(double).
                homeland = "".
		temp = brains{current}.
                homeland = temp get-homeland.
                fit = temp get-fitness.
                self set-display-text to "Driver #$current $fit $homeland " at-x -.95 at-y -.9.
	+ to iterate:
		vehicle control at-time (self get-time).
                self display-current-driver.
		super iterate.
        + to breed-brains:
                tmp(list).
                n(int).
                id(int).
                obj(object).
                val(double).
                tmp {0} = brains {0}.
                tmp {1} = brains {1}.
                tmp {2} = brains {2}.
                tmp {3} = brains {3}.
                #for n = 0,n<|tmp|, n++:{
                #    obj = brains{n}.
                #    val = ((brains{n}) get-fitness).
                #    print "driver $n had fitness $val".
                #}
                sort tmp with compare-difference.
                print "tmp $tmp".
                print "brains $brains".
                tmp{3} crossover-omni-point from-parent-1 tmp{0} from-parent-2 tmp{1}.
                tmp{2} crossover-omni-point from-parent-1 tmp{1} from-parent-2 tmp{0}. 
                tmp{3} mutate-connections connects MUTATE.
                tmp{2} mutate-connections connects MUTATE.
                if META:{
                 	tmp{3} crossover-meta-omni-point from-parent-1 tmp{0} from-parent-2 tmp{1}.
                	tmp{2} crossover-meta-omni-point from-parent-1 tmp{1} from-parent-2 tmp{0}. 
                	tmp{3} mutate-meta-connections connects MUTATE.
                	tmp{2} mutate-meta-connections connects MUTATE.                
                }
                brains{0} = (tmp{0}).
                brains{1} = (tmp{1}).
                brains{2} = (tmp{2}).
                brains{3} = (tmp{3}).
                for n = 0, n < |tmp|, n++:{
                    obj = brains{n}.
                    val = ((brains{n}) get-fitness).
                    print "driver $n had fitness $val".
                }
                if SAVE:{
                    self save-drivers.
                    self save-results.
                }
                
	+ to save-drivers:
		driver (object).
                n = 0.
		foreach driver in brains: {
                        ((driver get-genome) save-as-xml file "native$homeurl$n.xml").
			n++.
		}
                n = 0.
                if !AT_HOME:{
                    foreach driver in migrants: {
                            ((driver get-genome) save-as-xml file "migrant$homeurl$n.xml").
                            n++.
                    }
                }
                n = 0.
                
	+ to load-drivers:
                home (string).
                obj (object).
                obj = new NeuralGenome2.
                brains = POP new NeuralNetwork4.
                if !AT_HOME: migrants = POP new NeuralNetwork4.
		for n=0, n < POP,n++: {
			obj load-from-xml file "native$homeurl$n.xml".
                        brains{n} start-with-genome genome obj.
                        if !AT_HOME:{
                            obj load-from-xml file "migrant$homeurl$n.xml".
                            migrants{n} start-with-genome genome obj.       #this is tricky if the simulation is reloading start migrants with local populace
                        }
                }
                self pick-drivers.
        + to load-results:
                results = new Results.
                results load-from-xml file "results$homeurl.xml".
                totaltrials = (results get-total-trials).
                run = (results get-total-runs) + 1.
        + to save-results:
                results save-as-xml file "results$homeurl.xml".
	+ to pick-drivers:
		# pick 4 new drivers at random.  we do this by sorting the 
                # list randomly and picking the first 4 items.
		sort brains with random-sort.
 	+ to random-sort objectA a (object) objectB b (object):
		return random[2] - 1. 
 	+ to compare-difference of a (object) with b (object):
		result (float).	
		result = (b get-fitness) - (a get-fitness).
		print "compare diferrence has result $result".
		return result.               
        + to build-brains:
                t(string).
                f(double).
                for n=0, n < POP, n++:{
                        temp = new NeuralNetwork4.
                        temp init-with neuron-count NCOUNT.
                        temp init-fully-connected-structure with-autapses 1.
                        temp randomize-weights between -1.0.
                        if META:{
                        	temp init-meta-with neuron-count NCOUNT.
                        }
                        f = (self get-real-time).
                        t = self convert-to-realtime from f.
                        #temp set-homeland of "$homeurl:$n:$t". 
                        push temp onto brains.                  #brains{n}=temp.
                        if !AT_HOME:{
                            temp = new NeuralNetwork4.
                            temp init-with neuron-count NCOUNT.
                            temp init-fully-connected-structure with-autapses 1.
                            temp randomize-weights between -1.0.
                            if META:{
                        	temp init-meta-with neuron-count NCOUNT.
                            }                            
                            f = (self get-real-time).
                            t = self convert-to-realtime from f.
                            temp set-homeland of "$homeurl:$n:$t".                         
                            migrants{n} = temp.
                        }
                }
                #print "build brains of mainpulator controller built $brains".
        + to memtest:
                foreach vehicle in vehicles:{
                    free vehicle.
                }
                self start-new-manipulators number-of 10.
                self schedule method-call "change-drivers-memtest" at-time (self get-time) + 1.0.
        + to init-statics with cubes(int):
  		for n=0, n < cubes, n++:{
		 	temp = new Segment.
                        #Size = (random[2.2] + .2, random[2.2] + .2, random[2.2] + .2).
                        Size = (2,2,2).
                        tempShape = (new Cube init-with size Size). 
                        temp set-shape to tempShape of Size.
                        temp set-color-integer to 1.
                        #temp move to (20 * sin(n * 6.28 / 10), 1, 20 * cos(n * 6.28 / 10)).
                        temp move to ( (n+10) * sin(n * 6.28 / 10), 1.5, (n+10) * cos(n * 6.28 / 10)).
                }
        + to start-new-manipulators number-of num(int):
                for n=0, n < num, n++:{
                    #push vehicle onto vehicles.
                    vehicle = (self start-vehicle).
                    vehicles{n} = vehicle.
                }
        + to start-vehicle:
                leftSensor, rightSensor (object).
		leftWheel, rightWheel (object).
                fleftWheel, frightWheel (object).
                bleftSensor, brightSensor(object).
                carLight (object).
 	limb = 0.
        sns = 0.
        aft = (NCOUNT - 1). 	
                Size = (4.0, .75, 3.0).
                tempShape = (new Cube init-with size Size).             
                temp = new Segment.
                temp set-shape to tempShape of Size.
                vehicle = new BraitenbergVehicle2.
                vehicle add-body with temp.
                vehicle move to (6, 6, (|vehicles| * 5) ).
#vehicle init-and-start-brain of NCOUNT.
                leftWheel = (vehicle add-wheel at (-1.0, 0, -1.6)).
                rightWheel = (vehicle add-wheel at (-1.0, 0, 1.6)).
                leftWheel set-natural-velocity to 0.0 .
                rightWheel set-natural-velocity to 0.0 .
                leftWheel set-start to aft.
                aft -= 1.
                rightWheel set-start to aft.
                aft -= 1.
                
                fleftWheel = (vehicle add-wheel at (1.0, 0, -1.6)).
                frightWheel = (vehicle add-wheel at (1.0, 0, 1.6)).
                fleftWheel set-natural-velocity to 0.0 .
                frightWheel set-natural-velocity to 0.0 .    
                fleftWheel set-start to aft.
                aft -= 1.
                frightWheel set-start to aft.
                aft -= 1.
#               frightWheel set-last.
                    
                rightSensor = (vehicle add-sensor at (2.0, .4, 1.65) of-kind 1 with-normal (1, 0, 0)).
                leftSensor = (vehicle add-sensor at (2.0, .4, -1.65) of-kind 1 with-normal (1, 0, 0)).
                leftSensor set-start to sns.
                sns += KINDS.
                rightSensor set-start to sns.
                sns += KINDS.
                #leftSensor set-bias to (15.0).
                #rightSensor set-bias to (15.0).
                ##rightSensor set-last.
                brightSensor = (vehicle add-sensor at (-2.0, .4, 1.65) of-kind 1 with-normal (1, 0, 0)).
                bleftSensor = (vehicle add-sensor at (-2.0, .4, -1.65) of-kind 1 with-normal (1, 0, 0)).
                bleftSensor set-start to sns.
                sns += KINDS.
                brightSensor set-start to sns.
                sns += KINDS.
                #bleftSensor set-bias to (15.0).
                #brightSensor set-bias to (15.0).
                bleftSensor set-direction to (0, -1, 0).
                brightSensor set-direction to (0, -1, 0).
                brightSensor set-last.                    
                ##vehicle add-light at (0,1.5, 0) of-kind 1. 
	        self add-arm of 2 at-location ( 0, .2, 1.5).	
                self add-arm of 2 at-location ( 0, .2, -1.5).

                vehicle init-and-start-brain of NCOUNT with-meta META.
                #deprecating type 2.
                #vehicle add-kind-sensors of 2. 
                vehicle add-kind-sensors of 3.
                return vehicle.
        + to convert-to-realtime from z (double):
                #Years Days Hours Minutes Seconds
                # by mod %??
                rtn (string).
                years(int).
                spy(int).
                days(int).
                spd(int).
                hours(int).
                sph(int).
                minutes(int).
                spm(int).
                seconds(double).
                spy = (60 * 60 * 24 * 365.25).
                spd = (60 * 60 * 24).
                sph = (60 * 60).
                spm = 60.
                years = z / spy.
                days = z - (spy * years).
                days = days / spd.
                hours = z - ((spy * years) + (spd * days)).
                hours = hours / sph.
                minutes = z - ((spy * years) + (spd * days) + (sph * hours)).
                minutes = minutes / spm.
                seconds = z - ((spy * years) + (spd * days) + (sph * hours) + (spm * minutes)).
                #seconds = seconds / 60.
                years = years + 1970.
                hours = ((hours - 5) % 24).#for E.S.T.
                rtn = "$years/$days/$hours:$minutes:$seconds".
                return rtn.
        + to add-arm of num-segs(int) at-location loc(vector):
        	armSegment(object).
        	tiptop, tipbot, tipleft, tipright (object).        	
                Size = (.2,2,.2).
	        tempShape = (new Cube init-with size Size).
	        armSegment = new BraitenbergWheel2.
	        armSegment set-shape to tempShape of Size.
	        armSegment set-start to aft.
	        aft -= 1.
	        vehicle add-new-arm.
	        vehicle add-arm-segment of armSegment to-limb limb at-parent-location (loc)
	        	with-child-location (0, -1.0  ,0) with-normal (0,1,0) with-rotation (0,1,0).
	        for m=0, m < num-segs, m++:{
	        	Size = (.2,2,.2).
	                tempShape = (new Cube init-with size Size).
	                armSegment = new BraitenbergWheel2.
	                armSegment set-start to aft.
	                aft -= 1.
	                armSegment set-shape to tempShape of Size.                  
	                vehicle add-arm-segment of armSegment to-limb limb at-parent-location (0,1.0,0)
	                	with-child-location (0, -1.0  ,0) with-normal (0,0,1) with-rotation (0,0,1). 
                }
                tiptop = (armSegment add-sensor at (.2, .8, 0) of-kind 1 with-normal (0, 1, 0)).
                tipbot = (armSegment add-sensor at ( -.2, .8, 0) of-kind 1 with-normal (0, 1, 0)).
                tipleft = (armSegment add-sensor at ( 0, .8, -.2) of-kind 1 with-normal (0, 1, 0)).
                tipright = (armSegment add-sensor at ( 0, .8, .2) of-kind 1 with-normal (0, 1, 0)).
                vehicle integrate-outboard-sensor obj tiptop.  #push tiptop onto sensors.
                vehicle integrate-outboard-sensor obj tipbot. #push tipbot onto sensors.
                vehicle integrate-outboard-sensor obj tipright. #push tipleft onto sensors.
                vehicle integrate-outboard-sensor obj tipleft. #push tipright onto sensors.
                tiptop set-start to sns.
                sns += KINDS.
                tipbot set-start to sns.
                sns += KINDS.
                tipleft set-start to sns.
                sns += KINDS.
                tipright set-start to sns.
                sns += KINDS.
                #tiptop set-bias to (15.0).
                #tipbot set-bias to (15.0).
                #tipleft set-bias to (15.0).
                #tipright set-bias to (15.0).
                tipright set-last.               
                armSegment add-tip at ( 0, 1.0, 0) of-kind 1 with-normal (0, 1, 0).                
                limb += 1.
        + to toggle-driver-lock:
		if locked == 1: {
			locked = 0.
			vehicle center.
			self schedule method-call "change-drivers" at-time (self get-time) + 30.0.
			lockMenu uncheck.
		} else {
			locked = 1.
			lockMenu check.
		}
}
Data : Results {
    + variables:        
        fits (list).
    + to add-fitness of fit(double) at-trial t (int) of-run r (int) with-data-string stuff (string):
        tmp(list).
        push fit onto tmp.
        push t onto tmp.
        push r onto tmp.
        push stuff onto tmp.
        push tmp onto fits.
    + to get-total-trials:
        rtn(int).
        tmp(list).
        tmp = fits{|fits| - 1}.
        rtn = tmp{1}.
        return rtn.
    + to get-total-runs:
        rtn(int).
        tmp(list).
        tmp = fits{|fits| - 1}.
        rtn = tmp{2}.
        return rtn.
    + to get-average-fitness over-last trials(int):
        tot(double).
        n(int).
        tmp(list).
        for n = (|fits| - 1), n > (|fits| - trials), n--:{
            tmp = fits{n}.
            tot += tmp{0}.
        }
        return (tot / (trials - 1)).
    + to set-to fitness obj (list):
        fits = obj.       
    + to get-fits:
        return fits.
}