@use PhysicalControl.
@use Link.
@use Stationary.
@use MultiBody.
@use GeneticAlgorithm.
#@use GeneticAlgorithmIndividual.
#@use Genome.

Controller myControl.

PhysicalControl : myControl
{
    + variables: 
        walker, ground, shape1 (object).
        creatures, objs (list).
        modifier (float).
        generation, numcreatures (int).
        
    + 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.
        
    + 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.
#        }
    + 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}.
        }
}

#Object : Brain
#{

#}

Object : CreatureGA
{
    + variables:
        modifier (float).
        
    + to init:
        modifier = (controller get-random-modifier).

    + 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.
    
    + 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)).
    
    + to calc-e0 with-angle1 j0 (float) with-angle2 j1 (float):
        return j1 * 1.5.
        
    + to calc-e1 with-angle1 j0 (float) with-angle2 j1 (float):
        return (self greater num j0 than (self wav)) * 1.5.
        
}

#GeneticAlgorithmIndividual : IndGA
#{

#    + variables:
#        g (object).
    
#    + to init:
#        g = new CreatureGenome.
    
#    + to compute-fitness start-point point1 (vector) end-point point2 (vector):
#        distance (float)
        
#        distance = sqrt((point1{0} - point2{0})^2 + (point1{1} - point2{1})^2 + (point1{2} - point2{2})^2).
#        self set-fitness to distance.
    
    #+ to copy:
    
    #+ to mutate:
    
    #+ to randomize:
    
#    + to crossover from-parent1 parent1 (object) from-parent2 parent2 (object):
#        g crossover from-parent-1 parent1 from-parent-2 parent2.

#}

#Genome : CreatureGenome
#{

#    + variables:
#        genomeData (5 floats).
    
    #+ to init:

#}

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).
        
    + 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.
        
    + to calc-fitness start-loc startloc (vector) end-loc endloc (vector):
        return sqrt((startloc{0} - endloc{0})^2 + (startloc{2} - endloc{2})^2).
    
    + 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.
        }
}