import hypermedia.video.*;
import java.awt.*;

OpenCV opencv;
PImage img;
int n;
PVector norm;
PVector tan;
Rectangle rect;
int blobNum;

float[] tilt(Blob[] blobs, PImage img)
{
  img.loadPixels();

  int brightestX = 0;
  int brightestY = 0;
  int brightestJ = 0;
  float brightestF = 0;
  int brightestLoc = 0;

  for (int j = 0; j < blobs[blobNum].points.length; j++)
  {
    int y = blobs[blobNum].points[j].y;
    int x = blobs[blobNum].points[j].x;
    int loc = x+y*width;

    float r = red(img.pixels[loc]);
    float g = green(img.pixels[loc]);
    float b = blue(img.pixels[loc]);

    float gray = (r+g+b)/3;
    if (gray > brightestF)
    {
      brightestF = gray;
      brightestX = x;
      brightestY = y;
      brightestJ = j;
      r = 255;
    }
    img.pixels[loc] = color(r, g, b);
  }

  brightestLoc = brightestX+brightestY*width;

  float r = red(img.pixels[brightestLoc]);
  float g = green(img.pixels[brightestLoc]);
  float b = blue(img.pixels[brightestLoc]);
  r = 0;
  g = 255;
  b = 0;
  img.pixels[brightestLoc] = color(r, g, b);

  //FIND NORMAL
  int dist = 10;
  int x2 = blobs[blobNum].points[brightestJ+dist].x;
  int x1 = blobs[blobNum].points[brightestJ-dist].x;
  int y2 = blobs[blobNum].points[brightestJ+dist].y;
  int y1 = blobs[blobNum].points[brightestJ-dist].y;
  int dx = x2-x1;
  int dy = y2-y1;

  //  tan = new PVector(dx, dy);
  PVector up = new PVector(0, 1);
  norm = new PVector(-dy, dx);
  //  float tilt = PVector.angleBetween(up, norm); //radians
  float tilt = norm.heading();
  //  println(degrees(bearing(norm)));

  img.updatePixels();
  line(x1, y1, x2, y2);
  line(brightestX, brightestY, brightestX-dy, brightestY+dx);

  float[] returns = new float[2];
  returns[0] = tilt;
  returns[1] = brightestF;

  return returns;
  //  print(norm);
}



float[] slant(float maxContourIntensity, PImage img, Rectangle rect)
{
  int brx = 0;
  int bry = 0;
  int brl = 0;
  float brf = 0;
  img.loadPixels();
  for (int i = rect.x; i < rect.x+rect.width; i++)
  {
    for (int j = rect.y; j < rect.y+rect.height; j++)
    {
      int loc = i+(j*width);
      //println(loc);
      float ra = red(img.pixels[loc]);
      float ga = green(img.pixels[loc]);
      float ba = blue(img.pixels[loc]);

      float gray = (ra+ga+ba)/3;
      if (gray > brf)
      {
        brf = gray;
        brx = i;
        bry = j;
        brl = loc;
        //        ba = 255;
      }
      img.pixels[loc] = color(ra, ga, ba);
    }
  }

  Point p = new Point(brx, bry);
  //  print(p);
  point(p.x, p.y);

  float bl = 255;
  float rl = 0;
  float gl = 0;

  img.pixels[brl] = color(rl, gl, bl);

  img.updatePixels();
  //  image(img, 0, 0);

  float cosa = (float)maxContourIntensity / (float)brf;
  float slant = acos(cosa); //radians i think
  //  print(cosa + " : " + slant);
  float[] slantReturns = new float[3];
  slantReturns[0] = slant;
  slantReturns[1] = p.x;
  slantReturns[2] = p.y;

  return slantReturns;
}




void setup()
{
  size(1690/2, 650/2, P3D);

  opencv = new OpenCV(this);
  //  opencv.capture(640, 480);
  opencv.loadImage("balls.jpeg", width, height);
  img = opencv.image();

  background(255);
  //  opencv.read();
  opencv.threshold(80);

  Blob[] blobs = opencv.blobs(10, width*height/2, 100, true, OpenCV.MAX_VERTICES*4);
  opencv.restore();

  float[] tiltReturns = tilt(blobs, opencv.image());
  //  println(degrees(tilt));

  float tilt = tiltReturns[0];
  float maxC = tiltReturns[1];
  //detect surface maxima

  blobNum = 2;
  rect = blobs[blobNum].rectangle;
  //  rect(rect.x, rect.y, rect.width, rect.height);
  float[] slantReturns = slant(maxC, img, rect);
  float slant = slantReturns[0];
  int bsx = (int)slantReturns[1];
  int bsy = (int)slantReturns[2];

  println("Tilt: " + degrees(tilt) + " : Slant: " + degrees(slant));

  pushMatrix();
  translate(width/2, height/2, 0);
  noFill();
  image(img, -width/2, -height/2, width, height);
  image(img, 0, 0);
  stroke(256, 0, 0);
  sphere(256);
  stroke(0, 0, 256);
  float x, y, z;
  x = y = z = 0;
  y = sin(tilt)*128;
  x = cos(tilt)*128;
  z = cos(slant)*128;
  println("X: "+ x + " Y: " + y + " Z: " + z);
  line(bsx, bsy, 0, bsx+2*x, bsy+2*y, 2*z);
  popMatrix();

  //  print(rect);
}