/* File        : ga.cpp
 * Authors     : Rick van der Zwet & Thomas Steenbergen
 * S-number    : 0433373 / 0117544 
 * Version     : $Id: ga.cpp 612 2008-05-13 22:25:56Z rick $
 * Licence     : BSD
 * Description : 4th Assignment AI 2008: Genetic Algoithm
 */

#include <iostream>
#include <climits>
#include <ctime>
#include <cstdlib>
#include <fstream>
#include <math.h>
#include <string>
#include <sysexits.h>

using namespace std;

/*NOTE: Graph can only have this many nodes */
#define MAX_NODES 50
#define MAX_ARCHS 250

/*NOTE: Maximum numbers of newly generated children */
#define MAX_POP 20

/*NOTE: The chance to which we mutate a given point */
#define MUT_LEV 50

/*NOTE: The maximum number of generations that the algorithm runs */
#define DEFAULT_LOOPS 100000

#define DEFAULT_FILENAME "input.txt"
#define MAX_COORDINATES 1000


struct arch {
    int a, b;
    double distance;

    arch() {
    a = -1;
    b = -1;
    distance = -1;
    }
};

/* Coordinate of point in graph */
struct point {
    int x, y;       /* X,Y coordinates */
    
    point(){
      x = -1;
      y = -1;
    }
};

/* Comparision between 2 points */
bool operator==(point &a, point &b) {
    if ( (a.x == b.x) && (a.y == b.y))
        return true;
    else
        return false;
}

struct graph {
    point nodes[MAX_NODES];   /* Location of nodes */
    int fitness;              /* Overall fitness graph */
    int fitnessDistance;
    int fitnessIntersection;
    
    graph() {
      fitness = -1;
      fitnessDistance = -1;
      fitnessIntersection = -1;
    }
};

/*
 * BEGIN Global variables 
 */

arch archs[MAX_ARCHS];                /* arch listing in graph */
int num_archs = -1;                   /* Number of archs in graph */
int num_nodes = -1;                   /* Number of nodes in graph */
int max_cord = -1;                    /* Domain e.g. maximum coord of X, Y*/
int lon_con = -1;                     /* Longest connection of two points */
int distances [MAX_NODES][MAX_NODES]; /* Distances between node i and j */
graph population[MAX_POP];            /* Graph list */

/*
 * END Global variables
 */



/* Calculates the distance between two points
 * Distance (A,B) = d((x1,y1),(x2,y2))=SQRT((x1-x2)^2+(y1-y2)^2)
 */
double calcDistance(point A, point B) {
  double dist = 0;
  dist = sqrt(pow((A.x - B.x),2) + pow((A.y - B.y),2));
  return dist;
}

/* How well is the scaling of the branches ofthis graph weel 
 * versus the input graph. The more it deviates of the orginal the higher
 * the fitness number.
 */
int fitnessDistance(graph& A) {
  int i,j;
  int org_dist = 0;  // distance between 2 points in input graph
  double new_dist = 0;  // distance between 2 points in population graph
  double diff_dist = 0; // absolute difference
  int tmp_fitness = 0;

  for (i=0; i< num_nodes; i++) {
    for (j=i+1; j< num_nodes; j++) {
      org_dist = distances[i][j];
      if (org_dist != 0){
        new_dist = calcDistance(A.nodes[i],A.nodes[j]);
        diff_dist = fabs(new_dist - org_dist);
        tmp_fitness += diff_dist;
      }
    }
  }

  return(tmp_fitness);
}

/* Output point itself */
void printPoint(point &A) {
    cerr <<  A.x << "," << A.y;
}

/* Calculates whether the line A-B crosses with line C-D and wether in
 * domain if so a it returns the point of intersection else return point
 * [-1,-1] All explained in:
 * http://www.topcoder.com/tc?module=Static&d1=tutorials&d2=geometry2
 * http://en.wikipedia.org/wiki/Line-line_intersection
 */
