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{
            mpool[p1] -= 0.5;
            do{
                p2 = SpinWheel(lastgen);
            }while(mpool[p2] < 0);
            mpool[p2] -= 0.5;
            CrossOver(i, lastgen[p1], lastgen[p2]);
        }
    }
}

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

    for(i=0;i<MAX_POP;i++){
        p1 = SpinWheel(lastgen);
        if((MAX_POP - i == 1) || (TossCoin(P_BUD)))
            individual[i] = new Chrome(lastgen[p1]);
        else{
            p2 = SpinWheel(lastgen);
            CrossOver(i, lastgen[p1], lastgen[p2]);
        }
    }
    for(i=0;i<MAX_POP;i++)
        individual[i]->SetLength();
}

void Swap(int tpool[MAX_POP][3],int loser, int bench){
    int temp[3];

    if(loser != bench){
        for(int i=0;i<3;i++){
            temp[i] = tpool[loser][i];
            tpool[loser][i] = tpool[bench][i]; // keep last player in
            tpool[bench][i] = temp[i]; // put loser on the bench
        }
    }
}

void Population::RutChromes(int *mpool, Chrome *buck[MAX_POP]){
// the chromes position in the population array is kept in tpool[][0],
// the tournament results are kept in tpool[][1] and the flag to indicate
// whether the buck is still in the fight is kept in tpool[][2].

    int tpool[MAX_POP][3] = {{0,0,0}};
    int competitors = MAX_POP-1;
    int i,j;

    for(i=0;i<MAX_POP;i++){
        tpool[i][0] = i;
        tpool[i][2] = 1;
    }
    while(competitors){
        SetRandMax(competitors);
        do{
            i = (int)RandGen();
        }while(!tpool[i][2]);
        do{
            j = (int)RandGen();
        }while((!tpool[j][2]) || (i == j));
        if(*buck[tpool[i][0]] > *buck[tpool[j][0]]){
            tpool[i][1]++;
            tpool[j][2] = 0;
            Swap(tpool,j,competitors);
        }
        else{
            tpool[j][1]++;
            tpool[i][2] = 0;
            Swap(tpool,i,competitors);
        }
        competitors--;
    }
    tpool[0][1]++;
    for(i=0;i<MAX_POP;i++)
        mpool[tpool[i][0]] = tpool[i][1];
}

void Population::Tournament(Chrome *lastgen[MAX_POP]){
    int i = 0, p1 = 0, p2 = 0;
    int mpool[MAX_POP] = {0};
 
    RutChromes(mpool, lastgen);
    for(i=0;i<MAX_POP;i++){
        SetRandMax(MAX_POP-1);
        do{
            p1 = (int)RandGen();
        }while(mpool[p1] == 0);
        mpool[p1]--;
        if((MAX_POP - i == 1) || (TossCoin(P_BUD)))    //only room for one more in population or time to bud.
            individual[i] = new Chrome(lastgen[p1]);
        else{
            SetRandMax(MAX_POP-1);
            do{
                p2 = (int)RandGen();
            }while(mpool[p2] == 0);
            mpool[p2]--;
            CrossOver(i, lastgen[p1], lastgen[p2]);
        }
    }
    for(i=0;i<MAX_POP;i++)
        individual[i]->SetLength();
}

Chrome *Population::CopyBest(){
    int i, best = 0;
    Chrome *star;

    for(i=0;i<MAX_POP;i++)
        if(*individual[i]>*individual[best])
            best = i;
    if((star = new Chrome(individual[best]))==NULL)
        ExitGP("Insufficient memory to store population\'s best");
    return star;
}

void Population::print(){
    cout << "Sum fitness = " << sfitness << endl;
    for(int i=0;i<MAX_POP;i++){
        individual[i]->print();
        cout << endl;
        cout << "where:" << endl;
        individual[i]->printvalues();
    }
}

boolean operator>(Chrome& c1, Chrome& c2){
    boolean result = FALSE;

    if (c1.GetRFitness() > c2.GetRFitness())
        result = TRUE;
    return result;
}

