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

OpenCV opencv;
Capture cam;

Bot machine;

import processing.serial.*;  // serial communication
import java.io.*; //Java

// configuration
boolean serialOn = true;
boolean writeToFile = true;
boolean printToConsole = true;

PrintWriter writer;


int threshold = 80;
int w=640;
int h=480;

//---------------------------------------------------
// physical setup
float MOTORSEP = 80.5;//96.0;//72.0; // how far apart are the two steppers

float PAGEW = 46.0;//24.0;
float PAGEH = 36.0;//18.0;u0

// page location relative to motors
float PAGE0X = (MOTORSEP-PAGEW)/2.0;  // upper left
float PAGE0Y = 21.5;//36.0;

// home position of pen carriage. where it is when the program starts
float HOMEA = 80.75;//81.0;//82.0;// 58.5225650924496; // 108
float HOMEB = 80.75;//81.0;//82.0;//56.02580320709378; // 108

// scale image to screen for display
float PPI = 640.0/PAGEW;//58.0; // 640 pixels / 11 inches 

int windowH = 800;
float onScreenScale=windowH/(PAGEH*PPI);
int windowW = int((PAGEW*PPI)*onScreenScale);

//-----------------------------------
// state variables
boolean On = false;// USED SO THAT THE KEY 'S' AND 'G' START AND STOP THE DRAWING SEQUENCE, starts off to calibrate
boolean jumping = false;
boolean atPoint = false;

float JUMPTHRESH = 300.0;//80.0;

// contrast/brightness values
int contrast_value    = 0;
int brightness_value  = 0;

PVector Last, Current;
int lastLine=0;
int lastSeen=0;
int lastSort=0;
int numdrawn=0;
boolean sorted=false;

int IDLETIME=1000;
int SORTTIME=10000;
ArrayList blobBuffer;

int MAXBUFFSIZE=1000;
ArrayList sortedBlobBuffer;

void setup() {
  // graphics
  size(1280, 1020);//windowW, windowH);

  String[] cameras = Capture.list();

  if (cameras.length == 0) {
    println("There are no cameras available for capture.");
    exit();
  } 
  else {
    println("Available cameras:");
    for (int i = 0; i < cameras.length; i++) {
      println(cameras[i]);
    }

    cam=new Capture(this, 640, 480, cameras[1]);
    cam.start();

    // You can get the list of resolutions (width x height x fps)  
    // supported capture device by calling the resolutions()
    // method. It must be called after creating the capture 
    // object. 
    Resolution[] res = cam.resolutions();
    println("Supported resolutions:");
    for (int i = 0; i < res.length; i++) { 
      println(res[i].width + "x" + res[i].height + ", " + 
        res[i].fps + " fps (" + res[i].fpsString +")");
    }
  }      

  opencv = new OpenCV( this );
  //opencv.capture( 640, 480);                   // open video stream
  opencv.allocate(640, 480);
  //opencv.cascade( OpenCV.CASCADE_FRONTALFACE_ALT );  // load detection description, here-> front face detection : "haarcascade_frontalface_alt.xml"


  // setup drawing bot
  machine = new Bot(this);
  machine.waitForStart();

  writer = createWriter("test.nc");
  machine.setupWriting(writer);

  // calc home values for X and Y (upper left corner of page)
  //HOMEA = sqrt(pow(PAGE0X, 2)+pow(PAGE0Y, 2));
  //HOMEB = sqrt(pow(MOTORSEP-(PAGE0X), 2)+pow(PAGE0Y, 2));
  if (printToConsole)
    println("(HOME: "+HOMEA + " " +HOMEB+")");

  machine.storeHome();
  machine.penUp();

  jumping=true;

  Last = new PVector(MOTORSEP/2, sqrt(81*81-(MOTORSEP/2*MOTORSEP/2)), 0);
  Current = new PVector();

  background(0);
  fill(255);
  rect(640, 0, 640, 480);

  blobBuffer = new ArrayList();
  blobBuffer.add(new PVector(Last.x, Last.y));
  sortedBlobBuffer = new ArrayList();
}

