import processing.core.*; 
import processing.data.*; 
import processing.event.*; 
import processing.opengl.*; 

import controlP5.*; 

import java.util.HashMap; 
import java.util.ArrayList; 
import java.io.File; 
import java.io.BufferedReader; 
import java.io.PrintWriter; 
import java.io.InputStream; 
import java.io.OutputStream; 
import java.io.IOException; 

public class CarLearning extends PApplet {



ControlP5 cp5;

Map gameMap;
Car gameCar;
Network neuralnet;
int gameH;
int gameW;
int networkH;
int networkW;


//PFont font;


public void setup() {
  
  gameH = height;
  gameW = floor(width*.4f);
  networkH = height;
  networkW = width-gameW;
  setupSigmoid();
  gameMap = new Map(floor(gameW*.8f),floor(gameH*.8f));
  neuralnet = new Network(2,3,2);
  PVector position = new PVector(width/2, height/2);
  PVector direction = new PVector(1,1);
  gameCar = new Car(position, direction, 10, 3, 1);
  background(100);
  gameMap.draw();
  loadPixels();
  // create a new button with name 'buttonA'
  // The vlw file should be saved in the sketch folder
  //font = loadFont("font.vlw");
  //textFont(font, 10);
  textSize(10);
  cp5 = new ControlP5(this);

  cp5.addButton("zeroAll")
  .setValue(0)
  .setPosition(gameW+20,20)
  .setSize(50,20)
  .setCaptionLabel("zero all")
  ;

  cp5.addButton("randomAll")
  .setValue(100)
  .setPosition(gameW+20,50)
  .setSize(50,20)
  .setCaptionLabel("randomise")
  ;

  cp5.addSlider("speed")
    .setPosition(20,20)
    .setRange(0.2f,5.0f)
    .setSize(50,20)
    .setValue(3.0f);
    ;
}

public void zeroAll(int theValue) {
  neuralnet.zeroAll();
}

public void randomAll(int theValue) {
    neuralnet.randomAll();
}

public void speed(float theValue) {
    //println(theValue);
    gameCar.speed = theValue;
}


public void draw() {

  background(0);
  fill(100);
  rect(0,0,gameW,gameH);
  gameMap.draw();
  gameCar.draw();
  float[] vision = { gameCar.eyeA, gameCar.eyeB};
  neuralnet.respond(vision);
  neuralnet.display();
}
class Car {
  PVector   position = new PVector();
  PVector   velocity = new PVector();
  PVector   rotation = new PVector();
  float     damper;
  float     diameter;
  float     speed;
  float     radius;
  float     eyeA;
  float     eyeB;
  boolean  draging;
  Car(PVector position, PVector direction, float diameter, float speed, float damper)
  {
    this.position = position;
    this.velocity = direction.normalize();
    this.damper = damper; // damper acts like resistance againsts the object, slowing it down over time
    this.rotation = velocity.copy();
    this.speed = speed;
    this.velocity.mult(speed); //velocity is the combination of speed and direction
    this.diameter = diameter;
    this.radius = diameter/2;
  }

  public void draw()
  {

    if(!dragNdrop()) {
      velocity.mult(damper);
      velocity.normalize();
      velocity.mult(speed);
      position.add(velocity);
      position = screenWrap(position);
    }
    // visuel stuff
    pushStyle();
    pushMatrix();
    translate(position.x, position.y);
    rotate(rotation.heading());
    noStroke();

    triangle(diameter+3, 0, -diameter, diameter, -diameter, -diameter);
    popMatrix();
    popStyle();

    //
    PVector eye1 = new PVector(20,10);
    eye1.rotate(rotation.heading());
    eye1.add(position.copy());
    PVector eye2 = new PVector(20,-10);
    eye2.rotate(rotation.heading());
    eye2.add(position.copy());
    noStroke();
    ellipse(eye1.x,eye1.y,5,5);
    eyeA = vision(eye2); // inverted, need to fix
    ellipse(eye2.x,eye2.y,5,5);
    eyeB = vision(eye1); // inverted, need to fix
  }

