Double pole balacing

////////////////////////////////////////////////////////////////////////////////////////

pole2startgenes

genomestart 1

trait 1 0.1 0 0 0 0 0 0 0

node 1 0 1 1

node 2 0 1 1

node 3 0 1 1

node 4 0 1 3

node 5 0 0 2

gene 1 1 5 0.0 0 1 0 1

gene 1 2 5 0.0 0 2 0 1

gene 1 3 5 0.0 0 3 0 1

gene 1 4 5 0.0 0 4 0 1

genomeend 1

////////////////////////////////////////////////////////////////////////////////////////

pole2startgenes1

genomestart 1

trait 1 0.1 0 0 0 0 0 0 0

trait 2 0.2 0 0 0 0 0 0 0

trait 3 0.3 0 0 0 0 0 0 0

node 1 0 1 1

node 2 0 1 1

node 3 0 1 1

node 4 0 1 1

node 5 0 1 1

node 6 0 1 1

node 7 0 1 3

node 8 0 0 2

gene 1 1 8 0.0 0 1 0 1

gene 2 2 8 0.0 0 2 0 1

gene 3 3 8 0.0 0 3 0 1

gene 1 4 8 0.0 0 4 0 1

gene 2 5 8 0.0 0 5 0 1

gene 2 6 8 0.0 0 6 0 1

gene 2 7 8 0.0 0 7 0 1

genomeend 1

////////////////////////////////////////////////////////////////////////////////////////

p2test.ne

trait_param_mut_prob 0.5 

trait_mutation_power 1.0

linktrait_mut_sig 1.0

nodetrait_mut_sig 0.5

weight_mut_power 1.0

recur_prob 0.05

disjoint_coeff 1.0

excess_coeff 1.0

mutdiff_coeff 7.0

compat_thresh 9.0

age_significance 1.0

survival_thresh 0.4

mutate_only_prob 0.25

mutate_random_trait_prob 0.1

mutate_link_trait_prob 0.1

mutate_node_trait_prob 0.1

mutate_link_weights_prob 0.8

mutate_toggle_enable_prob 0.0

mutate_gene_reenable_prob 0.00

mutate_add_node_prob 0.01

mutate_add_link_prob 0.3

interspecies_mate_rate 0.01

mate_multipoint_prob 0.6

mate_multipoint_avg_prob 0.4

mate_singlepoint_prob 0.0

mate_only_prob 0.2

recur_only_prob 0.2

pop_size 1000

dropoff_age 15

newlink_tries 20

print_every 60

babies_stolen 0

num_runs 1

////////////////////////////////////////////////////////////////////////////////////////

pole2_markov.ne

trait_param_mut_prob 0.5 

trait_mutation_power 1.0

linktrait_mut_sig 1.0

nodetrait_mut_sig 0.5

weigh_mut_power 2.5

recur_prob 0.00

disjoint_coeff 1.0

excess_coeff 1.0

mutdiff_coeff 0.4

compat_thresh 3.0

age_significance 1.0

survival_thresh 0.20

mutate_only_prob 0.25

mutate_random_trait_prob 0.1

mutate_link_trait_prob 0.1

mutate_node_trait_prob 0.1

mutate_link_weights_prob 0.9

mutate_toggle_enable_prob 0.00

mutate_gene_reenable_prob 0.000

mutate_add_node_prob 0.03

mutate_add_link_prob 0.05

interspecies_mate_rate 0.001

mate_multipoint_prob 0.6

mate_multipoint_avg_prob 0.4

mate_singlepoint_prob 0.0

mate_only_prob 0.2

recur_only_prob 0.0

pop_size 150

dropoff_age 15

newlink_tries 20

print_every 5

babies_stolen 0

num_runs 1

////////////////////////////////////////////////////////////////////////////////////////

#ifndef EXPERIMENTS_H

#define EXPERIMENTS_H

#include <iostream>

#include <iomanip>

#include <fstream>

#include <sstream>

#include <list>

#include <vector>

#include <algorithm>

#include <cmath>

#include <string>

#include "neat.h"

#include "network.h"

#include "population.h"

#include "organism.h"

#include "genome.h"

#include "species.h"

using namespace std;

using namespace NEAT;

//Double pole balacing evolution routines ***************************

class CartPole;

Population *pole2_test(int gens,int velocity);

bool pole2_evaluate(Organism *org,bool velocity,CartPole *thecart);

int pole2_epoch(Population *pop,int generation,char *filename,bool velocity, CartPole *thecart,int &champgenes,int &champnodes, int &winnernum, ofstream &oFile);

//rtNEAT validation with pole balancing *****************************

Population *pole2_test_realtime();

int pole2_realtime_loop(Population *pop, CartPole *thecart);

class CartPole {

public:

  CartPole(bool randomize,bool velocity);

  virtual void simplifyTask();

  virtual void nextTask();

  virtual double evalNet(Network *net,int thresh);

  double maxFitness;

  bool MARKOV;

  bool last_hundred;

  bool nmarkov_long;  //Flag that we are looking at the champ

  bool generalization_test;  //Flag we are testing champ's generalization

  double state[6];

  double jigglestep[1000];

protected:

