import procontroll.*;
import processing.serial.*;


import cc.arduino.*;
import processing.video.*;

Capture cam;

Arduino arduino;

ArrayList rows;
ArrayList textures;

float prevRightAngle =0;
float prevLeftAngle =0;

float deltaMouseX=0;
float deltaMouseY=0;

float xCam = 0;
float yCam = 0;

float PPI = 400;

ControllDevice usbMouse;

void setup() {
  size(640, 640, P3D); 
  background(0);
  lights();  
  rows = new ArrayList();
  textures = new ArrayList();

  cam = new Capture(this, 320, 240);

  RowPoints tempPoints = new RowPoints(.25,.24, 0,0);
  rows.add(tempPoints);

  ControllIO controll = ControllIO.getInstance(this);
  controll.printDevices();
  usbMouse = controll.getDevice(3);

  arduino = new Arduino(this, Arduino.list()[0], 57600);
  println(Arduino.list());
  
  cam.read();
  addTexture();
}

void draw() {
  cam.read();
  
  float a1 = arduino.analogRead(0);
  float a2 = arduino.analogRead(1);
  float angle2 = (a1-550)/200.0 * 90.0;
  float angle1 = (a2-500)/280.0 * 90.0;

  //println(angle1);
  //println(angle2);
  
  float tiltX = arduino.analogRead(4);
  float tiltY = arduino.analogRead(3);
  float tiltZ = arduino.analogRead(2);
  
  
  
  println(tiltX);
  
  //println(arduino.analogRead(1));
  deltaMouseX += usbMouse.getSlider(0).getValue();
  deltaMouseY += usbMouse.getSlider(1).getValue();
  println(deltaMouseX + " " +deltaMouseY);
  if(deltaMouseX>PPI) {
    deltaMouseX=0; 
    rows.add(new RowPoints(radians(angle1),radians(angle2), 30, 0));
    addTexture();
  }
  if (deltaMouseX < -1*PPI){
    deltaMouseX=0;
    rows.add(new RowPoints(radians(angle1),radians(angle2), -30, 0));
    addTexture();
  }
  if(deltaMouseY>PPI) {
    deltaMouseY=0; 
    rows.add(new RowPoints(radians(angle1),radians(angle2), 0, 30));
    addTexture();
  }
  if (deltaMouseY < -1*PPI){
    deltaMouseY=0;
    rows.add(new RowPoints(radians(angle1),radians(angle2), 0, -30));
    addTexture();
  }
  background(0);
  noStroke();

  pushMatrix();
  translate(width/2, height/2, 0);
  rotateY(radians(xCam));
  rotateX(radians(yCam));
  for(int i =0; i < rows.size(); i++) {
    RowPoints tempRow = (RowPoints)(rows.get(i));
    if(tempRow.xDelta > 0) {
      rotateY(prevRightAngle);
    }
    if(tempRow.xDelta < 0) {
      rotateY(-prevLeftAngle);
    }
    translate(tempRow.xDelta,tempRow.yDelta, 0 );  

    PImage tempTexture = (PImage)(textures.get(i));  
    beginShape();
    texture(tempTexture);
    vertex(-10, -10, 0, 0, 0);
    vertex(10, -10, 0, 320, 0);
    vertex(10, 10, 0, 320, 240);
    vertex(-10, 10, 0, 0, 240);
    endShape();

    pushMatrix();
    pushMatrix();
    rotateY(tempRow.rightAngle);
    translate(30,0,0);
     beginShape();
    texture(tempTexture);
    vertex(-10, -10, 0, 0, 0);
    vertex(10, -10, 0, 320, 0);
    vertex(10, 10, 0, 320, 240);
    vertex(-10, 10, 0, 0, 240);
    endShape();
    popMatrix();
    rotateY(-1*tempRow.leftAngle);
    translate(-30,0,0);
     beginShape();
    texture(tempTexture);
    vertex(-10, -10, 0, 0, 0);
    vertex(10, -10, 0, 320, 0);
    vertex(10, 10, 0, 320, 240);
    vertex(-10, 10, 0, 0, 240);
    endShape();
    popMatrix();
    prevRightAngle = tempRow.rightAngle;
    prevLeftAngle = tempRow.leftAngle;
  }
  popMatrix();


}


void drawRow() {

}

void keyPressed() {
  if (key == CODED) {
    if (keyCode == UP) {
      //rows.add(new RowPoints(.25,.24, 0, 30));
      yCam+=1;
    } 
    else if (keyCode == DOWN) {
      //rows.add(new RowPoints(.25,.24, 0, -30));
      yCam-=1;
    } 
    else if (keyCode == RIGHT) {
      //rows.add(new RowPoints(.25,.24, 30, 0));
      xCam+=1;
    }
    else if (keyCode == LEFT) {
      xCam-=1;
      //rows.add(new RowPoints(.25,.24, -30, 0));
    }
  } 
  else {
    if (key == 'c') {
      rows.clear(); 
      textures.clear();
    }
  }

}

class RowPoints {
  float rightAngle;
  float leftAngle;
  float xDelta;
  float yDelta;

  RowPoints(float rightAngleTemp, float leftAngleTemp, float xDeltaTemp, float yDeltaTemp){
    rightAngle = rightAngleTemp;
    leftAngle =  leftAngleTemp;
    xDelta = xDeltaTemp;
    yDelta = yDeltaTemp;
  }
}

void addTexture() {
  cam.read();
  PImage img = createImage(320 , 240, RGB);
    img.copy(cam, 0, 0, 320,  240, 0, 0, 320, 240); 
  textures.add(img);
}