import processing.opengl.*;
import oscP5.*;
import netP5.*;

OscP5 osc;
NetAddress myRemoteLocation;
float[] ir;
boolean DEBUG;



float sinLUT[];
float cosLUT[];
float SINCOS_PRECISION=1.0;
int SINCOS_LENGTH= int((360.0/SINCOS_PRECISION));
 
 void setup() {
  size(800, 800, OPENGL);
  
  // Fill the tables
  sinLUT=new float[SINCOS_LENGTH];
  cosLUT=new float[SINCOS_LENGTH];
  for (int i = 0; i < SINCOS_LENGTH; i++) {
    sinLUT[i]= (float)Math.sin(i*DEG_TO_RAD*SINCOS_PRECISION);
    cosLUT[i]= (float)Math.cos(i*DEG_TO_RAD*SINCOS_PRECISION);
  }

  // open an udp port for listening to incoming osc messages from darwiinremoteOSC
  osc = new OscP5(this,5600);

  osc.plug(this,"ir","/wii/irdata");
  osc.plug(this,"connected","/wii/connected");

  ir = new float[12];
 
  frameRate(30); 
  background(0);
}

void connected(int theFlag) {
  if(theFlag==1) {
    println("wii connected");
  } 
  else {
    println("wii DISCONNECTED");
  }
}

  float x[] = new float[4];
 float y[] = new float[4];
  int points = 0;
  

float fx, fz, fy;
void ir(
float f10, float f11,float f12, 
float f20,float f21, float f22,
float f30, float f31, float f32,
float f40, float f41, float f42
) {
  ir[0] = f10;
  ir[1] = f11;
  ir[2] = f12;
  ir[3] = f20;
  ir[4] = f21;
  ir[5] = f22;
  ir[6] = f30;
  ir[7] = f31;
  ir[8] = f32;
  ir[9] = f40;
  ir[10] = f41;
  ir[11] = f42;

  points = 0;  
  for (int i = 0; i < 4; i += 3) {
    if (ir[i+2] < 15) {
//      x[points] = ir[i] / 768.0;           
//      y[points] = ir[i+1] / 768.0;     
        x[points] = 0.5 - ir[i];
        y[points] = 0.5 - ir[i+1];
      points++;
    } 
  }
  
  float fov = (PI / 4.0);
  float barWidth = 150.0; // bar width in mm
  if (points >= 2) {
//    println("x1 " + x[0] + " y1 " + y[0]);
//    println("x2 " + x[1] + " y2 " + y[1]);
    float dx = x[0] - x[1];
    float dy = y[0] - y[1];
    float dist = sqrt(dx * dx + dy * dy);

//    println("dist "  + dist);
    float angle = fov * dist / 2.0;
    float headDist = (barWidth / 2.0) / tan(angle);
//    println("angle " + degrees(angle) + " headDist " + headDist);
    
    float mx = (x[0] + x[1]) / 2.0;
    float my = (y[0] + y[1]) / 2.0;
    
    float rx = sin(fov * mx) * headDist * 0.5;
    float ry = sin(fov * my) * headDist * 1.5;
//    println("rx " + rx + " ry " + ry);
    fx = -rx;
    fy = -ry;
    fz = headDist;
  }
}

void draw() {
  background(0);
  //  translate(width/2, height/2, 0);
  // camera(-width/2, -height/2, 0,
  //        width/2, height/2, 0,
  //        0, 1, 0);

//  float fx = (float)(-width/2.0 + mouseX) * 3.0;
//  float fy = 0;
//  float fz = 800.0 +  (float)(mouseY - height/2.0) * 3.0;
  
//  println("fx " + fx + " fz " + fz);

  // println("fx " + fx);
  camera(fx, fy, fz,
         fx, fy, 0,
         0, 1, 0);
  float near = fz;

float left, right, top, bottom;
  float angle = radians(60.0);
  float facd = (width/2.0) / tan(angle / 2.0);
  near = 20.0;
  left = -(width/2.0) + fx;
  right = (width/2.0) + fx;
  top = -(height/2.0) + fy;
  bottom = (height/2.0) + fy;
  left /= facd / fz;
  right /= facd / fz;
  top /= facd / fz;
  bottom /= facd / fz;
  left *= near / fz;
  right *= near / fz;
  top *= near / fz;
  bottom *= near / fz;
  frustum(left, right, top, bottom, near, 60000);
  //frustum(-200, 200, -200, 200, fz, fz+3000);
  //       perspective(PI/3.0, 1, mouseX, 2000);

  mainDraw();  
  /*
  noFill();
  stroke(0, 200, 0);
  pushMatrix();
  rotateX(PI/2.0);
  translate(fx, -fy, -fz + 50);
  ellipse(0, 0, 10, 10);
  popMatrix();
  */
  /*
  pushMatrix();
  drawTunnel(width, height);
  stroke(255, 0, 0);
  line(0, 0, 40, 0);
  stroke(0, 255, 0);
  line(0, 0, 0, 40);

  stroke(255);
  fill(160);
  popMatrix();
  //  box(40);

  if (points >= 2) {
//    zielScheibe(x[0] * 400.0, y[0] * 400.0, -100);
//    zielScheibe(x[1] * 400.0, y[1] * 400.0, -100);
  }
  */
}