  virtual void init(bool randomize);

private:

  void performAction(double output,int stepnum);

  void step(double action, double *state, double *derivs);

  void rk4(double f, double y[], double dydx[], double yout[]);

  bool outsideBounds();

  const static int NUM_INPUTS=7;

  const static double MUP = 0.000002;

  const static double MUC = 0.0005;

  const static double GRAVITY= -9.8;

  const static double MASSCART= 1.0;

  const static double MASSPOLE_1= 0.1;

  const static double LENGTH_1= 0.5;  /* actually half the pole's length */

  const static double FORCE_MAG= 10.0;

  const static double TAU= 0.01;  //seconds between state updates

  const static double one_degree= 0.0174532; /* 2pi/360 */

  const static double six_degrees= 0.1047192;

  const static double twelve_degrees= 0.2094384;

  const static double fifteen_degrees= 0.2617993;

  const static double thirty_six_degrees= 0.628329;

  const static double fifty_degrees= 0.87266;

  double LENGTH_2;

  double MASSPOLE_2;

  double MIN_INC;

  double POLE_INC;

  double MASS_INC;

  //Queues used for Gruau's fitness which damps oscillations

  int balanced_sum;

  double cartpos_sum;

  double cartv_sum;

  double polepos_sum;

  double polev_sum;

};

#endif

////////////////////////////////////////////////////////////////////////////////////////

/* ------------------------------------------------------------------ */

/* Double pole balacing                                               */

/* ------------------------------------------------------------------ */

//Perform evolution on double pole balacing, for gens generations

//If velocity is false, then velocity information will be withheld from the

//network population (non-Markov)

