RAND_RTYPE RandGen(void);
int TossCoin(float);
void SeedRand(RAND_RTYPE);
void SetRandMax(RAND_RTYPE);

#endif

#include <stdlib.h>
#include <stdio.h>
#include "randgen.h"

static RAND_RTYPE Seed = 7;
static RAND_RTYPE MaxLimit = MODULAR;

void SeedRand(RAND_RTYPE SeedIn){
    if ((SeedIn > 0) && (SeedIn < (MODULAR-1)))
        Seed = SeedIn;
}

void SetRandMax(RAND_RTYPE MaxIn){
    if ((MaxIn <= MODULAR) && (MaxIn > 0))
        MaxLimit = MaxIn;
    else
        MaxLimit = MODULAR;
}

int TossCoin(float odds){
    int heads = 0, value = 0;
    float toss;
    RAND_RTYPE oldmax = MaxLimit;

    value = (int)(odds * COIN_PREC);
    SetRandMax(COIN_PREC-1);
    toss = RandGen();
    if(value>toss)
        heads = 1;
    SetRandMax(oldmax);
    return heads;
}

RAND_RTYPE RandGen(){
    long Lo, Hi, Test;
    RAND_RTYPE RNum = 0;

    Hi = (double)((int)(Seed / QUOTIENT));
    Lo = Seed - QUOTIENT * Hi;
    Test = MULTIPLIER * Lo - REMAINDER * Hi;
    if (Test > 0.0)         /* change the seed. */
        Seed = Test;
    else
        Seed = Test + MODULAR;
    if(((MaxLimit) < MODULAR) && (Seed != MaxLimit))
        RNum = Seed % (MaxLimit+1);
    else
        RNum = Seed;
    return RNum;
}

#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>

#define _GP_UNIX_         // comment this out if not compiling for UNIX
// #define _GP_WIN_          // if compiling for windows

#include "tokens.h"
#include "chrome.h"
#include "glbdefs.h"
#include "randgen.h"
#include "dataout.h"

int main(int argc, char *argv[]){
    Population *pop1, *pop2;
    Chrome *best[MAX_GEN];
    NFITNESS_TYPE mfitness[MAX_GEN];
    int i,k = 0,bestofbest = 0;
    char *stype;

    stype = GetSType(argc, argv[1]);
    if((argc ==3)&&(atoi(argv[2])))
        SeedRand(atoi(argv[2]));
    FillFunctTable();
    for(i=0;i<MAX_RUNS;i++) {
        k = 0;
        pop1 = new Population;
        pop1->InitPop();
        pop1->Evaluate();
        for(k=0;k<MAX_GEN-1;k++) {
            mfitness[k] = pop1->GetMean();
            best[k] = pop1->CopyBest();
            pop2 = new Population;
            switch (*stype){
                case 't': pop2->Tournament(pop1->GetPop()); break;
                case 'e': pop2->ExpectVR3(pop1->GetPop()); break;
                default : pop2->Roulette(pop1->GetPop()); break;
            }
            delete pop1;
            pop1 = new Population(pop2);
            delete pop2;
            pop1->Evaluate();
        }
        best[k] = pop1->CopyBest();
        mfitness[k] = pop1->GetMean();
        bestofbest = GetBestofBest(best);
        best[bestofbest]->WriteSimStates(stype,i,bestofbest);
        WriteGenMean(mfitness, stype);
        WriteBest(best,stype,i);
        WriteBestFit(best, stype);
        WriteBestTime(best, stype);
        DeleteBestOfRun(best);
        delete pop1;
    }
    delete stype;
    return 0;
}

/*
Filename: cartpole.h
Author : Peter Hackett.
Description: Header file for cartpole.cc */

#ifndef _CARTPOLE
#define _CARTPOLE