void updateVision() {
  // grab a new frame
  // and convert to gray
  if (cam.available()==true) {
    cam.read();

    //image(cam, 0, 0);
    opencv.copy(cam);
    opencv.flip(OpenCV.FLIP_HORIZONTAL);

    // display the image
    image( opencv.image(), 0, 0);
    //image( opencv.image(OpenCV.MEMORY), 0, h ); // image in memory

    opencv.absDiff();
    image( opencv.image(), 0, h, 320, 240);   // GRAY image
    opencv.threshold(threshold);
    image( opencv.image(OpenCV.GRAY), 320, h, 320, 240);   // GRAY image

    // proceed detection
    Blob[] blobs = opencv.blobs( 100, w*h/3, 20, true );

    // draw face area(s)
    noFill();
    stroke(255, 0, 0);
    int biggest = -1;
    float biggestw = -1;

    for ( int i=0; i<blobs.length; i++ ) {

      Rectangle bounding_rect	= blobs[i].rectangle;
      float area = blobs[i].area;
      float circumference = blobs[i].length;
      Point centroid = blobs[i].centroid;
      Point[] points = blobs[i].points;

      // rectangle
      noFill();
      stroke( blobs[i].isHole ? 128 : 64 );
      rect( bounding_rect.x, bounding_rect.y, bounding_rect.width, bounding_rect.height );

      // centroid
      stroke(0, 0, 255);
      line( centroid.x-5, centroid.y, centroid.x+5, centroid.y );
      line( centroid.x, centroid.y-5, centroid.x, centroid.y+5 );
      noStroke();
      //fill(0, 0, 255);
      //text( area, centroid.x+5, centroid.y+5 );

      fill(255, 0, 255, 64);
      stroke(255, 0, 255);
      if ( points.length>0 ) {
        beginShape();
        for ( int j=0; j<points.length; j++ ) {
          vertex( points[j].x, points[j].y );
        }
        endShape(CLOSE);
      }

      noStroke();
      //fill(255, 0, 255);
      //text( circumference, centroid.x+5, centroid.y+15 );

      if (!blobs[i].isHole && (blobs[i].area > biggestw)) 
      {
        biggestw=blobs[i].area;
        biggest=i;
        //println("(biggest "+biggest+")");
      }
    }

    if ((blobs.length>0) && (biggest>=0)) {
      blobBuffer.add(new PVector(blobs[biggest].centroid.x, blobs[biggest].centroid.y));

      Last.x=Current.x;
      Last.y=Current.y;
      Current.x=blobs[biggest].centroid.x;
      Current.y=blobs[biggest].centroid.y;

      stroke(128, 128);
      fill(128, 128);
      drawDot(Current.x, Current.y);
      drawDot(Current.x+640, Current.y);

      //println("(dist "+Last.dist(Current)+")");
      if (Last.dist(Current)<JUMPTHRESH) {
        // not jumping
        stroke(128, 128);
        drawScaledSegment(Last.x+640, Last.y, Current.x+640, Current.y);
      }

      lastSeen=millis();
    }
  }
}

void updateDrawing() {
  if (millis()-lastLine>IDLETIME) {
    //machine.penUp();
  }

  if(!sorted && ((millis()-lastSort)>SORTTIME)) {
    // sort buffered points, nearest neighbor
    sortPoints();

    sorted=true;
    lastSort=millis();
    numdrawn=0;
  }

  if (sorted) {
    if (sortedBlobBuffer.size()>1) {
      // sort according to current position
      PVector p1 = (PVector) sortedBlobBuffer.get(0);
      PVector p2 = (PVector) sortedBlobBuffer.get(1);
      
      doMessage("(sending pt "+numdrawn+" val "+p2.x+","+p2.y+")");
      stroke(255, 0, 0);
      drawDot(p2.x, p2.y);
      
      if (p1.dist(p2)<JUMPTHRESH) {
        // not jumping
        stroke(255, 0, 0);
        strokeWeight(2);
        drawScaledSegment(p1.x+640, p1.y, p2.x+640, p2.y);
        strokeWeight(1);
      }
      
      //machine.sendVal(Current.x/PPI, Current.y/PPI, PEN_DOWN); // send current point
      //machine.waitForOk();
      sortedBlobBuffer.remove(0);
      println("("+sortedBlobBuffer.size()+" points left)"); 

      numdrawn++;
      if(numdrawn>30 && ((millis()-lastSort)>SORTTIME)) {
        sorted=false;
        println("("+sortedBlobBuffer.size()+" points left)"); 
        sortedBlobBuffer.clear();
      }
    } else {
      sorted=false;
    }
  }
}