Population *pole2_test(int gens,int velocity) {

    Population *pop;

    Genome *start_genome;

    char curword[20];

    int id;

    ostringstream *fnamebuf;

    int gen;

    CartPole *thecart;

    //Stat collection variables

    int highscore;

    int record[NEAT::num_runs][1000];

    double recordave[1000];

    int genesrec[NEAT::num_runs][1000];

    double genesave[1000];

    int nodesrec[NEAT::num_runs][1000];

    double nodesave[1000];

    int winnergens[NEAT::num_runs];

    int initcount;

    int champg, champn, winnernum;  //Record number of genes and nodes in champ

    int run;

    int curtotal; //For averaging

    int samples;  //For averaging

    ofstream oFile("statout",ios::out);

    champg=0;

    champn=0;

    //Initialize the stat recording arrays

    for (initcount=0;initcount<200;initcount++) {

      recordave[initcount]=0;

      genesave[initcount]=0;

      nodesave[initcount]=0;

      for (run=0;run<NEAT::num_runs;++run) {

record[run][initcount]=0;

genesrec[run][initcount]=0;

nodesrec[run][initcount]=0;

      }

    }

    char *non_markov_starter="pole2startgenes2";

    char *markov_starter="pole2startgenes1";

    char *startstring;

    if (velocity==0) startstring=non_markov_starter;

    else if (velocity==1) startstring=markov_starter;

    ifstream iFile(startstring,ios::in);

    //ifstream iFile("pole2startgenes",ios::in);

    cout<<"START DOUBLE POLE BALANCING EVOLUTION"<<endl;

    if (!velocity)

      cout<<"NO VELOCITY INPUT"<<endl;

    cout<<"Reading in the start genome"<<endl;

    //Read in the start Genome

    iFile>>curword;

    iFile>>id;

    cout<<"Reading in Genome id "<<id<<endl;

    start_genome=new Genome(id,iFile);

    iFile.close();

    cout<<"Start Genome: "<<start_genome<<endl;

    for (run=0;run<NEAT::num_runs;run++) {

      cout<<"RUN #"<<run<<endl;

      //Spawn the Population from starter gene

      cout<<"Spawning Population off Genome"<<endl;

      pop=new Population(start_genome,NEAT::pop_size);

      //Alternative way to start off of randomly connected genomes

      //pop=new Population(pop_size,7,1,10,false,0.3);

      cout<<"Verifying Spawned Pop"<<endl;

      pop->verify();

      //Create the Cart

      thecart=new CartPole(true,velocity);

      for (gen=1;gen<=gens;gen++) {

cout<<"Epoch "<<gen<<endl;

fnamebuf=new ostringstream();

(*fnamebuf)<<"gen_"<<gen<<ends;  //needs end marker

#ifndef NO_SCREEN_OUT

cout<<"name of fname: "<<fnamebuf->str()<<endl;

#endif

char temp[50];

        sprintf (temp, "gen_%d", gen);

highscore=pole2_epoch(pop,gen,temp,velocity, thecart,champg,champn,winnernum,oFile);

//highscore=pole2_epoch(pop,gen,fnamebuf->str(),velocity, thecart,champg,champn,winnernum,oFile);

//cout<<"GOT HIGHSCORE FOR GEN "<<gen<<": "<<highscore-1<<endl;

record[run][gen-1]=highscore-1;

genesrec[run][gen-1]=champg;

nodesrec[run][gen-1]=champn;

fnamebuf->clear();

delete fnamebuf;

//Stop right at the winnergen

if (((pop->winnergen)!=0)&&(gen==(pop->winnergen))) {

 winnergens[run]=NEAT::pop_size*(gen-1)+winnernum;

 gen=gens+1;

}

//In non-MARKOV, stop right at winning (could go beyond if desired)

if ((!(thecart->MARKOV))&&((pop->winnergen)!=0))

 gen=gens+1;

#ifndef NO_SCREEN_OUT

      cout<<"gen = "<<gen<<" gens = "<<gens<<endl;

#endif

      if (gen==(gens-1)) oFile<<"FAIL: Last gen on run "<<run<<endl;

      }

      if (run<NEAT::num_runs-1) delete pop;

      delete thecart;

    }

    cout<<"Generation highs: "<<endl;

    oFile<<"Generation highs: "<<endl;

    for(gen=0;gen<=gens-1;gen++) {

      curtotal=0;

      for (run=0;run<NEAT::num_runs;++run) {

if (record[run][gen]>0) {

 cout<<setw(8)<<record[run][gen]<<" ";

 oFile<<setw(8)<<record[run][gen]<<" ";

 curtotal+=record[run][gen];

}

else {

 cout<<"         ";

 oFile<<"         ";

 curtotal+=100000;

}

recordave[gen]=(double) curtotal/NEAT::num_runs;

      }

      cout<<endl;

      oFile<<endl;

    }

    cout<<"Generation genes in champ: "<<endl;

    for(gen=0;gen<=gens-1;gen++) {

      curtotal=0;

      samples=0;

      for (run=0;run<NEAT::num_runs;++run) {

if (genesrec[run][gen]>0) {

 cout<<setw(4)<<genesrec[run][gen]<<" ";

 oFile<<setw(4)<<genesrec[run][gen]<<" ";

 curtotal+=genesrec[run][gen];

 samples++;

}

else {

 cout<<setw(4)<<"     ";

 oFile<<setw(4)<<"     ";

}

      }

      genesave[gen]=(double) curtotal/samples;

      cout<<endl;

      oFile<<endl;

    }

    cout<<"Generation nodes in champ: "<<endl;

    oFile<<"Generation nodes in champ: "<<endl;

    for(gen=0;gen<=gens-1;gen++) {

      curtotal=0;

      samples=0;

      for (run=0;run<NEAT::num_runs;++run) {

if (nodesrec[run][gen]>0) {

 cout<<setw(4)<<nodesrec[run][gen]<<" ";

 oFile<<setw(4)<<nodesrec[run][gen]<<" ";

 curtotal+=nodesrec[run][gen];

 samples++;

}

else {

 cout<<setw(4)<<"     ";

 oFile<<setw(4)<<"     ";

}

      }

      nodesave[gen]=(double) curtotal/samples;

      cout<<endl;

      oFile<<endl;

    }

    cout<<"Generational record fitness averages: "<<endl;

    oFile<<"Generational record fitness averages: "<<endl;

    for(gen=0;gen<gens-1;gen++) {

      cout<<recordave[gen]<<endl;

      oFile<<recordave[gen]<<endl;

    }

    cout<<"Generational number of genes in champ averages: "<<endl;

    oFile<<"Generational number of genes in champ averages: "<<endl;

    for(gen=0;gen<gens-1;gen++) {

      cout<<genesave[gen]<<endl;

      oFile<<genesave[gen]<<endl;

    }

    cout<<"Generational number of nodes in champ averages: "<<endl;

    oFile<<"Generational number of nodes in champ averages: "<<endl;

    for(gen=0;gen<gens-1;gen++) {

      cout<<nodesave[gen]<<endl;

      oFile<<nodesave[gen]<<endl;

    }

    cout<<"Winner evals: "<<endl;

    oFile<<"Winner evals: "<<endl;

    curtotal=0;

    samples=0;

    for (run=0;run<NEAT::num_runs;++run) {

      cout<<winnergens[run]<<endl;

      oFile<<winnergens[run]<<endl;

      curtotal+=winnergens[run];

      samples++;

    }

    cout<<"Average # evals: "<<((double) curtotal/samples)<<endl;

    oFile<<"Average # evals: "<<((double) curtotal/samples)<<endl;

    oFile.close();

    return pop;

}

//This is used for list sorting of Species by fitness of best organism

//highest fitness first

//Used to choose which organism to test

//bool order_new_species(Species *x, Species *y) {

//

//  return (x->compute_max_fitness() >

//  y->compute_max_fitness());

//}