bool calcIntersection(point A, point B, point C, point D, point& tmp) {
  double K, L, M;
  double S, T, R;
  double det;
  bool result;
  double distance_a_b;

  // rewrite line A-B into formula form: Kx + Ly = M
  K = B.y - A.y;
  L = A.x - B.x;
  M = K * A.x + L * A.y;
  
  // rewrite line C-D into formula form: Sx + Ty = R
  S = D.y - C.y;
  T = C.x - D.x;
  R = S * C.x + T * C.y;

  // Now we calculate the intersection between the lines
  det = K*T - S*L;

  if(det == 0){
    /* Lines A-B & C-D are parallel, checking wether they are on top of
     * each other
     */
    tmp.x = -1;
    tmp.y = -1;
    result = false;

    distance_a_b = calcDistance(A,B);
    if ((calcDistance(A,C) + calcDistance(B,C) == distance_a_b)) {
      tmp.x = C.x;
      tmp.y = C.y;
      result = false;
    } else if ((calcDistance(A,D) + calcDistance(B,D) == distance_a_b)) {
      tmp.x = D.x;
      tmp.y = D.y;
      result = false;
    }

  } else {
    tmp.x = (T*M - L*R)/det;
    tmp.y = (K*R - S*M)/det;
    result = true;

    /* Verify intersection in domain */
    if (tmp.x < 0 || tmp.x >= max_cord || tmp.y < 0 || tmp.y >= max_cord) {
        result = false;
    }

    /* Verify intersection not a actual end point */
    if ((tmp == A || tmp == B) && (tmp == C || tmp == D)) {
        result = false;
    }
  }

  return result;
}

bool calcIntersection(point A, point B, point C, point D) {
    point tmp;
    return calcIntersection(A, B, C, D, tmp);
}

/*
 * The number of intersections a graph. How more intersections the higher
 * the fitness number.
 */
int fitnessIntersection(graph& A) {
  int i,j;
  int tmp_fitness = 0;

  for (i = 0; i < num_archs; i++) {
    for (j = i + 1; j < num_archs; j++) {
      if ( calcIntersection(A.nodes[archs[i].a],
                            A.nodes[archs[i].b],
                            A.nodes[archs[j].a],
                            A.nodes[archs[j].b])) {
         tmp_fitness++;
      }
    }
  }
  return(tmp_fitness);
}


/* Calculates the fitness of every graph in the population */
void calcFitness(graph& A) {
    A.fitnessIntersection = fitnessIntersection(A);
    A.fitnessDistance = fitnessDistance(A);
    A.fitness = A.fitnessDistance + A.fitnessIntersection;
}

void crossover(graph& A, graph& B){
    /* XXX: Find clever way to combine the different graphs to be able
     * to make new ones. Three to expiriment with:
     * - uniform crossover
     * - single-point crossover
     * - partially mapped crossover
     * All explained in: http://www.liacs.nl/~kosters/AI/genetisch.pdf
     */

}

// Combine two graphs using single point crossover
// A single random point is chosen in a graph's node
// array dividing it into two halves e.g. the head and the tail.
// Then heads are swapped between parents A & B
void crossSingle(graph& A, graph& B){
   point tmp;
   unsigned int cut;
   unsigned int i;

   cut = rand() % num_nodes;
   for (i=0; i< cut;i++) {
       tmp = A.nodes[i];
       A.nodes[i] = B.nodes[i];
       B.nodes[i] = tmp;
   }
}

// Combine two graphs using uniform crossover
// The points are swapped with a fixed probability of 0.5.
void crossUniform(graph& A, graph& B){
   point tmp;
   int i, rnd;

   for (i=0; i< num_nodes;i++) {
     rnd = rand()% 2;
     if (rnd == 1){
       tmp.x = A.nodes[i].x;
       A.nodes[i].x = B.nodes[i].x;
       B.nodes[i].x = tmp.x;
     }
     rnd = rand()% 2;
     if (rnd == 1){
       tmp.y = A.nodes[i].y;
       A.nodes[i].y = B.nodes[i].y;
       B.nodes[i].y = tmp.y;
     }
   }
}


// Copies the contents of graph A to graph B
void copyGraph(graph & A, graph& B){
  int i;

  for (i=0; i< num_nodes;i++) {
    B.nodes[i] = A.nodes[i];
  }
  B.fitness = A.fitness;
  B.fitnessIntersection = A.fitnessIntersection;
  B.fitnessDistance = A.fitnessDistance;
}

/* Mutate random point in a graph and change it to random value
 * within the domain of points
 */
void mutateGraph (int mutationLevel, graph& A){
  int i,x,y;

  if ((rand() % 100) > mutationLevel) {
    return;
  }

  i = rand() % num_nodes;
  x = (rand()% max_cord)+1;
  y = (rand()% max_cord)+1;

  A.nodes[i].x = x;
  A.nodes[i].y = y;
}

/* To do selection we use roulettewheel selection, only we 
 * invert adjust the regular algorithm so it prefers
 * the lowest fitness numbers e.g. the biggest slice of
 * piece is now the least attractive.
 */