void sortPoints() {
  println("(sorting points by distance, nearest neighbor -- "+blobBuffer.size()+"points)");
  
  boolean [] pointUsed = new boolean[blobBuffer.size()];
  
  sortedBlobBuffer.clear();
  //sortedBlobBuffer = new ArrayList();
  
  for (int i=0; i<blobBuffer.size(); i++)
    pointUsed[i]=false;

  boolean done=false;
  int i=blobBuffer.size()-1;
  int count=0;

  while (!done) {
    PVector finish = (PVector) blobBuffer.get(i);
    sortedBlobBuffer.add(finish);
    pointUsed[i]=true;

    float testdist = 999999.0;
    int testindex=-1;

    for (int j=0; j<blobBuffer.size(); j++) {
      if (!pointUsed[j]) {
        PVector test = (PVector) blobBuffer.get(j);
        float thisdist=dist(finish.x, finish.y, test.x, test.y);

        if (thisdist < testdist) {
          testdist=thisdist;
          testindex=j;
        }
      }
    }

    if (testindex > 0) {
      pointUsed[testindex]=true;
      i=testindex;
      count++;
      if (count>=blobBuffer.size())
        done=true;
    } 
    else {
      println("(got a negative index. i:"+i+" count:"+count+")");
      done=true;
    }
  }
  println("(done sorting. "+sortedBlobBuffer.size()+" sorted points. tested "+count+")");
  blobBuffer.clear();
}

void storeBackground() {
  
    // store background movement
    opencv.copy(cam);
    opencv.flip(OpenCV.FLIP_HORIZONTAL);
    opencv.remember(OpenCV.BUFFER);

}


void draw() {

  updateVision();
  updateDrawing();
}

void drawScaledSegment(float x1, float y1, float x2, float y2) {
  line(x1, y1, x2, y2);//*onScreenScale, y1*onScreenScale, x2*onScreenScale, y2*onScreenScale);
};

void drawDot(float x, float y) {
  float px, py;
  px=x;//*onScreenScale;
  py=y;//*onScreenScale;

  //println(px+","+py);
  ellipse(x, y, 2, 2);
};

void stop() {
  doMessage("(stop)");
  machine.penUp();
  machine.DirectCommand("G0 X"+HOMEA+" Y"+HOMEB+" Z0.0");
  machine.stop();

  opencv.stop();
  super.stop();
};

// keyboard interface
void keyPressed() {

  if (key==' ' ) {
    storeBackground();
  }

  if (key == 'g') { 
    // start movement
    On = true;
    doMessage("(GO!)");
    if (serialOn) {
      machine.DirectCommand("G100 S100"); //moves servo, push pen off page
      machine.waitForOk();
    }
    jumping=true;
  }

  if (key == 's') {
    // stop movement
    On = false;
    if (printToConsole)println("(STOP)");
  }

  if (key=='h') {
    machine.goHome();
  }
  if (!On) {
    switch(key) {
    case '1':
      // top left
      machine.sendVal(0, 0, 0);
      break;
    case '2':
      // top right
      machine.sendVal(PAGEW, 0, 0);
      break;
    case '3':
      // lower right
      machine.sendVal(PAGEW, PAGEH, 0);
      break;
    case '4':
      // lower left
      machine.sendVal(0, PAGEH, 0);
      break;
    case '0':
      machine.penUp();
      doMessage("(going to stored home position)");
      machine.DirectCommand("G0 X"+HOMEA+" Y"+HOMEB+" Z0");
      break;
    case 'd':
      machine.penDown();
      break;
    case 'u':
      machine.penUp();
      break;
    default:
      break;
    }
  }
}


// -------- terminal output and logging -------- // 
void doMessage(String message) {
  if (printToConsole)
    println(message);
  if (writeToFile) {
    writer.write(message+"\n");
    writer.flush();
  }
}

void mouseDragged() {
  threshold = int( map(mouseX, 0, width, 0, 255) );
}