// Makes a simulation of a car in form of a arrow that 
// that makes automatically obstacles avoiding of 15 obstacle..
// The obstacle avoidance is made by a fuzzy logic controller..

// Click on the screen to set a target for the car and it will try
// to go there..


FAMM famm;
Mover car;
Target target;
Obstacle[] obstacles = new Obstacle[15];
Collision col;

PVector dir;
float fuzzyOutput;
float fuzzyOldOutput;
float smoothAngle;

PFont myFont;
PFont myFontSmall;

void setup(){
  
  size(1024, 600);
  smooth();
  frameRate(30);
  
  // Create famm
  famm = new FAMM(3);
  
  famm.m[0] = 90;
  famm.m[1] = 55;
  famm.m[2] = 35;
  famm.m[3] = 55;
  famm.m[4] = 35;
  famm.m[5] = 20;
  famm.m[6] = 35;
  famm.m[7] = 20;
  famm.m[8] = 5;
  
  FuzzySet fsDistance = new FuzzySet(0, 150);
  fsDistance.addMembership("small", -1.0, 0.0, 10.0, 25.0);
  //fsDistance.addMembership("small", -1.0, 12.0, 25.0, 40.0);
  fsDistance.addMembership("medium", 15.0, 30.0, 35.0, 45.0);
  //fsDistance.addMembership("medium", 30.0, 55.0, 65.0, 75.0);
  fsDistance.addMembership("large", 40.0, 50.0, 60.0, 70.0);
  //fsDistance.addMembership("large", 70.0, 90.0, 100.0, 150.0);
  
  FuzzySet fsAngle = new FuzzySet(0, 180);
  fsAngle.addMembership("small", -1.0, 0.0, 25.0, 30.0);
  //fsAngle.addMembership("small", -1.0, 0.0, 45.0, 60.0);
  fsAngle.addMembership("medium", 28.0, 35.0, 45.0, 55.0);
  //fsAngle.addMembership("medium", 55.0, 85.0, 95.0, 105.0);
  fsAngle.addMembership("large", 50.0, 65.0, 75.0, 120.0);
  //fsAngle.addMembership("large", 75.0, 110.0, 115.0, 180.0);
  
  Variable ivAngle = new Variable(fsAngle, "Angle");
  Variable ivDistance = new Variable(fsDistance, "Distance");
  
  famm.input[0] = ivAngle;
  famm.input[1] = ivDistance;
  
  myFont = createFont("Aharoni Fet", 14);
  //myFont = createFont("Aharoni Fet", 20);
  myFontSmall = createFont("Aharoni Fet", 14);

  dir = new PVector(0, 0);
  
  // Obsticales
  for(int i = 0; i<obstacles.length; i++){
    obstacles[i] = new Obstacle((int)random(20,width-20), (int)random(20,height-20), (int)random(20,width-20), (int)random(20,height-20));
    obstacles[i].display();
  }
  
  // Collision handler
  col = new Collision();
  
  // Car
  car = new Mover();
  car.display();

  // Target
  target = new Target();  
  
  // View data
  printInfoOnScreen();
  famm.displayInputs(30, height-260, 40, 200, 80);
  famm.displayFAMM(270, height-260, 210, 210);
}

void draw(){
  
  // Check if the car has reach the target
  if(car.checkIfCarAtTarget(target.location) && target.activated){
    background(200);
    
    // Check the direction to the target
    dir = PVector.sub(target.location, car.location);
    
    // clear collision handler
    col.clear();
    
    // Draw obsticales
    for(int i = 0; i<obstacles.length; i++){
      obstacles[i].update();
      obstacles[i].display();
      col.check(car.location, obstacles[i].location, dir);
    }
    
    // Make a new direction based of the fuzzy logic output
    famm.fuzzification(abs(col.angle), col.distance);
    fuzzyOutput = col.adjustAngle(famm.deffuzify(), car.location, dir);
    
    // Calculate a smooth new carAngle
    smoothAngle = SmoothCarHeading(fuzzyOutput, fuzzyOldOutput, 4, 5);
    fuzzyOldOutput = smoothAngle;
    
    // Calculate new direction from the smooth angle
    PVector newTarget = rotateVectorRoundPoint_2D(car.location, target.location, radians(smoothAngle));
    dir = PVector.sub(newTarget, car.location);
    
    // Draw nearest obstical
    fill(0, map(fuzzyOutput, 0, 90, 40, 220));
    stroke(0);
    arc(col.activeObstical.x, col.activeObstical.y, 20, 20, 0, TWO_PI); 
    
    // Draw target
    target.drawTarget();
   
    // Update position 
    car.update(dir);
    car.checkEdges();
    car.display();
    
    // View data
    printInfoOnScreen();
    famm.displayInputs(30, height-260, 40, 200, 80);
    famm.displayFAMM(270, height-260, 210, 210);
  }
}