int pole2_epoch(Population *pop,int generation,char *filename,bool velocity,

CartPole *thecart,int &champgenes,int &champnodes,

int &winnernum, ofstream &oFile) {

  //char cfilename[100];

  //strncpy( cfilename, filename.c_str(), 100 );

  //ofstream cfilename(filename.c_str());

  vector<Organism*>::iterator curorg;

  vector<Species*>::iterator curspecies;

  vector<Species*> sorted_species;  //Species sorted by max fit org in Species

  int pause;

  bool win=false;

  double champ_fitness;

  Organism *champ;

  //double statevals[5]={-0.9,-0.5,0.0,0.5,0.9};

  double statevals[5]={0.05, 0.25, 0.5, 0.75, 0.95};

  int s0c,s1c,s2c,s3c;

  int score;

  thecart->nmarkov_long=false;

  thecart->generalization_test=false;

  //Evaluate each organism on a test

  for(curorg=(pop->organisms).begin();curorg!=(pop->organisms).end();++curorg) {

    //shouldn't happen

    if (((*curorg)->gnome)==0) {

      cout<<"ERROR EMPTY GEMOME!"<<endl;

      cin>>pause;

    }

    if (pole2_evaluate((*curorg),velocity,thecart)) win=true;

  }

  //Average and max their fitnesses for dumping to file and snapshot

  for(curspecies=(pop->species).begin();curspecies!=(pop->species).end();++curspecies) {

    //This experiment control routine issues commands to collect ave

    //and max fitness, as opposed to having the snapshot do it,

    //because this allows flexibility in terms of what time

    //to observe fitnesses at

    (*curspecies)->compute_average_fitness();

    (*curspecies)->compute_max_fitness();

  }

  //Take a snapshot of the population, so that it can be

  //visualized later on

  //if ((generation%1)==0)

  //  pop->snapshot();

  //Find the champion in the markov case simply for stat collection purposes

  if (thecart->MARKOV) {

    champ_fitness=0.0;

    for(curorg=(pop->organisms).begin();curorg!=(pop->organisms).end();++curorg) {

      if (((*curorg)->fitness)>champ_fitness) {

champ=(*curorg);

champ_fitness=champ->fitness;

champgenes=champ->gnome->genes.size();

champnodes=champ->gnome->nodes.size();

winnernum=champ->gnome->genome_id;

      }

    }

  }

  //Check for winner in Non-Markov case

  if (!(thecart->MARKOV)) {

    cout<<"Non-markov case"<<endl;

    //Sort the species

    for(curspecies=(pop->species).begin();curspecies!=(pop->species).end();++curspecies) {

      sorted_species.push_back(*curspecies);

    }

    //sorted_species.sort(order_new_species);

    std::sort(sorted_species.begin(), sorted_species.end(), NEAT::order_new_species);

    //std::sort(sorted_species.begin(), sorted_species.end(), order_species);

    cout<<"Number of species sorted: "<<sorted_species.size()<<endl;

    //First update what is checked and unchecked

    for(curspecies=sorted_species.begin();curspecies!=sorted_species.end();++curspecies) {

      if (((*curspecies)->compute_max_fitness())>((*curspecies)->max_fitness_ever))

(*curspecies)->checked=false;

    }

    //Now find a species that is unchecked

    curspecies=sorted_species.begin();

    cout<<"Is the first species checked? "<<(*curspecies)->checked<<endl;

    while((curspecies!=(sorted_species.end()))&&

 ((*curspecies)->checked))

    {

      cout<<"Species #"<<(*curspecies)->id<<" is checked"<<endl;

      ++curspecies;

    }

    if (curspecies==(sorted_species.end())) curspecies=sorted_species.begin();

    //Remember it was checked

    (*curspecies)->checked=true;

    cout<<"Is the species now checked? "<<(*curspecies)->checked<<endl;

    //Extract the champ

    cout<<"Champ chosen from Species "<<(*curspecies)->id<<endl;

    champ=(*curspecies)->get_champ();

    champ_fitness=champ->fitness;

    cout<<"Champ is organism #"<<champ->gnome->genome_id<<endl;

    cout<<"Champ fitness: "<<champ_fitness<<endl;

    winnernum=champ->gnome->genome_id;

    cout<<champ->gnome<<endl;

    //Now check to make sure the champ can do 100,000

    thecart->nmarkov_long=true;

    thecart->generalization_test=false;

    //The champ needs tp be flushed here because it may have

    //leftover activation from its last test run that could affect

    //its recurrent memory

    (champ->net)->flush();

    //champ->gnome->print_to_filename("tested");

    if (pole2_evaluate(champ,velocity,thecart)) {

      cout<<"The champ passed the 100,000 test!"<<endl;

      thecart->nmarkov_long=false;

      //Given that the champ passed, now run it on generalization tests

      score=0;

      for (s0c=0;s0c<=4;++s0c)

for (s1c=0;s1c<=4;++s1c)

 for (s2c=0;s2c<=4;++s2c)

   for (s3c=0;s3c<=4;++s3c) {

     thecart->state[0] = statevals[s0c] * 4.32 - 2.16;

     thecart->state[1] = statevals[s1c] * 2.70 - 1.35;

     thecart->state[2] = statevals[s2c] * 0.12566304 - 0.06283152;

     /* 0.06283152 =  3.6 degrees */

     thecart->state[3] = statevals[s3c] * 0.30019504 - 0.15009752;

     /* 00.15009752 =  8.6 degrees */

     thecart->state[4]=0.0;

     thecart->state[5]=0.0;

     cout<<"On combo "<<thecart->state[0]<<" "<<thecart->state[1]<<" "<<thecart->state[2]<<" "<<thecart->state[3]<<endl;

     thecart->generalization_test=true;

     (champ->net)->flush();  //Reset the champ for each eval

     if (pole2_evaluate(champ,velocity,thecart)) {

cout<<"----------------------------The champ passed its "<<score<<"th test"<<endl;

score++;

     }

   }

      if (score>=200) {

cout<<"The champ wins!!! (generalization = "<<score<<" )"<<endl;

oFile<<"(generalization = "<<score<<" )"<<endl;

oFile<<"generation= "<<generation<<endl;

        (champ->gnome)->print_to_file(oFile);

champ_fitness=champ->fitness;

champgenes=champ->gnome->genes.size();

champnodes=champ->gnome->nodes.size();

winnernum=champ->gnome->genome_id;

win=true;

      }

      else {

cout<<"The champ couldn't generalize"<<endl;

champ->fitness=champ_fitness; //Restore the champ's fitness

      }

    }

    else {

      cout<<"The champ failed the 100,000 test :("<<endl;

      cout<<"made score "<<champ->fitness<<endl;

      champ->fitness=champ_fitness; //Restore the champ's fitness

    }

  }

  //Only print to file every print_every generations

  if  (win||

       ((generation%(NEAT::print_every))==0)) {

    cout<<"printing file: "<<filename<<endl;

    pop->print_to_file_by_species(filename);

  }

  if ((win)&&((pop->winnergen)==0)) pop->winnergen=generation;

  //Prints a champion out on each generation

  //IMPORTANT: This causes generational file output!

  print_Genome_tofile(champ->gnome,"champ");

  //Create the next generation

  pop->epoch(generation);

  return (int) champ_fitness;

}