#include "chrome.h"
#include "glbdefs.h"
#define MAXDEGREES 15                                                                     // Failure angle of pole (deg.).
#define pi 3.141592
#define THETAFAIL (float)pi/MAXDEGREES                                  // Failure angle of pole (0.21 rad.).
#define TRACKLENGTH 2.4                                                                 // Track length (m.).
#define LAMBDA 0.5                                                                             // 1/2 pole length (m.).
#define F_MAX 10                                                                                 // Bang-Bang force (Newtons).
#define C_MASS 1                                                                                 // Cart mass (kg.).
#define P_MASS 0.1                                                                              // Pole mass (kg.).
#define ALPHA (float)(F_MAX/(P_MASS + C_MASS))               // Adjusted force (Newtons).
#define RP_MASS (float)(P_MASS/(P_MASS + C_MASS))        // Reduced mass of pole (kg.).
#define GRAV 9.81
#define SUCCESS_TIME 1000                                                             // Balance time before success (sec.).
#define TRACK_TIME 20                                                                    // Period to monitor sim states on file (sec.).
#define T_STEP 0.02                                                                             // Time step for the simulation (sec.).
#define INIT_X 1                                                                                   // Initial position of the cart (m).
#define INIT_XDASH 1                                                                       // Initial velocity of the cart (m/sec.).
#define INIT_THETA 0.17                                                                   // Initial angle of the pole (rad.)
#define INIT_THETADASH .18                                                         // Initial angular velocity of the pole (rad/sec.).

class CartPole{
    SimState prevstate, currstate, rmsstates;
    int steps;
public:
    CartPole();
    ~CartPole(){};
    void InitStates();
    SimState &GetCurrState() {return currstate;};
    SimState &GetRMSState() {return rmsstates;};
    int GetSteps() {return steps;};
    boolean StillIn();
    void TakeSimStep(int);
    void RunCartPole(Chrome*);
    void PrintStates(FILE*, SimState);
    void print();
};

void RMS(SimState&,unsigned);

#endif

#include <stdlib.h>
#include <stdio.h>
#include <iostream.h>
#include <math.h>
#include "cartpole.h"

CartPole::CartPole(){InitStates();}

void CartPole::InitStates(){
    currstate.x = INIT_X;
    currstate.xdash = INIT_XDASH;
    currstate.theta = INIT_THETA;
    currstate.thetadash = INIT_THETADASH;
    prevstate = currstate;
    rmsstates = currstate;
    steps = 0;
}

void CartPole::PrintStates(FILE *stream, SimState s){
    fprintf(stream,"%.2f,%f,%f,%f,%f\n",(steps*T_STEP), s.x,s.xdash,s.theta,s.thetadash);
}

void CartPole::print(){
    cout << "\nx = " << currstate.x << " m.";
    cout << "\nx dash = " << currstate.xdash << " m/s.";
    cout << "\ntheta = " << currstate.theta << " rad.";
    cout << "\ntheta dash = " << currstate.thetadash << " rad./s.";
}

boolean CartPole::StillIn(){
    boolean ok = FALSE;
    if((SafeAbs(currstate.x) <= TRACKLENGTH) && ((steps*T_STEP)< SUCCESS_TIME)&& (SafeAbs(currstate.theta) <= THETAFAIL))
        ok = TRUE;
    return ok;
}

void CartPole::TakeSimStep(int bang){/* simulation equations for a time step in the cart-pole problem */
    double denom, haccel, aaccel;

    steps++;
    denom = (4/3 - (3 * RP_MASS * SafeSqr(cos(currstate.theta))));
    if(denom){                                                        /* calculate horizontal and angular acceleration */
        haccel =(((4/3 * (bang * ALPHA)) + ((4/3 * SafeSqr(currstate.thetadash) * LAMBDA) - (GRAV * cos(currstate.theta))) * RP_MASS * sin(currstate.theta)))/denom;
        aaccel =((GRAV * sin(currstate.theta))-((bang * ALPHA) * (cos(currstate.theta))) - (RP_MASS * SafeSqr(currstate.thetadash) * LAMBDA * cos(currstate.theta) * sin(currstate.theta)))/(LAMBDA * denom);
    }
    else{
        haccel = 0;
        aaccel = 0;
    }
    prevstate = currstate;
    currstate.x = prevstate.x + (T_STEP * prevstate.xdash);
    currstate.xdash = prevstate.xdash + (T_STEP * haccel);
    currstate.theta = prevstate.theta + (T_STEP * prevstate.thetadash);
    currstate.thetadash = prevstate.thetadash + (T_STEP * aaccel);
    rmsstates += currstate;
}

void CartPole::RunCartPole(Chrome *curr_gp){
    int bang = 1;
    InitStates();
    while (StillIn()){
        UpdateVars(currstate);
        bang = curr_gp->Evaluate();
        TakeSimStep(bang);
    }
    RMS(rmsstates,steps);
}