boolean operator>=(Chrome &c1, Chrome &c2){
    boolean result = FALSE;

    if (c1.GetRFitness() >= c2.GetRFitness())
        result = TRUE;
    return result;
}

int SpinGlassIf(RETURN_TYPE x){
    int result = -1;

    if(x>0)result = 1;return result;
}

int GetBestofBest(Chrome *best[MAX_GEN]){
    int i, bestofbest = 0;

    for(i=0;i<MAX_GEN;i++)
        if(*best[i]>*best[bestofbest])bestofbest = i;
            return bestofbest;
}

void DeleteBestOfRun(Chrome *best[MAX_GEN]){
    for(int i = 0;i<MAX_GEN;i++)
        delete best[i];
}

#ifndef _DATAOUT
#define _DATAOUT
#include "chrome.h"
#include "glbdefs.h"
#define MAX_STYPE 8 // selection type (converted to filename)
#define DELIMIT ',' // field delimitter in data file.

char *GetSType(int, char*);
void WriteBestFit(Chrome *best[MAX_GEN], char *filename);
void WriteBestTime(Chrome *best[MAX_GEN], char *filename);
void WriteBest(Chrome *best[MAX_GEN], char *filename, int runno);
void WriteGenMean(NFITNESS_TYPE genmean[MAX_GEN], char *filename);
void DisplayBestOfRun(Chrome *best[MAX_GEN]);
void DeleteBestOfRun(Chrome *best[MAX_GEN]);
#endif

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

void ShowArgs(){
    printf("\n\nArgument List for the C++ Genetic Program\n\n");
    printf("\ts : Stochastic (Roulette Wheel) Selection.\n");
    printf("\te : Expected Value Model Selection.\n");
    printf("\tt : Tournament Selection (Binary).\n");
    printf("\nThe program's second argument is an optional seed");
    printf("\nfor the random number generator.\n");
    printf("\nexamples :");
    printf("\n\tcppgp e 1 - Expected Value Model Selection, seed with 1.");
    printf("\n\tcppgp t - Tournament Selection using default seed.");
    printf("\n\nnote: A seed can only be supplied if the selection type is ");
    printf("\n explicitly stated on the command line.");
    printf("\n If no arguments are supplied \'Roulette Wheel Selection\',");
    printf("\n using the default seed, is assumed.\n\n");
}

char *GetSType(int args, char *stype){
    char *filename;

    filename = (char*)malloc((MAX_STYPE+1)*sizeof(char));
    if(args>1){
        switch (*stype){
            case 't': strcpy(filename,"tornment"); break;
            case 'e': strcpy(filename,"expctvr3"); break;
            case 'r': strcpy(filename,"random_s"); break;
            case 's': strcpy(filename,"roulette"); break;
            default : ShowArgs();
            ExitGP("Terminating Run");
        }
    }
    else strcpy(filename,"roulette");
    return filename;
}

void WriteGenMean(NFITNESS_TYPE meanfit[MAX_GEN], char *stype){
    char filename[13];
    FILE *stream;
    int i = 0;

    strcpy(filename,stype);
    filename[4] = '\0';
    strcat(filename,"mean.txt");
    if((stream=fopen(filename,"a"))!=NULL){
        for(i = 1;i<=MAX_GEN;i++){
            fprintf(stream, "%.2f", meanfit[i-1]);
            if(i<MAX_GEN)
                fputc(DELIMIT,stream);
        }
    fputc('\n',stream);
    fclose(stream);
    }
}

void WriteBestFit(Chrome *best[MAX_GEN], char *stype){
    char filename[13];
    FILE *stream;
    int i;

    strcpy(filename,stype);
    strcat(filename,".txt");
    if((stream=fopen(filename,"a"))!=NULL){
        for(i = 1;i<=MAX_GEN;i++){
            fprintf(stream, "%d", best[i-1]->GetRFitness());
            if(i<MAX_GEN)
                fputc(DELIMIT,stream);
        }
        fputc('\n', stream);
        fclose(stream);
    }
}

