//Parallel Bot by Aaron Steed

memory mem = new memory (100.0,100.0);
bot bOne = new bot(100.0,100.0,1.0,50.0,1.0,mem);

void setup() {
  size (500,300);
  background(240);
  fill(200,0,0);
  framerate(24);
}

void loop() {
req();
}//loop;

class memory{
  float [] xs=new float[1];
  float [] ys=new float[1];
  memory (float xtt,float ytt){
    xs[0]=xtt;
    ys[0]=xtt;
  }
  void extendx(float newx) {
    float[] tempx = new float[xs.length + 1];
    System.arraycopy(xs, 0, tempx, 0, xs.length);
    tempx[xs.length] = newx;
    xs = tempx;
  }
  void extendy(float newy) {
    float[] tempy = new float[ys.length + 1];
    System.arraycopy(ys, 0, tempy, 0, ys.length);
    tempy[ys.length] = newy;
    ys = tempy;
  }
}//memory;

class bot {
  float x; float y; float p=PI; float th; float nTh; float  L; int ind=1; int rad=12; memory mem;
  bot(float xt,float yt,float tht,float Lt,float nTht, memory memt){
    x=xt; y=yt; th=tht; L=Lt; nTh=nTht; mem=memt;
  }

void turn(){ th=nTh; }

  void traject(){
    if (r2()){
      ind=iwrap(ind+1,rad);
    } else {
      ind=iwrap(ind-1,rad);
    }
    nTh=(TWO_PI/rad)*ind;
  }//traject;

  void sniff(){
    float sniffx;
    float sniffy;
    float c=0.0;
    for (int i=0;i<L;i++){
      c=i+8;
      sniffx=fwrap ((x+(cos(th)*c)),width);
      sniffy=fwrap ((y+(sin(th)*c)),height);
      if(get(int(sniffx),int(sniffy))==#000000){
      if (r2()){break;}
        println("collide");
        nTh=getTh(sniffx,sniffy);

        break;
      }
    }//for;
    float cEnd=c-8;
    x=fwrap((x+(cos(th)*cEnd)),width);
    y=fwrap((y+(sin(th)*cEnd)),height);
  }//sniff;

  void dline(){
  background(240);
  for (int i = 1; i < mem.xs.length; i++){
    float x1=mem.xs[i-1];
    float y1=mem.ys[i-1];
    float x2=mem.xs[i];
    float y2=mem.ys[i];
    //skip lines that span the screen
    float checkL=sqrt(sq(abs(x2-x1))+sq(abs(y2-y1)));
  if (checkL<=L){
  line (x1,y1,x2,y2);
  }
  }
  float x1=mem.xs[mem.xs.length-1];
  float y1=mem.ys[mem.ys.length-1];
  float x2=mem.xs[mem.xs.length-2];
  float y2=mem.ys[mem.ys.length-2];
  float thet = atan2(x1-x2,y1-y2);
  triangle(x1,y1,x1-(cos(thet+(PI/4))*10),y1-(sin(thet+(PI/4))*10),x1-(cos(thet-(PI/4))*10),y1-(sin(thet-(PI/4))*10));
  }//dline;

  void stack(){
    mem.extendx(mem.xs[mem.xs.length-1]); // <--
    mem.extendy(mem.ys[mem.ys.length-1]); // <--
    mem.xs[mem.xs.length-1]=x;
    mem.ys[mem.ys.length-1]=y;
  }//stack;

}//bot;

int iwrap (int ivalu, int ilimi){
  int icar=ivalu;
if (icar>ilimi){ icar=(icar)%ilimi; }
  if (icar<0){
    icar=ilimi-icar;
    icar=(icar)%ilimi;
    icar=ilimi-icar;
  }
  return icar;
}//iwrap;

float fwrap (float fvalu, float flimi){
  float fcar=fvalu;
if (fcar>flimi){ fcar=(fcar)%flimi; }
  if (fcar<0){
    fcar=flimi-fcar;
    fcar=(fcar)%flimi;
    fcar=flimi-fcar;
  }
  return fcar;
}//fwrap;

boolean aprx(float v1,float v2,float er){
  if ((v1 < (v2+er)) && (v1 >(v2-er))){
    return true;
 } else { return false; }
 }//aprx;

 float getTh (float xp, float yp){
   float xm1; float ym1;
   float xm2; float ym2;
   float xm3=0.0; float ym3=0.0;
   for (int i=1;i<mem.xs.length;i++){
     xm1=mem.xs[i-1];
     ym1=mem.ys[i-1];
     xm2=mem.xs[i];
     ym2=mem.ys[i];
     if (overRect (xp,yp,xm1,ym1,xm2,ym2) && thTrue (xp,yp,xm1,ym1,xm2,ym2)){
       xm3=xm1-xm2;
       ym3=ym1-ym2;
     }
   }//for;
   return atan2(ym3,xm3);
 }//getTh;

 boolean thTrue (float xq, float yq, float x1, float y1, float x2, float y2){
   float xa=x2-x1;
   float ya=y2-y1;
   float xb=xq-x1;
   float yb=yq-y1;
   if (aprx(atan2(ya,xa),atan2(yb,xb),0.01)){
     return true;
   } else {
     return false;
   }
 }//thTrue;

 boolean overRect(float xo, float yo, float x1, float y1, float x2, float y2){
   if (xo >= x1 && xo <= x2 &&
   yo >= y1 && yo <= y2) {
     return true;
   } else {
     return false;
   }
 }

 void req(){
   bOne.traject();
   bOne.sniff();
   bOne.turn();
   bOne.stack();
   bOne.dline();
 }//req;

 boolean r2() {
if (int(random(2))==1){return true;}else{return false;}
 }//r2;