void RMS(SimState &i, unsigned nosteps){
    i.x = SafeSqrt(SafeSqr(i.x/nosteps));
    i.xdash = SafeSqrt(SafeSqr(i.xdash/nosteps));
    i.theta = SafeSqrt(SafeSqr(i.theta/nosteps));
    i.thetadash = SafeSqrt(SafeSqr(i.thetadash/nosteps));
}

#ifndef _CHROMES
#define _CHROMES
#include <stdio.h>
#include "tokens.h"
#include "glbdefs.h"
#if (MAX_POP < 1)
    #error MAX_POP (in glbdefs.h) must have value of 2 or more
#endif

struct Node{
    Token &token_ptr;
    Node *left_ptr;
    Node *right_ptr;
    Node(Token&);
    ~Node(){};
    RETURN_TYPE Evaluate() {return (RETURN_TYPE)token_ptr.Evaluate(this);};
    RETURN_TYPE EvalLeftArg();
    RETURN_TYPE EvalRightArg();
    void print() {token_ptr.print(stdout);};
};

class GProgram{
protected:
    Node *root;
    int length;
public:
    GProgram();
    GProgram(Node* s);
    ~GProgram();
    void AddToGP(Token *new_token);
    Node *Bud(Node *current);
    void DeleteGP(Node*);
    void SetLength();
    int GetHeight(Node*,boolean);
    int GetLength() {return length;};
    void PutLength(int n) {length = n;};
    void PutRoot(Node *r) {root = r;};
    Node *GetRoot() {return root;};
    Node *AddToTree(Node*, Node*,boolean);
    void GraftToTree(Node*,int);
    void GenerateProg();
    void GenerateTestProg(int,...);
    Node *PruneNthNode(Node*,int);
    int Evaluate();
    void Mutate();
    void print(FILE *stream = stdout){print(root,stream);};
    void print(Node*,FILE*);
    void printvalues();
    void printvalues(Node*);
};

class Chrome:public GProgram{
protected:
    RFITNESS_TYPE rfitness; // rawfitness
    NFITNESS_TYPE nfitness; // relative fitness
    int steps; // number of time steps
    SimState rms;
public:
    Chrome();
    Chrome(Chrome*);
    RFITNESS_TYPE GetRFitness() {return rfitness;};
    NFITNESS_TYPE GetNFitness() {return nfitness;};
    int GetSteps() {return steps;};
    float GetRunTime();
    SimState &GetRMSState() {return rms;};
    void CalcRFitness();
    void PutRFitness(RFITNESS_TYPE rf) {rfitness = rf;};
    void PutNFitness(NFITNESS_TYPE nf) {nfitness = nf;};
    void PutSteps(int st) {steps = st;};
    void PutRMSState(SimState&);
    void ResetAttributes();
    void WriteSimStates(char*,int,int);
    void print(FILE *stream) {GProgram::print(stream);};
    void print();
    void PrintRMS(FILE *stream);
};

class Population{
protected:
    Chrome* individual[MAX_POP];
    SFITNESS_TYPE sfitness;
public:
    Population();
    Population(unsigned long);
    Population(Population*);
    ~Population();
    Chrome *GetChrome(int i) {return individual[i];};
    Chrome **GetPop() {return individual;};
    void SetFitness();
    SFITNESS_TYPE GetSFitness() {return sfitness;};
    void PutSFitness(SFITNESS_TYPE sf) {sfitness = sf;};
    NFITNESS_TYPE GetMean();
    void InitPop();
    void Evaluate();
    void GetCrossPoint(int,int&,int&);
    void CrossOver(int&, Chrome*, Chrome*);
    void Tournament(Chrome *lastgen[MAX_POP]);
    void ExpectVR3(Chrome *lastgen[MAX_POP]);
    void Roulette(Chrome *lastgen[MAX_POP]);
    void RutChromes(int mpool[MAX_POP], Chrome *lastgen[MAX_POP]);
    int SpinWheel(Chrome *lastgen[MAX_POP]);
    void Mutate();
    Chrome *CopyBest();
    void print();
};

boolean operator>(Chrome&,Chrome&);
boolean operator>=(Chrome&,Chrome&);
Node *AddToTree(Node*,Node*,boolean);
Node *GenerateProg(GProgram*);
Node *GetNthNode(Node*,Node**,int);
int GetNodeCount(Node*);
Node *PruneNthNode(Node*,int&);
int GetBestofBest(Chrome *best[MAX_GEN]);
void DeleteBestOfRun(Chrome *best[MAX_GEN]);
#endif