bool pole2_evaluate(Organism *org,bool velocity, CartPole *thecart) {

  Network *net;

  int thresh;  /* How many visits will be allowed before giving up

 (for loop detection)  NOW OBSOLETE */

  int pause;

  net=org->net;

  thresh=100;  //this is obsolete

  //DEBUG :  Check flushedness of org

  //org->net->flush_check();

  //Try to balance a pole now

  org->fitness = thecart->evalNet(net,thresh);

#ifndef NO_SCREEN_OUT

  if (org->pop_champ_child)

    cout<<" <<DUPLICATE OF CHAMPION>> ";

  //Output to screen

  cout<<"Org "<<(org->gnome)->genome_id<<" fitness: "<<org->fitness;

  cout<<" ("<<(org->gnome)->genes.size();

  cout<<" / "<<(org->gnome)->nodes.size()<<")";

  cout<<"   ";

  if (org->mut_struct_baby) cout<<" [struct]";

  if (org->mate_baby) cout<<" [mate]";

  cout<<endl;

#endif

  if ((!(thecart->generalization_test))&&(!(thecart->nmarkov_long)))

  if (org->pop_champ_child) {

    cout<<org->gnome<<endl;

    //DEBUG CHECK

    if (org->high_fit>org->fitness) {

      cout<<"ALERT: ORGANISM DAMAGED"<<endl;

      print_Genome_tofile(org->gnome,"failure_champ_genome");

      cin>>pause;

    }

  }

  //Decide if its a winner, in Markov Case

  if (thecart->MARKOV) {

    if (org->fitness>=(thecart->maxFitness-1)) {

      org->winner=true;

      return true;

    }

    else {

      org->winner=false;

      return false;

    }

  }

  //if doing the long test non-markov

  else if (thecart->nmarkov_long) {

    if (org->fitness>=99999) {

      //if (org->fitness>=9000) {

      org->winner=true;

      return true;

    }

    else {

      org->winner=false;

      return false;

    }

  }

  else if (thecart->generalization_test) {

    if (org->fitness>=999) {

      org->winner=true;

      return true;

    }

    else {

      org->winner=false;

      return false;

    }

  }

  else {

    org->winner=false;

    return false;  //Winners not decided here in non-Markov

  }

}

CartPole::CartPole(bool randomize,bool velocity)

{

  maxFitness = 100000;

  MARKOV=velocity;

  MIN_INC = 0.001;

  POLE_INC = 0.05;

  MASS_INC = 0.01;

  LENGTH_2 = 0.05;

  MASSPOLE_2 = 0.01;

  // CartPole::reset() which is called here

}

//Faustino Gomez wrote this physics code using the differential equations from

//Alexis Weiland's paper and added the Runge-Kutta himself.

double CartPole::evalNet(Network *net,int thresh)