void WriteBest(Chrome *best[MAX_GEN], char *stype, int runno){
    char filename[13];
    FILE *stream;
    int bestofbest = 0,i;

    strcpy(filename,stype);
    filename[4] = '\0';
    strcat(filename,"best.txt");
    if((stream=fopen(filename,"a"))!=NULL){
        fprintf(stream,"Results of Run %d.",runno);
        for(i = 1;i<=MAX_GEN;i++){
            fprintf(stream, "\nBest of Generation %d,", (i-1));
            best[i-1]->PrintRMS(stream);
            best[i-1]->print(stream);
            if(*best[i-1] > *best[bestofbest])
                bestofbest = i;
        }
        fprintf(stream, "\nBest of Run %d occured ",runno);
        fprintf(stream,"in Generation %d.\n", bestofbest);
        best[bestofbest]->print(stream);
        fprintf(stream,"\n\n");
        fclose(stream);
    }
}

void WriteBestTime(Chrome *best[MAX_GEN], char *stype){
    char filename[13];
    FILE *stream;
    int i;

    strcpy(filename,stype);
    filename[4] = '\0';
    strcat(filename,"time.txt");
    if((stream=fopen(filename,"a"))!=NULL){
        for(i = 1;i<=MAX_GEN;i++){
            fprintf(stream, "%4.2f", best[i-1]->GetRunTime());
            if(i<MAX_GEN)
                fputc(DELIMIT,stream);
        }
        fputc('\n',stream);
        fclose(stream);
    }
}

void DisplayBestOfRun(Chrome *best[MAX_GEN]){
    unsigned cols = 4;
 
    cout << "\n\nResults of last Run (Best of Generation): " << endl;
    for(int i=1;i<=MAX_GEN;i++){
        cout << "[" << setw(2) << (i-1) << "] : ";
        cout << setw(4) << best[i-1]->GetRFitness() << " ";
        if ((i>=(cols-1)) && !(i%(cols-1)))
            cout << endl;
    }
    cout << endl;
}

#ifndef _GLBDEFS
#define _GLBDEFS

#include <float.h>
#ifdef _GP_WIN_
#define CLEAR                                        // Compiled as a windows program.
#define MAX_POP 500                          // Windows population size.
#else
#ifdef _GP_UNIX_
#define MAX_POP 4000                        // Unix population size.
#else
#define MAX_POP 100                         // Dos population size.
#endif
#define CLEAR cout << "\033[2J"      // Dos or Unix (ANSI)
#endif
#define MAX_RUNS 50                      // Number of runs per execution.
#define MAX_GEN 51                         // Total generation (i.e., gen.0 + gen.1 ... 50).
#define P_VAR 0.5                               // Probability of selecting a variable as terminal.
#define P_BUD 0.1                               // Probability of reproduction (i.e., 90% crossover)
#define P_MUTE 0                              // Probability of mutation.
#define MAX_TOKENS 1024           // Total tokens in the function & terminal sets.
#define MAX_LENGTH 15               // Max. length of chromes before incurring penalty.
#define INIT_DEPTH 6                     // Min. initial length of chromes in generation 0.
#define X_BONUS 10                       // Max. additional fitness awarded for Cart position
#define RETURN_TYPE float          // Return type of the (GP) functions generated.
#define MAX_RTYPE FLT_MAX // Max. allowable value for this data type.
#define RFITNESS_TYPE int          // Precision of raw fitness score.
#define NFITNESS_TYPE double // Precision of relative fitness.
#define SFITNESS_TYPE unsigned long // Precision of sum fitness.

enum boolean{FALSE,TRUE};
struct SimState{
    RETURN_TYPE x;
    RETURN_TYPE xdash;
    RETURN_TYPE theta;
    RETURN_TYPE thetadash;
    SimState &operator=(SimState&);
};

SimState operator+(SimState i, SimState j);
SimState operator+=(SimState &i, SimState j);
void pause(char *msg);
void ExitGP(char *msg);
#endif