  public void updateAngle(float theta) {
    if (!draging) {
      rotation.rotate(theta);
      velocity.rotate(theta);
    }
  }

  public float x() {
    return position.x;
  }

  public float y() {
    return position.y;
  }

  public float vision(PVector pos) {
    int pixel = floor(pos.x) + floor(pos.y)*width;
    if(pixel< width*height && pixel >0) {

      if (pixels[pixel] == color(255)) {
        return(1.0f);
        //  println("On track");
      }
    }
    return(-1.0f);
  }
  public void steerRight() {
    updateAngle(.1f);
  }

  public void steerLeft() {
    updateAngle(-.1f);
  }
  public void steer(float angle) {
    updateAngle(angle);
  }

  public boolean dragNdrop() {
    if (dist(mouseX,mouseY, position.x,position.y)<=diameter*4) {
          fill(0, 116, 217);
      if (mousePressed == true) {
        position.x = mouseX;
        position.y = mouseY;
        draging = true;
        return true;
      }
    } else {
      fill(0);
    }
    draging = false;
    return false;
  }

  public PVector screenWrap(PVector pos) {
    // make sure objects always stay on screen by wraping the game space.
    // check if object is off screen to the right
    if (pos.x > gameW+radius)
    {
      pos.x = 0-radius;
    }
    // check if object is off screen to the left
    if (pos.x < 0-radius)
    {
      pos.x = gameW+radius;
    }
    // check if object is off screen to the bottom
    if (pos.y > gameH+radius)
    {
      pos.y = 0-radius;
    }
    // check if object is off screen to the top
    if (pos.y < 0-radius)
    {
      pos.y = gameH+radius;
    }
    return pos;
  }
}
public class Map {
  private ArrayList<PVector> MapPoints;
  int noPoints = 10;
  Map(int w, int h) {
    MapPoints = new ArrayList<PVector>();
    //MapPoints.add(random(w),random(h));
    for (int i = 0; i < noPoints; i++) {
      // make sure minimum distance from all point
      float angle = PI*2/noPoints;
      angle*=i;
      PVector point =  new PVector(sin(angle), cos(angle));
      point.mult((w*.4f)+random(-40,40));
      point.add(gameW/2,gameH/2); // place in center
      MapPoints.add(point);
    }
  }

  public void draw() {
    noFill();
    strokeWeight(50);
    stroke(255);
    beginShape();
    PVector point =  MapPoints.get(0);
    curveVertex(point.x, point.y);
    for (int i = 0; i < MapPoints.size(); i++) {
      point =  MapPoints.get(i);
      curveVertex(point.x, point.y);
    }
    point =  MapPoints.get(0);
    curveVertex(point.x, point.y);

    point =  MapPoints.get(1);
    curveVertex(point.x, point.y);

    endShape(CLOSE);
  }

}
/*

Charles Fried - 2017
ANN Tutorial
Part #2

NETWORK

This class is for the neural network, which is hard coded with three layers: input, hidden and output

*/

ArrayList connec = new ArrayList();
ArrayList conStr = new ArrayList();

class Network {

  Neuron [] input_layer;
  Neuron [] hidden_layer;
  Neuron [] output_layer;

  int w;
  int h;
  float wheelX = 0;
  float wheelY = 0;
  int bestIndex = 0;
  float steeringAverage =0;