{

  int steps=0;

  double input[NUM_INPUTS];

  double output;

  int nmarkovmax;

  double nmarkov_fitness;

  double jiggletotal; //total jiggle in last 100

  int count;  //step counter

  //init(randomize); // restart at some point

  if (nmarkov_long) nmarkovmax=100000;

  else if (generalization_test) nmarkovmax=1000;

  else nmarkovmax=1000;

  init(0);

  if (MARKOV) {

    while (steps++ < maxFitness) {

      input[0] = state[0] / 4.8;

      input[1] = state[1] /2;

      input[2] = state[2]  / 0.52;

      input[3] = state[3] /2;

      input[4] = state[4] / 0.52;

      input[5] = state[5] /2;

      input[6] = .5;

      net->load_sensors(input);

      //Activate the net

      //If it loops, exit returning only fitness of 1 step

      if (!(net->activate())) return 1.0;

      output=(*(net->outputs.begin()))->activation;

      performAction(output,steps);

      if (outsideBounds()) // if failure

break; // stop it now

    }

    return (double) steps;

  }

  else {  //NON MARKOV CASE

    while (steps++ < nmarkovmax) {

     //Do special parameter summing on last hundred

     //if ((steps==900)&&(!nmarkov_long)) last_hundred=true;

     /*

     input[0] = state[0] / 4.8;

     input[1] = 0.0;

     input[2] = state[2]  / 0.52;

     input[3] = 0.0;

     input[4] = state[4] / 0.52;

     input[5] = 0.0;

     input[6] = .5;

     */

      //cout<<"nmarkov_long: "<<nmarkov_long<<endl;

      //if (nmarkov_long)

      //cout<<"step: "<<steps<<endl;

     input[0] = state[0] / 4.8;

     input[1] = state[2]  / 0.52;

     input[2] = state[4] / 0.52;

     input[3] = .5;

      net->load_sensors(input);

      //cout<<"inputs: "<<input[0]<<" "<<input[1]<<" "<<input[2]<<" "<<input[3]<<endl;

      //Activate the net

      //If it loops, exit returning only fitness of 1 step

      if (!(net->activate())) return 0.0001;

      output=(*(net->outputs.begin()))->activation;

      //cout<<"output: "<<output<<endl;

      performAction(output,steps);

      if (outsideBounds()) // if failure

break; // stop it now

      if (nmarkov_long&&(outsideBounds())) // if failure

break; // stop it now

    }

   //If we are generalizing we just need to balance it a while

   if (generalization_test)

     return (double) balanced_sum;

   //Sum last 100

   if ((steps>100)&&(!nmarkov_long)) {

     jiggletotal=0;

     cout<<"step "<<steps-99-2<<" to step "<<steps-2<<endl;

     //Adjust for array bounds and count

     for (count=steps-99-2;count<=steps-2;count++)

       jiggletotal+=jigglestep[count];

   }

   if (!nmarkov_long) {

     if (balanced_sum>100)

       nmarkov_fitness=((0.1*(((double) balanced_sum)/1000.0))+

(0.9*(0.75/(jiggletotal))));

     else nmarkov_fitness=(0.1*(((double) balanced_sum)/1000.0));

#ifndef NO_SCREEN_OUTR

     cout<<"Balanced:  "<<balanced_sum<<" jiggle: "<<jiggletotal<<" ***"<<endl;

#endif

     return nmarkov_fitness;

   }

   else return (double) steps;

  }

}

void CartPole::init(bool randomize)

{

  static int first_time = 1;

  if (!MARKOV) {

    //Clear all fitness records

    cartpos_sum=0.0;

    cartv_sum=0.0;

    polepos_sum=0.0;

    polev_sum=0.0;

  }

  balanced_sum=0; //Always count # balanced

  last_hundred=false;

  /*if (randomize) {

    state[0] = (lrand48()%4800)/1000.0 - 2.4;

    state[1] = (lrand48()%2000)/1000.0 - 1;

    state[2] = (lrand48()%400)/1000.0 - 0.2;

    state[3] = (lrand48()%400)/1000.0 - 0.2;

    state[4] = (lrand48()%3000)/1000.0 - 1.5;

    state[5] = (lrand48()%3000)/1000.0 - 1.5;

  }

  else {*/

  if (!generalization_test) {

    state[0] = state[1] = state[3] = state[4] = state[5] = 0;

    state[2] = 0.07; // one_degree;

  }

  else {

    state[4] = state[5] = 0;

  }

    //}

  if(first_time){

    cout<<"Initial Long pole angle = %f\n"<<state[2]<<endl;;

    cout<<"Initial Short pole length = %f\n"<<LENGTH_2<<endl;

    first_time = 0;

  }

}

void CartPole::performAction(double output, int stepnum)

{

  int i;

  double  dydx[6];

  const bool RK4=true; //Set to Runge-Kutta 4th order integration method

  const double EULER_TAU= TAU/4;

  /*random start state for long pole*/

  /*state[2]= drand48();   */

  /*--- Apply action to the simulated cart-pole ---*/

  if(RK4){

    for(i=0;i<2;++i){

      dydx[0] = state[1];

      dydx[2] = state[3];

      dydx[4] = state[5];

      step(output,state,dydx);

      rk4(output,state,dydx,state);

    }

  }

  else{

    for(i=0;i<8;++i){

      step(output,state,dydx);

      state[0] += EULER_TAU * dydx[0];

      state[1] += EULER_TAU * dydx[1];

      state[2] += EULER_TAU * dydx[2];

      state[3] += EULER_TAU * dydx[3];

      state[4] += EULER_TAU * dydx[4];

      state[5] += EULER_TAU * dydx[5];

    }

  }

  //Record this state

  cartpos_sum+=fabs(state[0]);

  cartv_sum+=fabs(state[1]);

  polepos_sum+=fabs(state[2]);

  polev_sum+=fabs(state[3]);

  if (stepnum<=1000)

    jigglestep[stepnum-1]=fabs(state[0])+fabs(state[1])+fabs(state[2])+fabs(state[3]);

  if (false) {

    //cout<<"[ x: "<<state[0]<<" xv: "<<state[1]<<" t1: "<<state[2]<<" t1v: "<<state[3]<<" t2:"<<state[4]<<" t2v: "<<state[5]<<" ] "<<

    //cartpos_sum+cartv_sum+polepos_sum+polepos_sum+polev_sum<<endl;

    if (!(outsideBounds())) {

      if (balanced_sum<1000) {

cout<<".";

++balanced_sum;

      }

    }

    else {

      if (balanced_sum==1000)

balanced_sum=1000;

      else balanced_sum=0;

    }

  }

  else if (!(outsideBounds()))

    ++balanced_sum;

}

