int[] sensor=new int[51];
boolean dragp,dragn,dragv,dragf,drags,drag,dragy,playout,dragangle;
float xpos,xposneu;
float ypos,yposneu;
float nnoise,snu,sme,sed,pslide,nslide,vslide,fslide,factor,sslide,drive,drivem,steer,steerold,speed,angle,newangle,sxl,sxr,syl,syr,hx,hy,velocity;
int sxm,sym,x,y,xx,yy,playbu,s,sensormenge,sensornummer,senlow,senval;

PImage b; 

void setup()
{
  PFont fontA = loadFont("CourierNew36.vlw");
  textFont(fontA, 12);
  drive=0;
  playbu=1;
  sslide=0;
  vslide=20;
  drivem=10;
  nslide=-20;
  sensormenge=10;
  fslide=0;
  pslide=0;
  b = loadImage("track1.jpg"); 
  size(512,384,P3D);

  colorMode(RGB, 255);
  xpos=256;
  ypos=200;
  ellipseMode(CENTER);

  background(255);
}



void draw()
{

  // refresh
  noStroke();
  fill(255);
  rect(0,0,512,318);
  rect(0,318,280,100);

  //track
  image(b, 0, 0); 

  //shift scan to right
  if (playbu==1){
    loadPixels();
    for (x=510;x>=280;x--){
      for (y=318;y<=370;y++){
        pixels[y*512+x+1]=pixels[y*512+x];
      }
    }
    stroke(0);
  }



  sxl=(cos(-angle)*-10)+(sin(-angle)*(-25-pslide));    //Sensor left end
  syl=(-sin(-angle)*-10)+(cos(-angle)*(-25-pslide));

  sxr=(cos(-angle)*10)+(sin(-angle)*(-25-pslide));    //Sensor right end
  syr=(-sin(-angle)*10)+(cos(-angle)*(-25-pslide));

  for (sensornummer=0; sensornummer<=sensormenge; sensornummer++){
    sxm=int((((sxr+xpos)-(sxl+xpos))/sensormenge*(sensormenge-sensornummer))+(sxl+xpos));
    sym=int((((syr+ypos)-(syl+ypos))/sensormenge*(sensormenge-sensornummer))+(syl+ypos));
    s=int(brightness(get(sxm,sym)));
    s=int((s*(50-nnoise)+random(255)*(nnoise))/50);
    sensor[sensornummer]=s;
    stroke (s);
    sme=sensormenge;
    sed=(45/(sme+1));
    line (280,319+sed*sensornummer-1,280,319+sed*(sensornummer+1)-1);
    stroke(0);
  }


  //calculate steering value
  //get the lowest value
  senval=sensor[0];
  senlow=0;
  for (sensornummer=1; sensornummer<=sensormenge; sensornummer++){
    if (sensor[sensornummer]<senval){
      senval=sensor[sensornummer];
      senlow=sensornummer;
    }
  }
  stroke(0,255,0);
  sme=sensormenge;
  sed=((45/(sme+1))*senlow);
  point(280,319+sed);            // green line
  stroke(0);
  steer=(senlow-sensormenge/2)*(-factor);           


  //drawing environment
  //play-button

  if (playbu==1){  
    fill(0,255,0);
  }
  else{
    fill(255,0,0);
  }

  if (mouseX>11 && mouseX<28 && mouseY>319 && mouseY<336 && drag==false && dragy==false && drags==false && dragf==false && dragv==false && dragn==false && dragp==false && dragangle==false){
    fill(0,0,255);
    if (mousePressed && playout){
      playout=false;
      playbu=playbu*-1;
    }
  }
  if (mousePressed==false){
    playout=true;
  }
  rect(12,320,15,15);
  fill(0);
  text("p",17,330);
  fill(255);

  //reset
  if (mouseX>11 && mouseX<28 && mouseY>345 && mouseY<362 && drag==false && dragy==false && drags==false && dragf==false && dragv==false && dragn==false && dragp==false && dragangle==false){
    fill(0,0,255);
    if (mousePressed){
      drive=0;
      playbu=1;
      sslide=0;
      vslide=20;
      drivem=10;
      nslide=-20;
      sensormenge=10;
      fslide=0;
      pslide=0;
    }
  }
  rect(12,346,15,15);
  fill(0);
  text("r",16,357);
  fill(255);






  //speed

  line (40,320,90,320); 

  if ((mouseX<=70+drivem) && (mouseX>=60+drivem) && (mouseY<=325) && (mouseY>=315 && drag==false && drags==false && dragf==false && dragv==false && dragn==false && dragp==false && dragangle==false)){
    fill (255,0,0);
    if (mousePressed){
      dragy=true;
    }
  }   
  if (mousePressed==false){
    dragy=false;
  }	
  if (dragy){
    fill (255,0,0);
    drivem=mouseX-65;
    drivem=constrain (drivem,-25,25);
  }
  rect (60+drivem,315,10,10);
  fill (255);
  drive=-drivem;
  fill(0);
  text("drv "+int(drivem),99,323);
  fill(255);

  //amount:sensors

  line (40,340,90,340); 
  if ((mouseX<=70+sslide) && (mouseX>=60+sslide) && (mouseY<=345) && (mouseY>=335) && drag==false && dragy==false && dragf==false && dragv==false && dragn==false && dragp==false&& dragangle==false){
    fill (255,0,0);
    if (mousePressed){
      drags=true;
    }
  }   
  if (mousePressed==false){
    drags=false;
  }	
  if (drags){
    fill (255,0,0);
    sslide=mouseX-65;
    sslide=constrain (sslide,-25,25);
  }
  rect (60+sslide,335,10,10);
  fill (255);
  sensormenge=int((sslide+25)/50*17+2);
  fill(0);
  text("sen "+int(sensormenge+1),99,343);
  fill(255);

  //steerfactor

  line (40,360,90,360); 
  if ((mouseX<=70+fslide) && (mouseX>=60+fslide) && (mouseY<=365) && (mouseY>=355) && drag==false && dragy==false && drags==false && dragv==false && dragn==false && dragp==false&& dragangle==false){
    fill (255,0,0);
    if (mousePressed){
      dragf=true;
    }
  }   
  if (mousePressed==false){
    dragf=false;
  }	
  if (dragf){
    fill (255,0,0);
    fslide=mouseX-65;
    fslide=constrain (fslide,-25,25);
  }
  rect (60+fslide,355,10,10);
  fill (255);
  factor=int((fslide+25)/50*32+3);
  fill(0);
  text("fac "+int(factor),99,363);
  fill(255);

  //velocity

  line (160,320,210,320); 
  if ((mouseX<=190+vslide) && (mouseX>=180+vslide) && (mouseY<=325) && (mouseY>=315) && drag==false && dragy==false && drags==false && dragf==false && dragn==false && dragp==false&& dragangle==false){
    fill (255,0,0);
    if (mousePressed){
      dragv=true;
    }
  }   
  if (mousePressed==false){
    dragv=false;
  }	
  if (dragv){
    fill (255,0,0);
    vslide=mouseX-185;
    vslide=constrain (vslide,-25,25);
  }
  velocity=vslide+25;
  rect (180+vslide,315,10,10);
  fill (255);
  fill(0);
  text("vel "+int(velocity),219,323);
  fill(255);
  steer=(steer*(50-velocity)+steerold*(velocity))/50;
  steerold=steer;

  //noise

  line (160,340,210,340); 
  if ((mouseX<=190+nslide) && (mouseX>=180+nslide) && (mouseY<=345) && (mouseY>=335) && drag==false && dragy==false && drags==false && dragf==false && dragv==false && dragp==false&& dragangle==false){
    fill (255,0,0);
    if (mousePressed){
      dragn=true;
    }
  }   
  if (mousePressed==false){
    dragn=false;
  }	
  if (dragn){
    fill (255,0,0);
    nslide=mouseX-185;
    nslide=constrain (nslide,-25,25);
  }
  rect (180+nslide,335,10,10);
  fill (255);
  fill(0);
  nnoise=nslide+25;
  text("noi "+int(nnoise),219,343);
  fill(255);

  //position

  line (160,360,210,360); 
  if ((mouseX<=190+pslide) && (mouseX>=180+pslide) && (mouseY<=365) && (mouseY>=355) && drag==false && dragy==false && drags==false && dragf==false && dragv==false && dragn==false&& dragangle==false){
    fill (255,0,0);
    if (mousePressed){
      dragp=true;
    }
  }   
  if (mousePressed==false){
    dragp=false;
  }	
  if (dragp){
    fill (255,0,0);
    pslide=mouseX-185;
    pslide=constrain (pslide,-25,25);
  }
  rect (180+pslide,355,10,10);
  fill (255);
  fill(0);
  text("pos "+int(pslide+25),219,363);
  fill(255);

  //calculate wheelchair position  
  if (playbu==1){
    speed=(drive)/30;
    angle=angle+(steer)/300;
    xposneu=sin(-angle)*speed;
    yposneu=cos(-angle)*speed;
    xpos=xpos+xposneu;
    ypos=ypos+yposneu;
  }

  //drawing wheelchair
  hx=(sin(-angle)*-40);
  hy=(cos(-angle)*-40);
  if (playbu==-1){
    if (dist(xpos+hx,ypos+hy,mouseX,mouseY)<8 && drag==false && dragy==false && drags==false && dragf==false && dragv==false && dragn==false && dragp==false){
      fill(0,0,255);
      if (mousePressed && drag==false){
        dragangle=true;
      }
    }
    if (mousePressed==false){
      dragangle=false;
    }
    if (dragangle){
      x=int(mouseX-xpos);
      y=int(mouseY-ypos);
      angle=atan2(y,x)+1.6;

    }  
    line(xpos,ypos,xpos+hx,ypos+hy);
    ellipse(xpos+hx,ypos+hy,15,15);
    fill(255);
  }

  noFill();
  if (dist(xpos, ypos, mouseX, mouseY)<30 && dragangle==false && drag==false && dragy==false && drags==false && dragf==false && dragv==false && dragn==false && dragp==false) {
    fill (0,0,255);
    if (mousePressed == true){
      drag=true;
    }
  }
  if (mousePressed==false){
    drag=false;
  } 
  if (drag){
    playbu=-1;
    xpos=mouseX;
    ypos=mouseY;
  }
  xpos=constrain(xpos,40,472);
  ypos=constrain(ypos,40,270);
  ellipse (xpos,ypos,60,60);
  fill(255);


  strokeWeight(1);
  translate(xpos, ypos); 
  rotateZ(angle); 
  stroke(255,0,0); 
  stroke(0); 
  rect(-15, -20, 30, 30); 
  rect(-12, -22, 24, 30); 
  stroke(255,0,0);
  //line (-15,-25,15,-25);
  fill (255);
  point(0,0);

  // draw sensors
  stroke(255,0,0);
  snu=sensornummer;
  sme=sensormenge;
  for (snu=0; snu<=sme; snu++){
    sed=(20/sme)*snu;
    line(-10+sed,-27-pslide,-10+sed,-24-pslide);
  }
  stroke(0);

}