int SCREENWIDTH = 800;
int SCREENHEIGHT = 600;

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

float FRONTANGLE = -PI/4;
float LEFTANGLE = PI/4;

float VIEWANGLE = PI/2;

// robot
float rx = 0;
float ry = -140;
float ra = PI;

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

ArrayList walls = new ArrayList();

class Wall {
  float x1, y1, x2, y2;
  Wall(float _x1, float _y1, float _x2, float _y2) {
    x1 = _x1;
    y1 = _y1;
    x2 = _x2;
    y2 = _y2;
  }
}

float WALLRADIUS = 200;

void setup() {
  //size(SCREENWIDTH,SCREENHEIGHT);
  size(600,600);  // must be constant for export
  background(0);
  smooth();
  frameRate(15);
  /*
  for (float a=0; a<PI-0.001; a+=PI/8) {
    walls.add(new Wall(WALLRADIUS*cos(a)-WALLRADIUS, -WALLRADIUS*sin(a), WALLRADIUS*cos(a+PI/8)-WALLRADIUS, -WALLRADIUS*sin(a+PI/8)));
    walls.add(new Wall(WALLRADIUS*cos(a)+WALLRADIUS, WALLRADIUS*sin(a), WALLRADIUS*cos(a+PI/8)+WALLRADIUS, WALLRADIUS*sin(a+PI/8)));
  }
*/
  
  for (float a=PI/2; a<PI*3/2; a+=PI/8) {
    walls.add(new Wall(WALLRADIUS*cos(a), WALLRADIUS*sin(a), WALLRADIUS*cos(a+PI/8), WALLRADIUS*sin(a+PI/8)));
  }
  /*
  walls.add(new Wall(-350,-280,-250,280));
  walls.add(new Wall(-250,280,100,270));
  walls.add(new Wall(100,270,350,0));
  walls.add(new Wall(300,-200,-350,-280));
  walls.add(new Wall(300,-200,350,0));
  walls.add(new Wall(-317,-100,100,0));
  walls.add(new Wall(0,-25,-100,200));
  */
}

int sample = 0;

void draw() {
  background(0);

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

  float frontRange = getRange(FRONTANGLE);
  float leftRange = getRange(LEFTANGLE);

  if (sample++ > 2) {
    sample = 0;

    if (frontRange < ROBOTSIZE || leftRange < ROBOTSIZE*0.7) {
      ml = 1;
      mr = -1;
    } 
    else {
      float adj = f(leftRange-ROBOTSIZE);
      if (adj > 0.8) adj = 0.8;
      if (adj < -.8) adj = -.8;
      ml = 1 - adj;
      mr = 1 + adj;
    }

  }

  // 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);
  fill(255,0,0);
  arc(SCREENWIDTH/2+rx,SCREENHEIGHT/2-ry,frontRange*2,frontRange*2,-(ra+FRONTANGLE)-.03,-(ra+FRONTANGLE)+.03);
  arc(SCREENWIDTH/2+rx,SCREENHEIGHT/2-ry,leftRange*2,leftRange*2,-(ra+LEFTANGLE)-.03,-(ra+LEFTANGLE)+.03);

  //walls
  stroke(255);
  strokeWeight(3);
  for (int i=0; i<walls.size(); i++) {
    Wall wall = (Wall)walls.get(i);
    line(SCREENWIDTH/2+wall.x1, SCREENHEIGHT/2-wall.y1,
    SCREENWIDTH/2+wall.x2, SCREENHEIGHT/2-wall.y2);
  }

  //delay(100);
}

float getRange(float angle) {
  float minrange = SCREENWIDTH*10;
  for (int i=0; i<walls.size(); i++) {
    Wall wall = (Wall)walls.get(i);
    float m = (wall.y2-wall.y1)/(wall.x2-wall.x1);
    float b = wall.y1 - m*wall.x1;
    float range = (m*rx+b-ry)/(sin(ra+angle) - m*cos(ra+angle));
    float x = rx + range*cos(ra+angle);
    if (range < minrange && range > 0
      && ((x >= wall.x1 && x <= wall.x2)
      || (x >= wall.x2 && x <= wall.x1))) {
      minrange = range;
    }
  }
  return minrange;
}

float lasterr = 0;
float integral = 0;

float f(float err) {
  if (abs(err) < ROBOTSIZE/2)
    integral += err;

  float output = .02*err + 0.004*integral;

  lasterr = err;
  return output;
}