void CartPole::step(double action, double *st, double *derivs)

{

    double force,costheta_1,costheta_2,sintheta_1,sintheta_2,

          gsintheta_1,gsintheta_2,temp_1,temp_2,ml_1,ml_2,fi_1,fi_2,mi_1,mi_2;

    force =  (action - 0.5) * FORCE_MAG * 2;

    costheta_1 = cos(st[2]);

    sintheta_1 = sin(st[2]);

    gsintheta_1 = GRAVITY * sintheta_1;

    costheta_2 = cos(st[4]);

    sintheta_2 = sin(st[4]);

    gsintheta_2 = GRAVITY * sintheta_2;

    ml_1 = LENGTH_1 * MASSPOLE_1;

    ml_2 = LENGTH_2 * MASSPOLE_2;

    temp_1 = MUP * st[3] / ml_1;

    temp_2 = MUP * st[5] / ml_2;

    fi_1 = (ml_1 * st[3] * st[3] * sintheta_1) +

           (0.75 * MASSPOLE_1 * costheta_1 * (temp_1 + gsintheta_1));

    fi_2 = (ml_2 * st[5] * st[5] * sintheta_2) +

           (0.75 * MASSPOLE_2 * costheta_2 * (temp_2 + gsintheta_2));

    mi_1 = MASSPOLE_1 * (1 - (0.75 * costheta_1 * costheta_1));

    mi_2 = MASSPOLE_2 * (1 - (0.75 * costheta_2 * costheta_2));

    derivs[1] = (force + fi_1 + fi_2)

                 / (mi_1 + mi_2 + MASSCART);

    derivs[3] = -0.75 * (derivs[1] * costheta_1 + gsintheta_1 + temp_1)

                 / LENGTH_1;

    derivs[5] = -0.75 * (derivs[1] * costheta_2 + gsintheta_2 + temp_2)

                  / LENGTH_2;

}

void CartPole::rk4(double f, double y[], double dydx[], double yout[])

{

int i;

double hh,h6,dym[6],dyt[6],yt[6];

hh=TAU*0.5;

h6=TAU/6.0;

for (i=0;i<=5;i++) yt[i]=y[i]+hh*dydx[i];

step(f,yt,dyt);

dyt[0] = yt[1];

dyt[2] = yt[3];

dyt[4] = yt[5];

for (i=0;i<=5;i++) yt[i]=y[i]+hh*dyt[i];

step(f,yt,dym);

dym[0] = yt[1];

dym[2] = yt[3];

dym[4] = yt[5];

for (i=0;i<=5;i++) {

yt[i]=y[i]+TAU*dym[i];

dym[i] += dyt[i];

}

step(f,yt,dyt);

dyt[0] = yt[1];

dyt[2] = yt[3];

dyt[4] = yt[5];

for (i=0;i<=5;i++)

yout[i]=y[i]+h6*(dydx[i]+dyt[i]+2.0*dym[i]);

}

bool CartPole::outsideBounds()

{

  const double failureAngle = thirty_six_degrees;

  return

    state[0] < -2.4              ||

    state[0] > 2.4               ||

    state[2] < -failureAngle     ||

    state[2] > failureAngle      ||

    state[4] < -failureAngle     ||

    state[4] > failureAngle;

}

void CartPole::nextTask()

{

   LENGTH_2 += POLE_INC;   /* LENGTH_2 * INCREASE;   */

   MASSPOLE_2 += MASS_INC; /* MASSPOLE_2 * INCREASE; */

   //  ++new_task;

   cout<<"#Pole Length %2.4f\n"<<LENGTH_2<<endl;

}

void CartPole::simplifyTask()

{

  if(POLE_INC > MIN_INC) {

    POLE_INC = POLE_INC/2;

    MASS_INC = MASS_INC/2;

    LENGTH_2 -= POLE_INC;

    MASSPOLE_2 -= MASS_INC;

    cout<<"#SIMPLIFY\n"<<endl;

    cout<<"#Pole Length %2.4f\n"<<LENGTH_2;

  }

  else

    {

      cout<<"#NO TASK CHANGE\n"<<endl;

    }

}

/* ------------------------------------------------------------------ */

/* Real-time NEAT Validation                                          */

/* ------------------------------------------------------------------ */

//Perform evolution on double pole balacing using rtNEAT methods calls

//Always uses Markov case (i.e. velocities provided)

//This test is meant to validate the rtNEAT methods and show how they can be used instead

// of the usual generational NEAT

