# Kelley Hecker
# Evolving Motor Techniques for Artificial Life
# Computer Systems 2007-2008
# Period 7

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

Controller myControl.

# The Controller object that runs the simulation.
PhysicalControl : myControl
{
    + variables: 
        walker, ground, shape1 (object).
        creatures, objs (list).
        modifier (float).
        generation, numcreatures (int).
    
    # Initializes the simulation. Sets of variables and creates the
    # first walker
    + to init:
        generation = 1.
        numcreatures = 0.
        ground = new Floor.
        walker = new SimpleWalker.
        walker init-with with-generation generation with-velocities {0, 0}.
        walker move to (0, 10, 0).
        self watch item walker.
        
        modifier = random[1.0].
        
    + to get-random-modifier:
        return modifier.
    
    # Called when ten creatures have been tested and added to the
    # creatures list. It should compare each creatures' fitness value
    # and select the best two, passing their genes on to the next
    # generation. The rest of the creatures should be neuron-controlled, 
    # and the simulation should repeat its tests of ten creatures. Right
    # now, I believe this is the method that is causing the freeze error.
    + to next-generation:
        parent1, parent2 (list).
        rand, x (int).
    
        generation++.
        parent1 = creatures{0}.
        parent2 = creatures{1}.
        print parent1.
        print parent2.
        self end-simulation.
        rand = random[10].
        objs = 10 new SimpleWalker.
        for x = 0, x<rand, x++:
        {
            objs{x} init-with with-generation generation with-velocities (parent1{1}+parent2{1}/2)*random[(2.0, 2.0, 2.0)].
        }
        for x = rand, x<10, x++:
        {
            objs{x} init-with with-generation 1.
        }

    # Adds the creature that was just tested to the list of creatures. 
    # Each element in the list contains a list of the creature's velocity 
    # at each timestep, and the fitness level of the creature.
    + to add-creature with-velocity vel (list) with-fitness fit (float) with-creature walker (object):
        temp (list).
        x (int).
        
        numcreatures++.
        free walker.
        
        push fit onto temp.
        push vel onto temp.
        if | creatures | == 0:
        {
            push temp onto creatures.
        }
        else
        {
            for x = 0, x < | creatures |, x++:
            {
                if creatures{x}{0} < temp{0}:
                {
                    insert temp at creatures{x}.
                }
            }
        }
        
        if numcreatures == 10:
        {
        print creatures{0}.
        print creatures{1}.
        }
        else
        {
        walker = new SimpleWalker.
        walker init-with with-generation 1 with-velocities {0,0}.
        }
}

# The "brain" of neuron-controlled creatures.
Object : CreatureGA
{
    + variables:
        modifier (float).
        
    + to init:
        modifier = (controller get-random-modifier).
    
    # Returns the effector values for the creature after being modified
    # by the neurons.
    + to update with-angle1 j0 (float) with-angle2 j1 (float):
        effectors (list).
        
        effectors{0} = (self calc-e0 with-angle1 j0 with-angle2 j1).
        effectors{1} = (self calc-e1 with-angle1 j0 with-angle2 j1).
        return effectors.
    
    # Various neuron functions.
    + to saw:
        return ((controller get-time)*modifier) - ((controller get-time)*modifier)%1.
        
    + to sum of x (float) and y (float):
        return x + y.
        
    + to product of x (float) times y (float):
        return x * y.
        
    + to divide num x (float) by y (float):
        return x / y.
        
    + to greater num x (float) than y (float):
        if x > y:
        {
            return x.
        }
        else
        {
            return y.
        }
        
    + to abs num x (float):
        return abs(x).
        
    + to wav:
        return sin((controller get-time)%30 * 2 * acos(-1)).
    
    # Calculates the effector value of E0.
    + to calc-e0 with-angle1 j0 (float) with-angle2 j1 (float):
        return j1 * 1.5.
    
    # Calculates the effector value of E1.
    + to calc-e1 with-angle1 j0 (float) with-angle2 j1 (float):
        return (self greater num j0 than (self wav)) * 1.5.
        
}