  Network(int inputs, int hidden, int outputs) {

    input_layer = new Neuron [inputs];
    hidden_layer = new Neuron [hidden];
    output_layer = new Neuron [outputs];

    for (int i = 0; i < input_layer.length; i++) {
      PVector pos =  new PVector(networkW * 0.15f, i * ( networkH / 2));
      input_layer[i] = new Neuron(pos);
    }

    for (int j = 0; j < hidden_layer.length; j++) {
      PVector pos =  new PVector(networkW * 0.5f, j * ( networkH / 4));
      hidden_layer[j] = new Neuron(input_layer, pos);
    }

    for (int k = 0; k < output_layer.length; k++) {
      PVector pos =  new PVector(networkW * 0.85f, k * ( networkH / 2));
      output_layer[k] = new Neuron(hidden_layer,pos);

    }
    // steering wheel possition
    wheelX = networkW * 0.85f;
    wheelY = networkH / 2;
  }

  public void respond(float[] inputs) {

    for (int i = 0; i < input_layer.length; i++) {
      input_layer[i].output = inputs[i];
    }
    //now feed forward through the hidden layer
    for (int j = 0; j < hidden_layer.length; j++) {
      hidden_layer[j].respond();
    }
    for (int k = 0; k < output_layer.length; k++) {
      output_layer[k].respond();
    }
  }

  public void zeroAll() {
    for (int i = 0; i < hidden_layer.length; i++) {
      for (int j = 0; j < input_layer.length; j++) {
        hidden_layer[i].weights[j] = 0;
      }
    }
    for (int i = 0; i < output_layer.length; i++) {
      for (int j = 0; j < hidden_layer.length; j++) {
        output_layer[i].weights[j] = 0;
      }
    }
  }
  public void randomAll() {
    for (int i = 0; i < hidden_layer.length; i++) {
      for (int j = 0; j < input_layer.length; j++) {
        hidden_layer[i].weights[j] = random(-1.0f,1.0f);
      }
    }
    for (int i = 0; i < output_layer.length; i++) {
      for (int j = 0; j < hidden_layer.length; j++) {
        output_layer[i].weights[j] = random(-1.0f,1.0f);
      }
    }
  }
  // void respond(Card card) {
  //
  //   for (int i = 0; i < input_layer.length; i++) {
  //     input_layer[i].output = card.inputs[i];
  //   }
  //   // now feed forward through the hidden layer
  //   for (int j = 0; j < hidden_layer.length; j++) {
  //     hidden_layer[j].respond();
  //   }
  //   for (int k = 0; k < output_layer.length; k++) {
  //     output_layer[k].respond();
  //   }
  // }

  public void display() {
    pushMatrix();
    pushStyle();
    strokeWeight(1);
    translate(gameW,gameH/4);
    drawCon();
    // Draw the input layer
    for (int i = 0; i < input_layer.length; i++) {
      pushMatrix();
      translate(input_layer[i].x,  input_layer[i].y);
      input_layer[i].display();
      if (i%2 == 0) {
        text("Left Eye",-50, 5);
      } else {
        text("Right Eye",-50, 5);
      }
      popMatrix();
    }

    // Draw the hidden layer
    for (int j = 0; j < hidden_layer.length; j++) {
      pushMatrix();
      translate(hidden_layer[j].x,  hidden_layer[j].y);
      hidden_layer[j].display();
      popMatrix();
    }

    // Draw the output layer
    float [] resp = new float [output_layer.length];
    float respTotal = 0.0f;
    for (int k = 0; k < output_layer.length; k++) {
      resp[k] = output_layer[k].output;
      respTotal += resp[k]+1;
    }
    float steering = 0;

    for (int k = 0; k < output_layer.length; k++) {
      pushMatrix();

      translate(output_layer[k].x,  output_layer[k].y);
      //normalise output
      if (output_layer[k].output<0) {
        output_layer[k].output = 0;
      }
      output_layer[k].display();
      fill(255);

      String percent = nfc(((output_layer[k].output+1)/respTotal)*100, 1);
      //  float steering = ((output_layer[k].output+1)/respTotal)-.5;
      float steerTemp = output_layer[k].output;
      //  if (steerTemp>0) {

      //  }
      textAlign(CORNER);
      if (k%2 == 0) {
        //  text("Left: "+percent + "%",60, 5);
        steering -= steerTemp/10;
        text("Left",30, 5);
      } else {
        //  text("Righ  t: "+percent + "%",60, 5);
        steering += steerTemp/10;
        text("Right",30, 5);
      }
      textAlign(CENTER);
      popMatrix();
      // strokeWeight(1);
    }
    steering = steering/2;
    steeringAverage = steeringAverage*.6f;
    steeringAverage += steering*.4f;
    gameCar.steer(steeringAverage);

    //(gameCar.updateAngle();
    //
    float best = -1.0f;
    for (int i =0; i < resp.length; i++) {
      if (resp[i]>best) {
        best = resp[i];
        bestIndex = i;
      }
    }
    stroke(255, 0, 0);
    noFill();
    // indicate output
    //  ellipse(
    //    width * 0.85, (bestIndex%10) * height / 15.0 + height * 0.2,
    //  25, 25);

    popStyle();
    popMatrix();
    pushMatrix();
    //steering wheel
    fill(255);
    translate(gameW+wheelX+60,gameH/2);
    rotate(steeringAverage*20);
    ellipse(0,0,50,50);
    strokeWeight(2);
    stroke(0);
    line(0,0,25,0);
    popMatrix();
  }