Population *pole2_test_realtime() {

    Population *pop;

    Genome *start_genome;

    char curword[20];

    int id;

    ostringstream *fnamebuf;

    int gen;

    CartPole *thecart;

    double highscore;

    ifstream iFile("pole2startgenes1",ios::in);

    cout<<"START DOUBLE POLE BALANCING REAL-TIME EVOLUTION VALIDATION"<<endl;

    cout<<"Reading in the start genome"<<endl;

    //Read in the start Genome

    iFile>>curword;

    iFile>>id;

    cout<<"Reading in Genome id "<<id<<endl;

    start_genome=new Genome(id,iFile);

    iFile.close();

    cout<<"Start Genome: "<<start_genome<<endl;

    //Spawn the Population from starter gene

    cout<<"Spawning Population off Genome"<<endl;

    pop=new Population(start_genome,NEAT::pop_size);

    //Alternative way to start off of randomly connected genomes

    //pop=new Population(pop_size,7,1,10,false,0.3);

    cout<<"Verifying Spawned Pop"<<endl;

    pop->verify();

    //Create the Cart

    thecart=new CartPole(true,1);

    //Start the evolution loop using rtNEAT method calls

    highscore=pole2_realtime_loop(pop,thecart);

    return pop;

}

int pole2_realtime_loop(Population *pop, CartPole *thecart) {

  vector<Organism*>::iterator curorg;

  vector<Species*>::iterator curspecies;

  vector<Species*>::iterator curspec; //used in printing out debug info

  vector<Species*> sorted_species;  //Species sorted by max fit org in Species

  int pause;

  bool win=false;

  double champ_fitness;

  Organism *champ;

  //double statevals[5]={-0.9,-0.5,0.0,0.5,0.9};

  double statevals[5]={0.05, 0.25, 0.5, 0.75, 0.95};

  int s0c,s1c,s2c,s3c;

  int score;

  //Real-time evolution variables

  int offspring_count;

  Organism *new_org;

  thecart->nmarkov_long=false;

  thecart->generalization_test=false;

  //We try to keep the number of species constant at this number

  int num_species_target=NEAT::pop_size/15;

  //This is where we determine the frequency of compatibility threshold adjustment

  int compat_adjust_frequency = NEAT::pop_size/10;

  if (compat_adjust_frequency < 1)

    compat_adjust_frequency = 1;

  //Initially, we evaluate the whole population

  //Evaluate each organism on a test

  for(curorg=(pop->organisms).begin();curorg!=(pop->organisms).end();++curorg) {

    //shouldn't happen

    if (((*curorg)->gnome)==0) {

      cout<<"ERROR EMPTY GEMOME!"<<endl;

      cin>>pause;

    }

    if (pole2_evaluate((*curorg),1,thecart)) win=true;

  }

  //Get ready for real-time loop

  //Rank all the organisms from best to worst in each species

  pop->rank_within_species();

  //Assign each species an average fitness

  //This average must be kept up-to-date by rtNEAT in order to select species probabailistically for reproduction

  pop->estimate_all_averages();

  //Now create offspring one at a time, testing each offspring,

  // and replacing the worst with the new offspring if its better

  for (offspring_count=0;offspring_count<20000;offspring_count++) {

    //Every pop_size reproductions, adjust the compat_thresh to better match the num_species_targer

    //and reassign the population to new species

    if (offspring_count % compat_adjust_frequency == 0) {

      int num_species = pop->species.size();

      double compat_mod=0.1;  //Modify compat thresh to control speciation

      // This tinkers with the compatibility threshold

      if (num_species < num_species_target) {

NEAT::compat_threshold -= compat_mod;

      }

      else if (num_species > num_species_target)

NEAT::compat_threshold += compat_mod;

      if (NEAT::compat_threshold < 0.3)

NEAT::compat_threshold = 0.3;

      cout<<"compat_thresh = "<<NEAT::compat_threshold<<endl;

      //Go through entire population, reassigning organisms to new species

      for (curorg = (pop->organisms).begin(); curorg != pop->organisms.end(); ++curorg) {

pop->reassign_species(*curorg);

      }

    }

    //For printing only

    for(curspec=(pop->species).begin();curspec!=(pop->species).end();curspec++) {

      cout<<"Species "<<(*curspec)->id<<" size"<<(*curspec)->organisms.size()<<" average= "<<(*curspec)->average_est<<endl;

    }

    cout<<"Pop size: "<<pop->organisms.size()<<endl;

    //Here we call two rtNEAT calls:

    //1) choose_parent_species() decides which species should produce the next offspring

    //2) reproduce_one(...) creates a single offspring fromt the chosen species

    new_org=(pop->choose_parent_species())->reproduce_one(offspring_count,pop,pop->species);

    //Now we evaluate the new individual

    //Note that in a true real-time simulation, evaluation would be happening to all individuals at all times.

    //That is, this call would not appear here in a true online simulation.

    cout<<"Evaluating new baby: "<<endl;

    if (pole2_evaluate(new_org,1,thecart)) win=true;

    if (win) {

      cout<<"WINNER"<<endl;

      pop->print_to_file_by_species("rt_winpop");

      break;

    }

    //Now we reestimate the baby's species' fitness

    new_org->species->estimate_average();

    //Remove the worst organism

    pop->remove_worst();

  }

}