class Robot{
  float x,y,rot,radius=10,diameter=20;
  float driveLeft, driveRight;
  Trail trail;

  float maxDrive = 1;
  float turnSpeed = 0.02;
  float driveSpeed = 0.005;
  float minAngleToTarget = 0.02;
  //--------------
  void ai(){
    float angleToBall = measureAngleToBall();
    float angleToGoal = measureAngleToGoal();

    if( abs(angleToGoal-angleToBall) < 5*minAngleToTarget ){
      turnToAngle( angleToGoal );
      if( abs(angleToGoal) < minAngleToTarget )
        driveForward( 1 );
    }
    else if( angleToGoal-angleToBall > 5*minAngleToTarget ){
      if( abs(angleToBall-0.45*PI) < minAngleToTarget ){
        driveLeft(0.8);
        driveRight(0.5);
      }
      else
        turnToAngle( angleToBall-0.45*PI );
    }
    else if( angleToGoal-angleToBall < 5*minAngleToTarget ){
      if( abs(angleToBall+0.45*PI) < minAngleToTarget ){
        driveLeft(0.5);
        driveRight(0.8);
      }
      else
        turnToAngle( angleToBall+0.45*PI );
    }
  }
  //--------------
  void setPos(float x, float y){
    this.x = x;
    this.y = y;
    trail = new Trail(x,y,1000,10,color(255,128));
  }
  //--------------
  // en svart låda som på något sätt mäter vinkeln i robotens lokala referensram
  float measureAngleToGoal(){
    return measureAngle(goal.y,goal.x);
  }
  float measureAngleToBall(){
    return measureAngle(ball.y,ball.x);
  }
  float measureAngle(float ty, float tx){
    return atan2(ty-y,tx-x)-rot;
  }
  //--------------
  float angularDistance(float a, float b){
    if(a<0) a+=TWO_PI;
    if(b<0) b+=TWO_PI;
    float diff = a-b;
    return diff;
  }
  //--------------
  void update(){
    trail.update(x,y);
    rot -= tan( (driveRight-driveLeft)/diameter );
    x += cos(rot)*(driveRight+driveLeft)/2;
    y += sin(rot)*(driveRight+driveLeft)/2;
    driveRight *= 0.95;
    driveLeft *= 0.95;
  }
  //--------------
  void turnToAngle(float angle){
    driveLeft( turnSpeed*angle );
    driveRight( -turnSpeed*angle );
  }
  //--------------
  void driveForward(float d){
    driveLeft(d);
    driveRight(d);
  }
  //--------------
  void driveLeft(float d){
    driveLeft += d;
    if(driveLeft>maxDrive) driveLeft = maxDrive;
    if(driveLeft<-maxDrive) driveLeft = -maxDrive;
  }
  //--------------
  void driveRight(float d){
    driveRight += d;
    if(driveRight>maxDrive) driveRight = maxDrive;
    if(driveRight<-maxDrive) driveRight = -maxDrive;
  }
  //--------------
  void visualize(){
    trail.visualize();
    
    fill(0,255,0);
    stroke(0);
    strokeWeight(2);
    pushMatrix();
    translate(x,y);
    ellipse(0,0,diameter,diameter);
    rotate(rot);
    strokeWeight(1);

    pushMatrix();
    stroke(255,255,0,64);
    //rotate(minAngleToTarget);
    line(radius,0,width,0);
    //rotate(-2*minAngleToTarget);
    line(radius,0,width,0);
    popMatrix();

    noStroke();
    fill(0);
    triangle(radius-2,0,radius-8,-2,radius-8,2);
    popMatrix();
  }
}