#include <stdlib.h>
#include <iostream.h>
#include <stdio.h>
#include <string.h>
#include <float.h>
#include <limits.h>
#include <assert.h>
#include <stdarg.h>
#include "chrome.h"
#include "cartpole.h"
#include "randgen.h"

// these three globals are defined in tokens.cpp
extern int T_VARS;
extern int T_FUNCTS;
extern class Token *FunctTable[MAX_POP];

Node::Node(Token &t):token_ptr(t){
    left_ptr = 0;
    right_ptr = 0;
}

RETURN_TYPE Node::EvalLeftArg(){
    RETURN_TYPE result = 0;

    result = left_ptr->Evaluate();
    return result;
}

RETURN_TYPE Node::EvalRightArg(){
    RETURN_TYPE result = 0;

    result = right_ptr->Evaluate();
    return result;
}

GProgram::GProgram(){
    root = NULL;
    length = 0;
}

GProgram::~GProgram(){
    DeleteGP(root);
}

void GProgram::AddToGP(Token *new_token){
    Node *newnode;

    if((newnode = new Node(*new_token))==NULL)
        ExitGP("Insufficient memory to add node to GProgram.");
    root = AddToTree(root,newnode,TRUE);
}

Node *GProgram::Bud(Node *current){
    Node *copy = NULL;

    if(current){
        if((copy = new Node(current->token_ptr))==NULL)
            ExitGP("Insufficient memory to copy node.");
        copy->left_ptr = Bud(current->left_ptr);
        copy->right_ptr = Bud(current->right_ptr);
    }
    return copy;
}

void GProgram::DeleteGP(Node *curr_ptr){
    if(curr_ptr){
        DeleteGP(curr_ptr->left_ptr);
        DeleteGP(curr_ptr->right_ptr);
        delete curr_ptr;}
}

void GProgram::SetLength(){
    length = GetNodeCount(root);
}

Node *GProgram::AddToTree(Node *current, Node *newnode, boolean firstrun){/* Add the new node to the tree. */
    static boolean done = FALSE;
    if(firstrun) done = FALSE;
    if (current == NULL) { /* Found a home for 'newnode'.*/
        current = newnode;
        done = TRUE;
    }
    else {  /* Keep Looking. */
        int args = current->token_ptr.GetArgNum();
        if(args) { /* not a terminal */
            current->left_ptr = AddToTree(current->left_ptr,newnode, FALSE);
            --args;
            if((!done) && (args))
                current->right_ptr = AddToTree(current->right_ptr,newnode, FALSE);
        }
    }
    return current;
}

void GProgram::GraftToTree(Node *new_branch, int oldroot){
    if(!oldroot)
        root = AddToTree(NULL,new_branch,TRUE);
    else
        root = AddToTree(root,new_branch,TRUE);
}

void GProgram::GenerateProg(){
    int i = INIT_DEPTH,id = 0;
    int leaves = 0;

    SetRandMax(T_FUNCTS-1);
    id = (int)RandGen();
    leaves += FunctTable[id]->GetArgNum();
    AddToGP(FunctTable[id]);
    while(leaves > 0) {
        if((leaves < i) && (leaves == 1)) {
            SetRandMax(T_FUNCTS-1);
            id = (int)RandGen();
        }
        else{
            if(TossCoin(P_VAR)) {
                SetRandMax(T_FUNCTS+T_VARS-1);
                while(!(FunctTable[id]->IsVar()))
                    id = (int)RandGen();
            }
            else{
                SetRandMax(MAX_TOKENS - 1);
                id = (int)RandGen();
            }
        }
        --leaves;
        leaves += FunctTable[id]->GetArgNum();
        AddToGP(FunctTable[id]);
        i--;
    }
    SetLength();
}

void GProgram::GenerateTestProg(int argno,...){
    va_list ap;
    int i, gene = 0;

    va_start(ap,argno);
    for(i=0;i<argno;++i){
        gene = va_arg(ap,int);
        AddToGP(FunctTable[gene]);
    }
    va_end(ap);
}

int GProgram::Evaluate(){
    int result;
    int SpinGlassIf(RETURN_TYPE);

    result = SpinGlassIf(root->Evaluate());
    return result;
}