  // void train(float [] outputs) {
  //   // adjust the output layer
  //   for (int k = 0; k < output_layer.length; k++) {
  //     output_layer[k].setError(outputs[k]);
  //     output_layer[k].train();
  //   }
  //   float best = -1.0;
  //   for (int i = 0; i < output_layer.length; i++) {
  //     if (output_layer[i].output > best) bestIndex = i;
  //   }
  //   // propagate back to the hidden layer
  //   for (int j = 0; j < hidden_layer.length; j++) {
  //     hidden_layer[j].train();
  //   }

  // The input layer doesn't learn: it is the input and only that
  //  }

  public void drawCon() {

    for (int i = 0; i < hidden_layer.length; i++) {
      for (int j = 0; j < input_layer.length; j++) {
        float weight = hidden_layer[i].weights[j];
        stroke(255);
        strokeWeight(pow(10, abs(weight))/10);
        line(input_layer[j].x,input_layer[j].y,hidden_layer[i].x,hidden_layer[i].y);
        //  PVector midPoint = new PVector((input_layer[j].x+hidden_layer[i].x)*.5,(input_layer[j].y+hidden_layer[i].y)*.5);
        PVector midPoint = new PVector((hidden_layer[i].x-input_layer[j].x),(hidden_layer[i].y-input_layer[j].y));
        midPoint.mult(.3f);
        midPoint.add(input_layer[j].x,input_layer[j].y);
        //ellipse(midPoint.x,midPoint.y,4,4);
        hidden_layer[i].weights[j] = weightDisplay(midPoint.x,midPoint.y,weight);
      }
    }

    for (int i = 0; i < output_layer.length; i++) {
      for (int j = 0; j < hidden_layer.length; j++) {
        float weight = output_layer[i].weights[j];
        stroke(255);
        strokeWeight(pow(10, abs(weight))/10);
        line(hidden_layer[j].x,hidden_layer[j].y,output_layer[i].x,output_layer[i].y);
        //  find location to place slider
        PVector midPoint = new PVector((output_layer[i].x-hidden_layer[j].x),(output_layer[i].y-hidden_layer[j].y));
        midPoint.mult(.6f);
        midPoint.add(hidden_layer[j].x,hidden_layer[j].y);
        // update weight
        output_layer[i].weights[j] = weightDisplay(midPoint.x,midPoint.y,weight);
      }
    }
    strokeWeight(1);
  }