// Limits the car of how much it can turn for each frame
float SmoothCarHeading(float newAngle, float oldAngle, int smallChange, int largeChange){
  float angle = oldAngle;
  
  if( newAngle - oldAngle > smallChange){
    if(newAngle > 60) { angle = oldAngle + largeChange; } else { angle = oldAngle + smallChange; }
  }else if( newAngle - oldAngle < -smallChange){
    if(newAngle < -60) { angle = oldAngle - largeChange; } else { angle = oldAngle - smallChange; }
  } 
  return angle;
}

// Shows information on the screen about the closest obstacle
void printInfoOnScreen(){
  
  // Draw lines from car
  stroke(170);
  line(car.location.x, car.location.y, target.location.x, target.location.y);
  stroke(180);
  line(car.location.x, car.location.y, col.activeObstical.x, col.activeObstical.y);
  stroke(100);
  
  float dir_mag = abs(map(col.angle, 0, 90, dir.mag(), 30));
  
  dir.normalize();
  dir.mult(dir_mag);
  
  //Get the end position of the vector
  PVector newTarget = PVector.add(car.location, dir);
  line(car.location.x, car.location.y, newTarget.x, newTarget.y);
  
  // Text info
  textFont(myFont);
  textAlign(LEFT);
  
  // Draw upper info
  fill(0,70);
  noStroke();
  arc(20,10,10,10,PI,TWO_PI-(PI/2)); 
  arc(20,30,10,10,(PI/2),PI);
  arc(width-20,10,10,10,TWO_PI-(PI/2),TWO_PI); 
  arc(width-20,30,10,10,0,(PI/2));
  rect(20, 10, width-40, 20);
  rect(15, 10, 5, 20);
  rect(20, 30, width-40, 5);
  rect(20, 5, width-40, 5);
  rect(width-20, 10, 5, 20);
  
  fill(255,150);
  text("Obstacle angle: " + round(col.angle) + char(176), 25, 25);
  text("Dist: " + round(col.distance), 165, 25);
  
  // Draw lower info
  fill(0,70);
  noStroke();
  arc(20, height-270 ,10, 10, PI, TWO_PI-(PI/2)); 
  arc(490, height-270, 10, 10, TWO_PI-(PI/2), TWO_PI); 
  arc(20, height-10, 10, 10, (PI/2), PI);
  arc(490, height-10 ,10, 10, 0, (PI/2));
  
  rect(20, height-270, 470, 260);
  rect(20, height-275, 470, 5);
  rect(15, height-270, 5, 260);
  rect(490, height-270, 5, 260);
  rect(20, height-10, 470, 5);
  
  
  fill(255,150);
  text("Fuzzy Output angle: " + round(fuzzyOutput) + char(176), 25, height - 17);
  text("Smooth Output angle: " + round(smoothAngle) + char(176), 195, height - 17);
}

// Rotates a Vector around a point
PVector rotateVectorRoundPoint_2D(PVector point_1, PVector point_2, float angle){
  float x, y;
  
  x = point_1.x + ((point_2.x - point_1.x) * cos(angle)) - ((point_2.y - point_1.y) * sin(angle)); 
  y = point_1.y + ((point_2.x - point_1.x) * sin(angle)) + ((point_2.y - point_1.y) * cos(angle));      
  
  return new PVector(x, y);
}

void mousePressed() {
  target.setTarget(mouseX,mouseY);
}