int GetNodeCount(Node *current) { /* Count the number of nodes below (and including) 'root'. */
    int i = 1;
    if (current){
        if (current->left_ptr != NULL)
            i += GetNodeCount(current->left_ptr);
        if (current->right_ptr != NULL)
            i += GetNodeCount(current->right_ptr);
    }
    return i;
}

Node *GetNthNode(Node *current, Node **nthnode, int n = 0) { /* Find and return the 'Nth' node in the tree. i.e. pre-order traversal. */
    static long i = 0;

    if (n) { /* Not recursive call.*/
        i = n;
        *nthnode = NULL;
    }
    i--;
    if ((!i) && (*nthnode == NULL)) /* Found node. */
        *nthnode = current;
    if (*nthnode == NULL) { /* Keep traversing tree. */
        if (current != NULL){
            if (current->left_ptr != NULL)
                current->left_ptr = GetNthNode(current->left_ptr, nthnode);
            if (current->right_ptr != NULL)
                current->right_ptr = GetNthNode(current->right_ptr, nthnode);
        }
    }
    return current;
}

Node *GProgram::PruneNthNode(Node *current,int n = 0) { /* Prunes and returns the 'nth' node in the tree. i.e. pre-order traversal. */
    Node *temp = NULL;
    static int i = 0;

    if(n)
        i = n;
    i--;
    if((!i)&&(current))
        return current;
    if(current){
        if(current->left_ptr)
            temp = PruneNthNode(current->left_ptr);
        if (temp == current->left_ptr) // prune from tree
            current->left_ptr = NULL;
        if(!temp){
            if(current->right_ptr)
                temp = PruneNthNode(current->right_ptr);
            if(temp == current->right_ptr) // prune from tree
                current->right_ptr = NULL;
        }
    }
    return temp;
}

void GProgram::Mutate(){
    Node *mute;
    int mnode = 0, mtok = 0,args,argno,nlength;

    SetRandMax(length-1);
    mnode = (int)RandGen();
    mute = PruneNthNode(root,mnode);
    nlength = GetNodeCount(root);
    args = mute->token_ptr.GetArgNum();
    SetRandMax(MAX_TOKENS - 1);
    while(args >= 0){
        do{
            mtok = (int)RandGen();
            argno = FunctTable[mtok]->GetArgNum();
        }while((nlength + argno) <= MAX_LENGTH);
        AddToGP(FunctTable[mtok]);
        args += argno;args--;length++;
    }
}

void GProgram::print(Node *node_ptr,FILE* stream = stdout){
    int args = 0;

    if(node_ptr){
        node_ptr->token_ptr.print(stream);
        args = node_ptr->token_ptr.GetArgNum();
        if(node_ptr->left_ptr){
            --args;
            print(node_ptr->left_ptr, stream);
            if(!args)
                fputc(')',stream);
        }
        if(node_ptr->right_ptr){
            --args;
            fputc(',',stream);
            print(node_ptr->right_ptr,stream);
            fputc(')',stream);
        }
    }
}

void GProgram::printvalues(Node *current){
    if(current){
        if((current->token_ptr.IsTerminal()) && (current->token_ptr.IsVar()))
            current->token_ptr.printvalue();
        printvalues(current->left_ptr);
        printvalues(current->right_ptr);
    }
}

void GProgram::printvalues(){
    printvalues(root);
    cout << "Evaluates to " << root->Evaluate() << endl;
    cout << "Action " << Evaluate() << endl;
}

Chrome::Chrome(){
    rfitness = 0;
    nfitness = 0;
    steps = 0;
    rms.x = INIT_X;
    rms.xdash = INIT_XDASH;
    rms.theta = INIT_THETA;
    rms.thetadash = INIT_THETADASH;
}

Chrome::Chrome(Chrome *c){
    this->root = Bud(c->root);
    this->length = c->length;
    rfitness = c->rfitness;
    nfitness = c->nfitness;
    steps = c->steps;
    rms = c->rms;
}

float Chrome::GetRunTime(){
    return (T_STEP * steps);
}

void Chrome::CalcRFitness(){
    RFITNESS_TYPE bonus = 0;
    bonus = (RFITNESS_TYPE)(X_BONUS)/(1+rms.x);rfitness = steps + bonus;
}

void Chrome::PutRMSState(SimState &r){
    rms.x = r.x;
    rms.xdash = r.xdash;
    rms.theta = r.theta;
    rms.thetadash = r.thetadash;
}

