public class Coord {
  float px;
  float py;
  int plx; 
  int ply;
  int estado;
  public float rt=0, gt=0, bt=0, r=0, g=0, b=0;
  float altura2 = 28;

  public Coord(float px, float py, int plx, int ply) {
    this.px = px;
    this.py = py;
    this.plx = plx;
    this.ply = ply;
    estado = 0;
  }

  public void setEstado(int estado) {
    this.estado = estado;
  }

  public boolean visitable() {
    return (estado >= 0 );
  }

  public void setColor() {

    if (estado==-2) {
      rt = 0;
      gt = 255;
      bt = 0;
    }
    else if (estado==-3){
      rt = 255;
      gt = 0;
      bt = 0;
    }
    else {
      rt = 0;
      gt = 0;
      bt = 250;
    }



    float ease = .05;
    r+=(rt-r)*ease;
    g+=(gt-g)*ease;
    b+=(bt-b)*ease;
    fill(r, g, b);
  }


  public void dibujar() {


    float altura = 0;
    if (estado>=-2) {
      altura = b/255*25;
      marca();
    } 
    else   if (estado==-3) {
      altura = altura2;

    } 
    setColor();
    pushMatrix();
    translate(px,  py);
    //stroke(.5);
    noStroke();
    box(35, 35, altura);
    popMatrix();
  }

  public void marca() {
    pushMatrix();
    translate(px,  py, 1);
    fill(255);
    rect(0, 0, 10, 10);
    popMatrix();
  }
}

//import processing.opengl.*;

Robot r2d2;
float rotacionT = .3;
float rotacion  = 0;
float px, py;

void setup() {
  size (650, 650, P3D);
  frameRate(200);
  rectMode(CENTER);
  noStroke();
  lights();
  r2d2 = new Robot();
}


void draw() {
  camera(width/2.0 + px/2, height/2.0 + py/2, (height/2.0) / tan(PI*60.0 / 360.0), width/2.0 + px, height/2.0 + py, 0, 0, 1, 0);
  
  
  px += (r2d2.px - px)*.02;
  py += (r2d2.py - py)*.02;
  if (rotacion<rotacionT-.01) {
    rotacion += (rotacionT-rotacion)*.006;
  }

  background(0);
  translate(width/2, height/2-25, 210);
  rotateX(rotacion);
  pointLight(255, 255, 255, px, py, 50);
  //spotLight(1, 1, 1, px, py, 0, 0, 0, -1, 0, 100);
  r2d2.m1.draw();
  r2d2.draw();
  r2d2.pensar();
}

public class Mundo {
  Coord[][] grid;
  int size;
  public float lado = 35;

  public Mundo(int size) {
    this.size = size;
    grid = new Coord[size][size];
    //ellipseMode(CORNER);
    for (int x = 0; x<grid.length; x++) {
      for (int y=0; y<grid[x].length; y++) {
        grid[x][y] = new Coord( (x- grid.length/2)*lado, (y - grid.length/2)*lado, x, y);
        grid[x][y].setEstado(1);
        if (random(0, 100)>75) {
          grid[x][y].setEstado(-3);
        }
      }
    }

  } 

  public void draw() {
    for (int i=0; i<grid.length; i++) {
      for (int j=0; j<grid.length; j++) {
        grid[i][j].dibujar();
      }
    }
   
  }

  public Coord getOrigen() {
    grid[0][7].setEstado(2);
    return grid[0][7];
  }

  public float getLado() {
    return lado;
  }

  public Coord getDerecha(int cx, int cy) {
    if (cx<size-1) {
      return  grid[cx+1][cy];
    }
    else return null;
  }

  public Coord getIzquierda(int cx, int cy) {
    if (cx>0) {
      return  grid[cx-1][cy];
    }
    else return null;
  }

  public Coord getAbajo(int cx, int cy) {
    if (cy<size-1) {
      return  grid[cx][cy+1];
    }
    else return null;
  }

  public Coord getArriba(int cx, int cy) {
    if (cy>0) {
      return  grid[cx][cy-1];
    }
    else return null;
  }



}

public class Robot{
  Mundo m1 = new Mundo(15);
  Coord raiz;
  Coord actual;
  Coord target;
  Stack anteriores;

  float rotacion = HALF_PI;
  int estado = 0;
  float px, py;
  float diametro = m1.getLado();
  float radio = m1.getLado()/2;


  public Robot() {
    raiz = m1.getOrigen();
    target = null;
    px = raiz.px;
    py = raiz.py;
    actual = raiz;
    anteriores = new Stack();
  }

  public void draw() {
    //Llantas
    pushMatrix();
    translate(px, py, 0);
    rotateZ(rotacion);
    fill(255, 255, 0);
    scale(1, .6, 1);
    box(m1.lado);
    
    fill(0, 100, 0);
    scale(1, 1/.6, 1);
    translate(8, 0, 0);
    scale(.3, .8, .9);
    box(m1.lado);
    translate(-60, 0, 0);
    box(m1.lado);
    
    popMatrix();
 
  }

  public void pensar() {
    if (estado==0) {

      if (sensorDerecho()!=null && sensorDerecho().visitable()) {
        target = sensorDerecho();
        actual.setEstado(-2);
        estado = 1;
        anteriores.push(actual);
        rotacion = 0;
      }
      else  if (sensorAbajo()!=null && sensorAbajo().visitable()) {
        target = sensorAbajo();
        actual.setEstado(-2);
        estado = 1;
        anteriores.push(actual);
        rotacion = HALF_PI;
      }
      else  if (sensorIzquierda()!=null && sensorIzquierda().visitable()) {
        target = sensorIzquierda();
        actual.setEstado(-2);
        estado = 1;
        anteriores.push(actual);
        rotacion = PI;
      }
      else  if (sensorArriba()!=null && sensorArriba().visitable()) {
        target = sensorArriba();
        actual.setEstado(-2);
        estado = 1;
        anteriores.push(actual);
        rotacion = HALF_PI + PI;
      }
      else {
        if (anteriores.size()>0){
          target = (Coord) anteriores.pop();
          if (target.plx == actual.plx && target.ply>actual.ply) {
            rotacion = HALF_PI;
          }
          else if (target.plx == actual.plx && target.ply<actual.ply) {
            rotacion = HALF_PI+PI;
          }
          else if (target.ply == actual.ply && target.plx<actual.plx) {
            rotacion = PI;
          }
          else if (target.ply == actual.ply && target.plx>actual.plx) {
            rotacion = 0;
          }
          actual.setEstado(-2);
          estado = 1;
        }
      }

    }

    if (estado==1) {
      float dx = target.px - px;
      float dy = target.py - py;
      float dis = sqrt(dx*dx + dy*dy);

      px += dx*.25;
      py += dy*.25;


      if (dis<=.1) {
        actual = target;
        estado = 0;
        target = null;
      }

    }
  }


  public Coord sensorDerecho() {
    return m1.getDerecha(actual.plx, actual.ply);
  } 
  public Coord sensorIzquierda() {
    return m1.getIzquierda(actual.plx, actual.ply);
  } 
  public Coord sensorArriba() {
    return m1.getArriba(actual.plx, actual.ply);
  } 
  public Coord sensorAbajo() {
    return m1.getAbajo(actual.plx, actual.ply);
  } 


}