float zielX = 0, zielY = 0, zielZ = 4000;

void mainDraw() {
  
  if (zielZ > (fz - 100)) {
    println("ziel " + zielX + ", " + zielY + " player " + fx + ", " + fy);
    float maxX = 80;
    float maxY = 80;
    zielX = random(-maxX, maxX);
    zielY = random(-maxY, maxY);
    zielZ = -3000;
  }
  
  zielScheibe(zielX, zielY, zielZ);
  zielZ += 50.0;
  
  drawTunnel(width, height);
}

void drawTunnel(float width, float height) {
  float w = width/2.0;
  float h = height/2.0;
  float offs = -50;
  stroke(255);

  float times = 20;
  for (float wx = 0; wx <= width/2; wx += width / (float)times) {
    line(-wx, h, 0, -wx, h, times * offs);
    line(-wx, -h, 0, -wx, -h, times * offs);
    line(wx, h, 0, wx, h, times * offs);
    line(wx, -h, 0, wx, -h, times * offs);
  }

  for (float hx = 0; hx <= width/2; hx += width / times) {
    line(-w, hx, 0, -w, hx, times * offs);
    line(-w, -hx, 0, -w, -hx, times * offs);
    line(w, hx, 0, w, hx, times * offs);
    line(w, -hx, 0, w, -hx, times * offs);
  }

  int myColor = 255;
  for (int i = 0; i <= times; i++) {
    stroke(myColor);
    myColor -= 7;
    line(-w, h, i * offs, w, h, i*offs);
    line(w, h, i * offs, w, -h, i*offs);
    line(w, -h, i * offs, -w, -h, i*offs);
    line(-w, -h, i * offs, -w, h, i*offs);
  }
 
 zielScheibe(30, 30, -100); 
  zielScheibe(-30, 30, -150);
 zielScheibe(30, 120, 100); 
 zielScheibe(-120, 80, -350);
 zielScheibe(90, 90, -850); 
 zielScheibe(75, -30, -150);
 zielScheibe(90, -120, 100); 
 zielScheibe(-200, -80, -350);
 zielScheibe(-200, 0, 350);
 
 stroke(0);
 strokeWeight(2.0);
 rotateX(PI/2.0);
}

void zielScheibe(float x, float y, float z) {
  pushMatrix();
  noStroke();
  translate(x, y, z);
  fill(255);
  ellipse(0, 0, 50, 50);
  translate(0, 0, 0.2);
  fill(0);
  ellipse(0, 0, 30, 30);
  translate(0, 0, 0.2);
  fill(255, 0, 0);
  ellipse(0, 0, 10, 10);

translate(0, 0, -0.3);
rotateX(PI/2.0);
fill(80);
 drawCylinder(5, 5, -8000, 20);
  popMatrix();
}

void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) {
  float angle = 0;
  float angleIncrement = TWO_PI / sides;
  beginShape(QUAD_STRIP);
  for (int i = 0; i < sides + 1; ++i) {
    vertex(topRadius*cos(angle), 0, topRadius*sin(angle));
    vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle));
    angle += angleIncrement;
  }
  endShape();
  
  // If it is not a cone, draw the circular top cap
  if (topRadius != 0) {
    angle = 0;
    beginShape(TRIANGLE_FAN);
    
    // Center point
    vertex(0, 0, 0);
    for (int i = 0; i < sides + 1; i++) {
      vertex(topRadius * cos(angle), 0, topRadius * sin(angle));
      angle += angleIncrement;
    }
    endShape();
  }

  // If it is not a cone, draw the circular bottom cap
  if (bottomRadius != 0) {
    angle = 0;
    beginShape(TRIANGLE_FAN);

    // Center point
    vertex(0, tall, 0);
    for (int i = 0; i < sides + 1; i++) {
      vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
      angle += angleIncrement;
    }
    endShape();
  }
}