void Chrome::ResetAttributes(){
    SetLength();
    rfitness = 0;
    nfitness = 0;
    rms.x = INIT_X;
    rms.xdash = INIT_XDASH;
    rms.theta = INIT_THETA;
    rms.thetadash = INIT_THETADASH;
}

void PrintSHeading(FILE *stream, int runno, int genno, int steps = 0, RFITNESS_TYPE rfit = 0){
    fprintf(stream,"\nState Changes for best of Run %d.\n",runno);
    fprintf(stream,"Occured In Generation [%d]",genno);
    if(steps)
        fprintf(stream," - Total Time: %.2f, Raw Fitness: %d\n",(steps*T_STEP),rfit);
    else
        fputc('\n',stream);
}

void Chrome::WriteSimStates(char *filename, int runno, int genno){
    FILE *tstream, *rstream;
    CartPole sim;
    SimState steprms;
    int i, bang =1;
    char trakfile[13], rmsfile[13];

    if(GetRunTime() == SUCCESS_TIME){
        strcpy(trakfile,filename);
        strcpy(rmsfile,filename);
        trakfile[4]='\0';
        rmsfile[5] = '\0';
        strcat(trakfile,"trak.txt");
        strcat(rmsfile,"rms.txt");
        if(((tstream = fopen(trakfile,"a"))!=NULL) &&((rstream = fopen(rmsfile,"a"))!=NULL)){
            PrintSHeading(tstream, runno, genno,steps,rfitness);
            PrintSHeading(rstream,runno,genno);
            fprintf(rstream,"\nRMS States after 1,000 seconds:\n");
            sim.PrintStates(rstream,rms);
            fputc('\n',rstream);
            sim.InitStates();
            steprms = sim.GetRMSState();
            sim.PrintStates(tstream, sim.GetCurrState());
            sim.PrintStates(rstream,steprms);
            for(i=2;(((i-1)*T_STEP)<=(TRACK_TIME)) && (sim.StillIn());i++){
                UpdateVars(sim.GetCurrState());
                bang = Evaluate();
                sim.TakeSimStep(bang);
                steprms += sim.GetCurrState();
                steprms.x = steprms.x/i;
                steprms.xdash = steprms.xdash/i;
                steprms.theta = steprms.theta/i;
                steprms.thetadash = steprms.thetadash/i;
                sim.PrintStates(tstream, sim.GetCurrState());
                sim.PrintStates(rstream,steprms);
            }
            fputc('\"',tstream);
            print(tstream);
            fprintf(tstream,"\"\n");
            fclose(tstream);
            fclose(rstream);
        }
    }
}

void Chrome::print(){
    GProgram::print();
    printf("\nRaw Fitness = %d\n",rfitness);
    printf("Rel Fitness = %f\n",nfitness);
    printf("Run Time = %f\n",(steps*T_STEP));
    printf("RMS x = %f\n",rms.x);
    printf("RMS x_d = %f\n",rms.xdash);
    printf("RMS theta = %f\n",rms.theta);
    printf("RMS theta_d = %f\n",rms.thetadash);
}

void Chrome::PrintRMS(FILE *stream = stdout){
    fprintf(stream,"RMS ");
    fprintf(stream,"x : %f ",rms.x);
    fprintf(stream,"x_dash : %f ",rms.xdash);
    fprintf(stream,"theta : %f ",rms.theta);
    fprintf(stream,"theta_dash : %f\n",rms.thetadash);
}

Population::Population(){
    int i = 0;

    for(i=0;i<MAX_POP;i++)
        individual[i] = NULL;
    sfitness = 0;
}

Population::Population(Population *c){
    int i = 0;

    for(i=0;i<MAX_POP;i++)
        if((individual[i] = new Chrome(c->individual[i]))==NULL)
            ExitGP("Insufficient memory to copy to next generation.");
    sfitness = c->sfitness;
}

Population::~Population(){
    int i = 0;

    for(i=0;i<MAX_POP;i++)
        if(individual[i])
            delete individual[i];
}

void Population::InitPop(){
    for(int i=0;i<MAX_POP;i++){
        if((individual[i] = new Chrome)==NULL)
            ExitGP("Insufficient memory to construct new population.");
        individual[i]->GenerateProg();
    }
    sfitness = 0;
}