int selectGraph() {
  int i;
  int choice = -1;
  int combined_fitness;
  int fitness_reverse[MAX_POP];
  int max_fitness = INT_MIN;
  int min_fitness = INT_MAX;
  int total_fitness = 0;
  int wheelnumber;

  /* Find minimum/maximum */
  min_fitness = population[0].fitness;
  max_fitness = population[MAX_POP - 1].fitness;

  /* Set balanced fitness */
  combined_fitness = min_fitness + max_fitness;
  for(i=0; i< MAX_POP; i++) {
    fitness_reverse[i] = combined_fitness - population[i].fitness;
    total_fitness += fitness_reverse[i];
  }

 /* Get random number of wheel */
  wheelnumber = rand() % total_fitness;

  /* Find matching graph */
  total_fitness = 0;
  for(i=0; i< MAX_POP; i++) {
    total_fitness += fitness_reverse[i];
    if (total_fitness > wheelnumber) {
        choice = i;
        break;
    }
  }

  return (choice);
}

/* Set the values of a  given graph to random numbers
 * In other word those graphs who aint fit enough 
 * for the next round will be discarded. 
 */
void setRandGraph(graph& A) {
  int i;
  for (i=0; i< num_nodes;i++) {
     A.nodes[i].x = (rand()%max_cord)+1;
     A.nodes[i].y = (rand()%max_cord)+1;
   }
  
  calcFitness(A);
}

/* Create a graph and set nodes to certain location */
graph initGraph() {
   graph A;
   setRandGraph(A);
   return A;
}

// Generates a population of MAX_POP graphs with random coordinates
void initPopulation () {
  int i;

  for (i=0; i< MAX_POP;i++) {
    population[i] = initGraph();
  }
}
// Using bubblesort we sort the graphs in the population on fitness
void sortPopulation () {
 graph tmp;
 int i, j;
 for (j = 1; j < MAX_POP; j++ ) {
  for (i = 0; i < MAX_POP-j; i++ ) {
    if (population[i].fitness > population[i+1].fitness) {
      tmp = population[i];
      population[i] = population[i+1];
      population[i+1] = tmp;
    }
  }
 }
}

/* Opens input file with adjacency-matrix which contains data about the
 * number of nodes (N) on the first line and the values of the
 * connections between those nodes in a N x N matrix in the following
 * lines
 */
void openFile(char * inputFile) {
  ifstream input;
  int i, j;
  input.open(inputFile, ios::in);

  if (input) {
    cerr << "Opening "<< inputFile << "..." << endl;
    input >> num_nodes;
    if (num_nodes <= 0) {
        input.close();
        cerr << "Error: Invalid data format!" << endl;
        exit(EX_DATAERR);
    } else if (num_nodes < MAX_NODES) {
      for (i=0; i< num_nodes; i++) {
        for (j=0; j< num_nodes; j++) {
          input >> distances[i][j];
          if (input.eof()) {
            cerr << "Error: Invalid data format!" <<endl;
            exit(EX_DATAERR);
          }
          if (distances[i][j] > lon_con){
            lon_con = distances[i][j];
          }
        }
      }
    } else {
        input.close();
        cerr << "Error: Number of nodes in "<< inputFile;
        cerr << " exceeds maximum number of nodes" << endl;
        exit(EX_DATAERR);
    }
  } else {
      input.close();
      cerr << "Error: Couldn't open file " << inputFile << endl;
      exit(EX_NOINPUT);
  }

  /* Transform 2d structure to 1d archs to allow easy computations */
  num_archs = 0;
  for (i = 0; i < num_nodes; i++) {
    for (j = i+1; j < num_nodes; j++) {
      if (distances[i][j] > 0) {
        archs[num_archs].a = i;
        archs[num_archs].b = j;
        archs[num_archs].distance = distances[i][j];
        num_archs++;
      }
    }
  }
}

// Prints the adjacency-matrix of openFile() to the screen
void printDistances() {
  int i, j;

  cout << " " << endl;
  for (i=0; i< num_nodes; i++) {
    for (j=0; j< num_nodes; j++) {
            cout << distances[i][j]<< " ";
    }
    cout << endl;     
  }
  cout << " " << endl;
}

/* Prints the coordinates of every point in a graph to the screen in
 * graphviz compatible output
 * cat <<EOF | neato  -Tpng -oga.png  && open ga.png
 */