  public float weightDisplay(float x, float y, float weight) {
    noStroke();
    float newWeight;
    int width = 55;
    int height = 15;
    y -= height/2;
    x -= width/2;
    pushMatrix();

    translate(x,y);
    x+= gameW;
    y+= gameH/4;
    fill(0,45,90);
    //&& mousePressed
    rect(0,0,55,15);
    if (mouseX > x && mouseX < x+width && mouseY > y && mouseY < y+height) {
      if (mousePressed) {
        weight = map(mouseX-x, 0,width, -1.0f,1.0f);
      }
      fill(0, 116, 217);
      float length = map(weight,-1.0f,1.0f, 0, 55);
      rect(0,0,length,15);
    }
    fill(255);
    //  rectMode(CORNER);
    text("w: "+nfc(weight,2),5,0,45,15);
    popMatrix();
    newWeight = weight;
    return newWeight;
  }

}
/*

Charles Fried - 2017
ANN Tutorial
Part #2

NEURON

This class is for the neural network, which is hard coded with three layers: input, hidden and output

*/

float LEARNING_RATE = 0.01f;


class Neuron {

  Neuron [] inputs; // Strores the neurons from the previous layer
  float [] weights;
  float output;
  float error;
  float x;
  float y;
  float diameter = 30;

  Neuron(PVector Pos) {
    error = 0.0f;
    this.x = Pos.x;
    this.y = Pos.y;
  }

  Neuron(Neuron [] p_inputs, PVector Pos) {
    this.x = Pos.x;
    this.y = Pos.y;
    inputs = new Neuron [p_inputs.length];
    weights = new float [p_inputs.length];
    error = 0.0f;
    for (int i = 0; i < inputs.length; i++) {
      inputs[i] = p_inputs[i];
      weights[i] = random(-1.0f, 1.0f);
    }

  }

  public void respond() {

    float input = 0.0f;
    for (int i = 0; i < inputs.length; i++) {
      input += inputs[i].output * weights[i];
    }
    output = lookupSigmoid(input);
    error = 0.0f;
  }

  public void setError(float desired) {
    error = desired - output;
  }

  public void train() {
    float delta =(1.0f - output) * (1.0f + output) *
    error * LEARNING_RATE;
    for (int i = 0; i < inputs.length; i++) {
      inputs[i].error += weights[i] * error;
      weights[i] += inputs[i].output * delta;
    }
  }

  public void display() {
    stroke(255);
    fill(255- (128 * (1 - output)));
    ellipse(0, 0, diameter, diameter);
    fill(255);
    textAlign(CENTER);
    text(nfc(output,1),-2,-diameter);
  }

  public float [] getStrength() {
    float ind = 0.0f;
    float str = 0.0f;
    for (int i = 0; i < weights.length; i++) {
      if (weights[i] > str) {
        ind = i;
        str = weights[i];
      }
    }
    float [] a = {ind, str};
    return a;
  }
}
/*

 Charles Fried - 2017
 ANN Tutorial
 Part #1

 SIGMOID
 Activation function

 A sigmoid function is the neuron's response to inputs the sigmoidal response ranges from -1.0 to 1.0
 For example, the weighted sum of inputs might be "2.1" our response would be lookupSigmoid(2.1) = 0.970
 This is a look up table for sigmoid (neural response) values which is valid from -5.0 to 5.0

 */

float [] g_sigmoid = new float [200];

public void setupSigmoid() {

  for (int i = 0; i < 200; i++) {
    float x = (i / 20.0f) - 5.0f;
    g_sigmoid[i] = 2.0f / (1.0f + exp(-2.0f * x)) - 1.0f;
  }
}

// once the sigmoid has been set up, this function accesses it:
public float lookupSigmoid(float x) {
  return g_sigmoid[constrain((int) floor((x + 5.0f) * 20.0f), 0, 199)];
}
  public void settings() {  size(1300,600); }
  static public void main(String[] passedArgs) {
    String[] appletArgs = new String[] { "--present", "--window-color=#666666", "--hide-stop", "CarLearning" };
    if (passedArgs != null) {
      PApplet.main(concat(appletArgs, passedArgs));
    } else {
      PApplet.main(appletArgs);
    }
  }
}