void Population::SetFitness(){
    int i = 0;
    NFITNESS_TYPE normfitness = 0.0;
    sfitness = 0;

    for(i=0;i<MAX_POP;i++)
        sfitness += individual[i]->GetRFitness();
    for(i=0;i<MAX_POP;i++){
        normfitness = 0.0;
        if(sfitness)
            normfitness = (individual[i]->GetRFitness()/(NFITNESS_TYPE)sfitness);
        individual[i]->PutNFitness(normfitness);
    }
}

NFITNESS_TYPE Population::GetMean(){
    return sfitness/(NFITNESS_TYPE)MAX_POP;
}

void Population::Evaluate(){
    CartPole sim;

    for(int i=0;i<MAX_POP;i++){
        if(!(individual[i]->GetRFitness())){ // pointless to retest a budded chromes
            sim.RunCartPole(individual[i]);
            individual[i]->PutSteps(sim.GetSteps());
            individual[i]->PutRMSState(sim.GetRMSState());
            individual[i]->CalcRFitness();
        }
    }
    SetFitness();
}

void Population::GetCrossPoint(int child1, int &cross1, int &cross2){
    int child2 = child1+1;
    int brlength1 = 0, brlength2 = 0, newlength1 = 0, newlength2 = 0;
    Node *branch_ptr1 = NULL, *branch_ptr2 = NULL;

    do{
        cross1 = cross2 = 1;
        SetRandMax((individual[child1]->GetLength()));
        do{
            cross1 = (int)RandGen();
        }while(!cross1);
        SetRandMax((individual[child2]->GetLength()));
        do{
            cross2 = (int)RandGen();
        }while(!cross2);
        GetNthNode(individual[child1]->GetRoot(), &branch_ptr1, cross1);
        GetNthNode(individual[child2]->GetRoot(), &branch_ptr2, cross2);
        brlength1 = GetNodeCount(branch_ptr1);
        brlength2 = GetNodeCount(branch_ptr2);
        newlength1 = (individual[child1]->GetLength() - brlength1) + brlength2;
        newlength2 = (individual[child2]->GetLength() - brlength2) + brlength1;
    }while((newlength1 > MAX_LENGTH) || (newlength2 > MAX_LENGTH));
}

void Population::CrossOver(int &cnum, Chrome *parent1, Chrome *parent2){
    int x1, x2;
    Node *branch_ptr1, *branch_ptr2;

    if(((individual[cnum] = new Chrome(parent1))==NULL) || ((individual[cnum+1] = new Chrome(parent2))==NULL))
        ExitGP("Insufficient memory for crossover.");
    GetCrossPoint(cnum,x1,x2);
    branch_ptr1 = individual[cnum]->PruneNthNode(individual[cnum]->GetRoot(),x1);
    branch_ptr2 = individual[cnum +1]->PruneNthNode(individual[cnum +1]->GetRoot(),x2);
    individual[cnum]->GraftToTree(branch_ptr2,branch_ptr1!=individual[cnum]->GetRoot());
    individual[cnum+1]->GraftToTree(branch_ptr1,branch_ptr2!=individual[cnum+1]->GetRoot());
    individual[cnum]->ResetAttributes();
    individual[cnum+1]->ResetAttributes();
    cnum++;
}

void Population::Mutate(){
    int i = 0;

    for(i=0;i<MAX_POP;i++)
        if(TossCoin(P_MUTE))
            individual[i]->Mutate();
}

int Population::SpinWheel(Chrome *lastgen[MAX_POP]){
    int i = 0;
    NFITNESS_TYPE spin = 0;
    SetRandMax(MODULAR); /* MODULAR is defined in randgen.h */

    do{
        spin = RandGen();
    } while(!spin);
    for(i=0;(spin>0) && (i<MAX_POP);i++)
        spin -= lastgen[i]->GetNFitness() * MODULAR;if(i) --i;
    return i;
}

void Population::ExpectVR3(Chrome *lastgen[MAX_POP]){
    int i = 0, p1 = 0, p2 = 0;
    NFITNESS_TYPE mpool[MAX_POP];

    for(i=0;i<MAX_POP;i++)
        mpool[i] = lastgen[i]->GetNFitness() * MAX_POP;
    for(i=0;i<MAX_POP;i++){
        do{
            p1 = SpinWheel(lastgen);
        }while(mpool[p1] < 0);
        if((MAX_POP - i == 1) || (TossCoin(P_BUD))){
            individual[i] = new Chrome(lastgen[p1]);
            mpool[p1]--;
        }
        else{