#include <stdlib.h>
#include <stdio.h>
#include <iostream.h>
#include "glbdefs.h"

SimState& SimState::operator=(SimState& i){
    this->x = i.x;
    this->xdash = i.xdash;
    this->theta = i.theta;
    this->thetadash = i.thetadash;
    return *this;
}

SimState operator+(SimState i, SimState j){
    SimState sum;

    sum.x = i.x + j.x;
    sum.xdash = i.xdash + j.xdash;
    sum.theta = i.theta + j.theta;
    sum.thetadash = i.thetadash + j.thetadash;
    return sum;
}

SimState operator+=(SimState &i, SimState j){
    i.x += j.x;
    i.xdash += j.xdash;
    i.theta += j.theta;
    i.thetadash += j.thetadash;
    return i;
}

void pause(char *msg){
    char c; cout << "\n" << msg << "\n<Enter> to continue." << endl;
    c = getchar();
}

void ExitGP(char *msg){
    pause(msg);
    exit(3);
}

#ifndef _TOKENS
#define _TOKENS

#include <stdio.h>
#include <iostream.h>
#include <iomanip.h>
#include <math.h>
#include <string.h>
#include <float.h>
#include "glbdefs.h"

struct Node;

class Token{
protected:
    char *name;
public:
    Token() {};
    Token(char*);
    ~Token() {delete name;};
    boolean IsVar();
    virtual void print(FILE *stream=stdout) {fprintf(stream,name);};
    virtual void printvalue() {};
    virtual RETURN_TYPE Evaluate(Node *c); //default behaviour
    virtual boolean IsTerminal() {return FALSE;};
    virtual void PutValue(RETURN_TYPE) {};
    virtual int GetArgNum() {return 0;}; // for terminals
};

class Function:public Token{
protected:
    RETURN_TYPE (*funct)(Node*);
    int args;
public:
    Function(){args = 0;};
    Function(char *n, RETURN_TYPE (*f)(Node*),int a):Token(n), funct(f), args(a){};
    ~Function() {};
    RETURN_TYPE Evaluate(Node *c) {return funct(c);};
    int GetArgNum() {return args;};
    void print(FILE *stream = stdout);
    void printvalue() {Token::print(); };
};

class Terminal:public Token{
protected:
    RETURN_TYPE value;
public:
    Terminal(){value = 0;};
    Terminal(char *n, RETURN_TYPE v):Token(n), value(v) {};
    ~Terminal() {};
    RETURN_TYPE Evaluate(Node *c);
    void PutValue(RETURN_TYPE v) {value = v;};
    boolean IsTerminal() {return TRUE;};
    void printvalue() {Token::print(); cout << " = " << value << endl;};
};

void FillFunctTable();
void DeleteFunctTable();
RETURN_TYPE SafeDiv(RETURN_TYPE, RETURN_TYPE);
RETURN_TYPE SafeSqr(RETURN_TYPE);
RETURN_TYPE SafeSqrt(RETURN_TYPE);
RETURN_TYPE SafeAbs(RETURN_TYPE);
void UpdateVars(SimState&);
void LoadTestTable(int,...);

#endif

#include <stdlib.h>
#include <stdio.h>
#include <stdarg.h>
#include "chrome.h"
#include "tokens.h"
#include "randgen.h"
#include "glbdefs.h"
#include "cartpole.h"

int T_VARS = 0;
int T_FUNCTS = 0;
class Token *FunctTable[MAX_TOKENS];

Token::Token(char *n){
    name = new char[strlen(n) + 1];
    strcpy(name,n);
}

RETURN_TYPE Token::Evaluate(Node *c){return 0;} //default behaviour

boolean Token::IsVar(){
    boolean result = FALSE;

    for (int i=T_FUNCTS;i<(T_FUNCTS+T_VARS);i++)
        if(this == FunctTable[i])
            result = TRUE;
    return result;
}

