// Custom Cube Class

class Cube{
  PVector[] vertices = new PVector[24];
  float w, h, d;
  PVector pos;
  PVector oldPos;
  ArrayList trace = new ArrayList();
  boolean isInside;
  boolean isKilled;

  // Default constructor
  Cube(){ 
  }

  // Constructor 2
  Cube(float w, float h, float d, PVector pos) {
    this.w = w;
    this.h = h;
    this.d = d;
    this.pos = pos;
    this.isInside = false;
    this.isKilled = false;

    // cube composed of 6 quads
    //front
    vertices[0] = new PVector(-w/2,-h/2,d/2);
    vertices[1] = new PVector(w/2,-h/2,d/2);
    vertices[2] = new PVector(w/2,h/2,d/2);
    vertices[3] = new PVector(-w/2,h/2,d/2);
    //left
    vertices[4] = new PVector(-w/2,-h/2,d/2);
    vertices[5] = new PVector(-w/2,-h/2,-d/2);
    vertices[6] = new PVector(-w/2,h/2,-d/2);
    vertices[7] = new PVector(-w/2,h/2,d/2);
    //right
    vertices[8] = new PVector(w/2,-h/2,d/2);
    vertices[9] = new PVector(w/2,-h/2,-d/2);
    vertices[10] = new PVector(w/2,h/2,-d/2);
    vertices[11] = new PVector(w/2,h/2,d/2);
    //back
    vertices[12] = new PVector(-w/2,-h/2,-d/2); 
    vertices[13] = new PVector(w/2,-h/2,-d/2);
    vertices[14] = new PVector(w/2,h/2,-d/2);
    vertices[15] = new PVector(-w/2,h/2,-d/2);
    //top
    vertices[16] = new PVector(-w/2,-h/2,d/2);
    vertices[17] = new PVector(-w/2,-h/2,-d/2);
    vertices[18] = new PVector(w/2,-h/2,-d/2);
    vertices[19] = new PVector(w/2,-h/2,d/2);
    //bottom
    vertices[20] = new PVector(-w/2,h/2,d/2);
    vertices[21] = new PVector(-w/2,h/2,-d/2);
    vertices[22] = new PVector(w/2,h/2,-d/2);
    vertices[23] = new PVector(w/2,h/2,d/2);
  }

  void Draw(boolean drawTrace){
    if(!isKilled)
    {
      // Draw cube
      pushMatrix();
      translate(pos.x,pos.y,pos.z);
      for (int i=0; i<6; i++){
        if(isInside) fill(255,0,0); 
        else fill(0,255,0);
        beginShape(QUADS);
        for (int j=0; j<4; j++){
          vertex(vertices[j+4*i].x, vertices[j+4*i].y, vertices[j+4*i].z);
        }
        endShape();
      }
      popMatrix();
    }
    if(drawTrace)
    {
      strokeWeight(1.5);
      noFill();
      beginShape();
      for(int i=0; i<trace.size()-1;i++)
      {
        stroke(200,i,75);
        PVector temp = (PVector)trace.get(i);
        vertex(temp.x,temp.y,temp.z);
      }
      endShape();
      strokeWeight(0.5);
    }
  }

  void Create(color[]quadBG){
    // Draw cube
    for (int i=0; i<6; i++){
      fill(quadBG[i]);
      beginShape(QUADS);
      for (int j=0; j<4; j++){
        vertex(vertices[j+4*i].x, vertices[j+4*i].y, vertices[j+4*i].z);
      }
      endShape();
    }
  }

  void Eval(PVector[][] vecStart, PVector[][] vecEnd)
  {
    if(!isKilled)
    {
      PVector newPos = new PVector(pos.x,pos.y,pos.z);
      //loop through all the vectors
      for(int i = 0; i < imgX; i+=2)
      {
        for(int j = 0; j < imgY; j+=2)
        {
          //evaluate distance from cube to vect
          float d = sqrt(abs(vecStart[j][i].x-pos.x)+
            abs(vecStart[j][i].y-pos.y)+
            abs(vecStart[j][i].z-pos.z));

          if (d<4)
          {
            newPos.x =newPos.x + (4-d)*(vecEnd[j][i].x)/50;
            newPos.y =newPos.y + (4-d)*(vecEnd[j][i].y)/50;
            newPos.z =newPos.z + (4-d)*(vecEnd[j][i].z)/50;
            stroke(0,0,255,30);
            line(pos.x,pos.y,pos.z,vecStart[j][i].x,vecStart[j][i].y,vecStart[j][i].z);
          }        
        }
      }

      oldPos = pos;

      if(((newPos.x-pos.x)<EPS) && ((newPos.y-pos.y)<EPS) && ((newPos.z-pos.z)<EPS)) 
      {
        pos.x+=0.3;
        //println(pos.x);
      }
      else
      { 
        pos = new PVector(newPos.x,newPos.y,newPos.z);
        pos.x+=0.3;
        //println(pos.x);
      }

      this.isInside = WithinBounds(pos);
      //if within bounds store the trace
      if((abs(oldPos.x-newPos.x)>0.1 || abs(oldPos.y-newPos.y)>0.1 || abs(oldPos.z-newPos.z) > 0.1) && isInside)
      {
        trace.add(pos);
      }

      if(!isInside && trace.size() > 0) Kill(this);
    }
  }
}


boolean WithinBounds(PVector pos)
{
  if((pos.x>=0)&&(pos.y>=0)&&(pos.x<=imgX)&&(pos.y<=imgY))
  {
    return true;
  }
  return false;
}

void Kill(Cube c)
{
  c.isKilled = true;
}