# The basic walker used in simulations.
MultiBody : SimpleWalker
{
    + variables:
        legSize, legColor, bodySize, bodyColor, jointLocBody, jointLocLeg (vector).
        numLegs, numJoints, x, age, generation (int).
        legs, joints, velocities (list).
        legShape, bodyShape, body (object).
        creatureGA (object).
        startloc (vector).
    
    # Creates the creature. If the creature is to be neuron-controlled 
    # its generation will be 1 and its velocities {0, 0}. If it is 
    # gene-controlled, it will have its parents' generation plus one, and 
    # the average of its parents' velocities.
    + to init-with with-generation gen (int) with-velocities vels (list):
        velocities = vels.
        generation = gen.
        creatureGA = new CreatureGA.
        
        jointLocBody = (-.25, 0, 0).
        jointLocLeg = (1, 0, 0).

        numLegs = 2.
        numJoints = numLegs.
    
        legSize = (2, .5, .5).
        legShape = (new Cube init-with size legSize).
        legColor = random[(1.0, 1.0, 1.0)].
        legs = numLegs new Link.
        legs set-shape to legShape.
        legs set-color to legColor.
        
        bodySize = (.5, .5, .5).
        bodyShape = (new Cube init-with size bodySize).
        bodyColor = random[(1.0, 1.0, 1.0)].
        body = new Link.
        body set-shape to bodyShape.
        body set-color to bodyColor.
        
        joints = numJoints new UniversalJoint.
        
        for x=0, x < numJoints, x++:
        {
            if x%2 == 0:
            {
                joints{x} link parent body to-child legs{x} with-normal (0, 1, 0) with-parent-point jointLocBody with-child-point jointLocLeg.
            }
            else
            {
                joints{x} link parent body to-child legs{x} with-normal (0, 1, 0) with-parent-point -jointLocBody with-child-point -jointLocLeg.
            }
        }
        
        self set-root to legs{0}.
        joints set-strength-limit to 300.
        
        joints set-joint-velocity axis-1 random[1.0] axis-2 random[1.0].
        #joints set-joint-velocity axis-1 0.0 axis-2 1.0.
        
        startloc = self get-location.
    
    # Calculates the fitness of a creature by finding its distance 
    # travelled.
    + to calc-fitness start-loc startloc (vector) end-loc endloc (vector):
        return sqrt((startloc{0} - endloc{0})^2 + (startloc{2} - endloc{2})^2).
    
    # Calculates the effector values of the creature if it is 
    # neuron-controlled; Otherwise, the creature will move based on the 
    # velocities given by its parents.
    + to iterate:
        x (int).
        temp (float).
        effectors (list).
        currentloc (vector).
        fitness (int).
        
        if generation == 1:
        {
        for x=0, x < numLegs, x++:
        {
            effectors = creatureGA update with-angle1 (joints{x} get-joint-angles){0} with-angle2 (joints{x} get-joint-angles){1}.
            if x%2 != 0:
            {
                temp = effectors{0}.
                effectors{0} = effectors{1}.
                effectors{1} = temp.
                push effectors onto velocities.
            }
            joints{x} set-joint-velocity axis-1 effectors{0} axis-2 effectors{1}.
        }
        }
        else
        {
            for x=0, x < numLegs, x++:
            {
                joints{x} set-joint-velocity axis-1 velocities{0} axis-2 velocities{1}.
            }
        }
        
        controller set-display-text to "Age: $age , Generation: $generation" at-x -.95 at-y -.95.
        
        currentloc = self get-location.
        age++.
        
        if age == 100:
        {
            fitness = self calc-fitness start-loc startloc end-loc currentloc.
            controller add-creature with-velocity velocities with-fitness fitness with-creature self.
        }
}