void Function::print(FILE *stream){
    fputc('(',stream);
    Token::print(stream);
}

RETURN_TYPE Terminal::Evaluate(Node *c){
    return (RETURN_TYPE)value;
}

RETURN_TYPE SafeDiv(RETURN_TYPE x, RETURN_TYPE y){
    RETURN_TYPE result = 0;

    if((y) && ((x/y)>MAX_RTYPE))
        result = MAX_RTYPE;
    if((y) && !(result))
        result = x/y;
    return result;
}

RETURN_TYPE SafeSqr(RETURN_TYPE x){
    RETURN_TYPE result = MAX_RTYPE;

    if(SafeAbs(x)<sqrt(MAX_RTYPE))
        result = x * x;return result;
}

RETURN_TYPE SafeSqrt(RETURN_TYPE x){
    RETURN_TYPE result = x;

    if(x<0)
        x = -(x);
    result = sqrt(x);
    return result;
}

RETURN_TYPE SafeAbs(RETURN_TYPE x){
    RETURN_TYPE result = x;

    if(x<0)
        result = -(x);
    return result;
}

RETURN_TYPE ADD(Node *c){return(c->EvalLeftArg()+ c->EvalRightArg());};
RETURN_TYPE SUB(Node *c){return(c->EvalLeftArg() - c->EvalRightArg());};
RETURN_TYPE MULT(Node *c){return(c->EvalLeftArg() * c->EvalRightArg());};
RETURN_TYPE DIV(Node *c){return(SafeDiv(c->EvalLeftArg(),c->EvalRightArg()));};
RETURN_TYPE ANTI(Node *c){return -(c->EvalLeftArg());};

void InitConstants(){
    int i, kstart = T_FUNCTS + T_VARS;
    float constval = 0;
    char conststr[10];

    SetRandMax(10000);
    for(i=kstart;i<MAX_TOKENS;i++){
        constval = RandGen()*0.001;
        if(TossCoin(0.5))
            constval = -(constval);
        sprintf(conststr,"%.3f",constval);
        FunctTable[i] = new Terminal(conststr,constval);
    }
}

void UpdateVars(SimState &j){
    int i = T_FUNCTS;
 
    FunctTable[i]->PutValue(j.x);
    FunctTable[++i]->PutValue(j.xdash);
    FunctTable[++i]->PutValue(j.theta);
    FunctTable[++i]->PutValue(j.thetadash);
}

void InitFunctTable(){
    int *i,*j;

    T_FUNCTS = 0;
    T_VARS = 0;
    i = &T_FUNCTS;
    j = &T_VARS;
    FunctTable[(*i)++] = new Function("+ ",ADD,2);
    FunctTable[(*i)++] = new Function("- ",SUB,2);
    FunctTable[(*i)++] = new Function("* ",MULT,2);
    FunctTable[(*i)++] = new Function("%% ",DIV,2);
    FunctTable[(*i)++] = new Function("Anti ",ANTI,1);
    FunctTable[*i+((*j)++)] = new Terminal("x",INIT_X);
    FunctTable[*i+((*j)++)] = new Terminal("x_dash",INIT_XDASH);
    FunctTable[*i+((*j)++)] = new Terminal("theta",INIT_THETA);
    FunctTable[*i+((*j)++)] = new Terminal("theta_dash",INIT_THETADASH);
}

void FillFunctTable(){
    InitFunctTable();
    InitConstants();
}

void LoadTestTable(int argno,...){
    int i, kstart;
    float constval;
    char *endptr, conststr[10];
    va_list ap;

    va_start(ap,argno);
    InitFunctTable();
    kstart = T_FUNCTS + T_VARS;
    for(i = kstart; i < (argno+kstart); ++i){
        strcpy(conststr, va_arg(ap,char*));
        constval = strtod(conststr, &endptr);
        FunctTable[i] = new Terminal(conststr,constval);
    }
    va_end(ap);
}

void DeleteFunctTable(){
    for(int i=0;i<MAX_TOKENS;i++)
        delete FunctTable[i];
}