void printGraph(graph& A) {
  int i;

  cout << "graph G { node [shape=circle,"
       << "fontname=\"Lucida Console\",margin=0,0];" << endl;
  for (i=0; i< num_nodes; i++) {
     cout << "C" << i << "[pos=\"" << A.nodes[i].x * 28 << "," <<
     A.nodes[i].y * 28 << "!\", label=\"C" << i << "\"];" << endl; 
  }
  for (i=0; i < num_archs; i++) {
    cout << "C" << archs[i].a << " -- C" << archs[i].b << ";" << endl;
  }
  cout << "}" << endl;
}


// Prints every graph stored in array population
void printPopulation () {
  int i;
  cerr << " " << endl;
  for (i=0; i< MAX_POP; i++) {
    cerr << i << " => ";
    printGraph(population[i]);
  }
  cerr << " " << endl;
}


/* Implementation of Steady State Evolution Algorithm based on p.119 AI
 * Book */
int main(int argc, char * argv[]) {
  int i,j;
  int org_dist, new_dist;
  int select_one, select_two;
  int loopCounter;
  unsigned int randomSeed;

  graph min;
  graph child_one, child_two;

  // use random seed to create x,y coordinates of a point
  randomSeed = (unsigned)time(0);
  randomSeed = 0;
  srand(randomSeed);
  /* Debug static seed */
  // srand(0);
  
  // Open the file that  contains data about
  // the number odf branches and which branches are 
  // connected with each other
  if (argc >= 2) {
    openFile(argv[1]);
  } else {
    cerr << "Usage: " << argv[0] << " <filename> <loopCount>" << endl;
    exit(EX_USAGE);
  }

  if (argc == 3) {
    loopCounter = atoi(argv[2]);
  } else {
    loopCounter = DEFAULT_LOOPS;
  }

  /* To optimize the speed of the genetic algoritm we limit the domain
   * of the points */
  if ((lon_con * num_nodes) < MAX_COORDINATES){
    max_cord = lon_con * num_nodes;
  } else {
    max_cord = MAX_COORDINATES;
  }
  cerr << "Domain of points is set to "
       << max_cord << " x " << max_cord << endl;
  
  // Minimum graph to store best found solution
  min.fitness = INT_MAX;
  
  /* Populate population, with random values */
  initPopulation();
  
  for (i=0; i< loopCounter; i++) {
     // Sort the population of graphs so that graph
     // with smallest fitness is placed in population[0]
     sortPopulation();
     
     // Store the lowest found fitness if it is better
     // then the fitness we already had stored
     if (min.fitness > population[0].fitness){
        copyGraph(population[0],min);
     }
          
     // Stop if optimal is found e.g fitness equals zero
     if(population[0].fitness == 0){
      break;
     }
          
     // Selection reproducing parents via roulette wheel
     // fittest parents get selected 
     select_one = selectGraph();
     do {
       select_two = selectGraph();
     }while (select_one == select_two);
               
     // set points in children with crossover result of parents 
     copyGraph(population[select_one], child_one);
     copyGraph(population[select_two], child_two); 
     
     // Crossover the selected parents  
     crossUniform(child_one, child_two);
     
     // mutate childs with small random probability usually 50%
     mutateGraph(99, child_one);
     mutateGraph(99, child_two);
     
     // Calculate fitness of both children
     calcFitness(child_one);
     calcFitness(child_two);
     
     // Least fittest graphs in population get replaced
     copyGraph(child_one,population[MAX_POP -2]);
     copyGraph(child_two,population[MAX_POP -1]);
  }
  
  cerr << "Best found coordinates after "<< i <<
  " epochs for given input graph: " << endl; 
  printGraph(min);
  
  if(min.fitness != 0){
    for (i=0; i< num_nodes; i++) {
      for (j=i+1; j< num_nodes; j++) {
        org_dist = distances[i][j];
        if (org_dist != 0){
          new_dist = calcDistance(min.nodes[i],min.nodes[j]);
          cerr << "Distance between point C"<< i << " - C" << j << " = ";
          cerr << calcDistance(min.nodes[i],min.nodes[j]) << " ";
          if (new_dist != org_dist){
            cerr << "SHOULD BE " << org_dist << endl;
          } else {
            cerr << "CORRECT" << endl;
          }
        }
      }
    }
   }
  cerr << "Fitness Intersection : " << min.fitnessIntersection << endl;
  cerr << "Fitness Distance     : " << min.fitnessDistance << endl;
  cerr << "Fitness Overall      : " << min.fitness << endl;
  cerr << "Random seed          : " << randomSeed << endl;
  return(EX_OK);
}