int SCREENWIDTH = 640;
int SCREENHEIGHT = 480;

int ROBOTSIZE = 70;
int BALLSIZE = 20;
int SPEED = 4;

float VIEWANGLE = PI/2;


// robot
float rx = 0;
float ry = -100;
float ra = PI/2;

// motor
float ml = 1;
float mr = 1;

//ball
float bx = 100;
float by = 200;

void setup() {
  //size(SCREENWIDTH,SCREENHEIGHT);
  size(640,480);  // must be constant for export
  background(0);
  smooth();
  frameRate(15);
}

int sample = 0;

void draw() {
  background(0);

  bx = mouseX - SCREENWIDTH/2;
  by = SCREENHEIGHT/2 - mouseY;

  rx += SPEED*(ml+mr)/2*cos(ra);
  ry += SPEED*(ml+mr)/2*sin(ra);
  ra += atan2(SPEED*(mr-ml),ROBOTSIZE);

  // ball angle
  float ba = atan2(by-ry,bx-rx) - ra;
  while (ba > PI) ba -= 2*PI;
  while (ba < -PI) ba += 2*PI;


  if (sample++ > 2) {
    sample = 0;
    if (abs(ba) > VIEWANGLE/2) {
      ml = -1;
      mr = 1;
    } 
    else {
      float adj = 0;
      ml = 1 - adj;              // uneven!
      mr = 1 + adj;
    }
  }

  if (dist(rx,ry,bx,by) < ROBOTSIZE/2) {
    //bx = random(-SCREENWIDTH/2,SCREENWIDTH/2);
    //by = random(-SCREENHEIGHT/2,SCREENHEIGHT/2);
    ml = 0;
    mr = 0;
  }

  // ball
  fill(255,0,0);
  ellipse(SCREENWIDTH/2+bx,SCREENHEIGHT/2-by,BALLSIZE,BALLSIZE);

  // robot
  stroke(0);
  fill(0,0,255);
  ellipse(SCREENWIDTH/2+rx,SCREENHEIGHT/2-ry,ROBOTSIZE,ROBOTSIZE);
  fill(0,255,0);
  arc(SCREENWIDTH/2+rx,SCREENHEIGHT/2-ry,ROBOTSIZE,ROBOTSIZE,-ra-VIEWANGLE/2,-ra+VIEWANGLE/2);

  //delay(100);
}

float lasterr = 0;
float integral = 0;

float f(float err) {
  integral += err;

  float output = 2*err + .2*integral;

  lasterr = err;
  return output;
}