//
// vitruvian square, circle
//
// rtwomey@u.washington.edu

// Key 1 = 0,0
// Key 2 = width,0
// Key 3 = width,height
// Key 4 = 0,height
// Keys 5-8, corners of svg image
// Key 0 = Home
// Key H = home device
// Key h = set origin and init coords.
// Key S = stop
// Key G = go

Bot machine;

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

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

float PAGEW = 54.0;//24.0;
float PAGEH = 36.0;//18.0;
float MARGIN = 0.5;

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

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

// scale image to screen for display
int windowH = 800;
float onScreenScale=windowH/PAGEH;
int windowW = int(PAGEW*onScreenScale);

// feed rates and gcode stuff
float SLOWFEED = 100.0;//15.0;

float MAXLENGTH=1.0;
float MAXARCLENGTH=0.5;

float RESTTIME = 1.0;
float LONGRAD = 60.0;

int WAITTIME = 0;//1500;

//-----------------------------------
// state variables
boolean On = false;// USED SO THAT THE KEY 'S' AND 'G' START AND STOP THE DRAWING SEQUENCE, starts off to calibrate
boolean done = false;
boolean doJump = false;
boolean atPoint = true; // if the arduino returns that it is at the point, turn true.
boolean atHome=true;

// state of machine
float zoom;
int state=0;
int count=0;
int s=0; 
int repeats=0;
float segs=0.0; // how many segments in current line
boolean segDone=false;

float lx, ly; // fractional x,y positions
PVector Endpoint; // end of current line
PVector Last; // end of last line

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

static int STATE_SQUARE = 0;
static int STATE_CIRCLE = 1;
static int STATE_DONE = 2;

//------------------------------------
// main program
void setup() {
  // graphics
  //size(windowW, windowH);
  size(1237, 900);

  // load a font for text display
  PFont font;
  font = loadFont("Serif-24.vlw"); 
  textFont(font, 12); 

  //  // store progress images
  //  Photo = new OutputImage();

  // arduino g-code interface
  machine = new Bot(this);
  machine.waitForStart();

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

  background(255);

  if (printToConsole)
    println("(HOME: "+HOMEA + " " +HOMEB+")");

  machine.storeHome();
  machine.penUp();
  Last = new PVector(MOTORSEP/2, sqrt(81*81-(MOTORSEP/2*MOTORSEP/2)), 0);
  Endpoint = new PVector();

  println("(at "+Last.x+","+Last.y+" relative to PAGE(0,0))");

  noCursor();

  zoom=1.0;
}


void draw() {  
  stroke(120);
  strokeWeight(2);
  fill(220);

  if (On && !done) {
    if (drawPatternAtScale(MARGIN+((PAGEW-PAGEH)/2.0), MARGIN, PAGEW-(MARGIN+((PAGEW-PAGEH)/2.0)), PAGEH-MARGIN, zoom)) {
      doMessage("(next zoom level "+zoom+")");
      zoom=zoom-0.05;
      state=0;
      count=0;
      s=0;
    }      
    if (zoom < 0.0) {
      println("(done.)");
      if(doRepeat) {
        println("(starting over)");
        background(255);
        repeats++;
        println("(pattern iteration "+repeats+")");
        zoom=1.0;
      } else {
        println("(stopping)");
        done=true;
      }
    }
  }
}

// -------- resuming drawing -------- //
//boolean doneFFWDing() {
//  if (!doResume)return true;
//
//  if ((x==runtoX) && (y==runtoY) && (zoomLevel==runtoZoom) && (count==runtoLine)) {
//    println("(found doResume point)");
//    serialOn=true;
//    doResume=false;
//  }
//  return false;
//}


// -------- drawing routines --------//

boolean drawPatternAtScale(float left, float top, float right, float bot, float zoom) {
  float midx=(left+right)/2.0;
  float midy=(top+bot)/2.0;

  float tl, tr, tt, tb;

  tl=lerp(midx, left, zoom);
  tr=lerp(midx, right, zoom);
  tt=lerp(midy, top, zoom);
  tb=lerp(midy, bot, zoom);

  switch(state) {
    case 0:
      if (drawsquare(tl, tt, tr, tb))
        state++;
      break;
    case 1:
      if (drawcircle((tl+tr)/2.0, (tt+tb)/2.0, (tr-tl)/2.0))
        state++;
      break;
    case 2:
      if (drawcross(tl, tt, tr, tb))
        state++;
      break;
    }
  
  if(state>2) {
    return true;
  }

  return false;
}

boolean drawsquare(float left, float top, float right, float bot) {
  switch (count) {
  case 0:
    drawScaledSegment(left, top, right, top);
    break;
  case 1:
    drawScaledSegment(right, top, right, bot);
    break;
  case 2:
    drawScaledSegment(right, bot, left, bot);
    break;
  case 3:
    drawScaledSegment(left, bot, left, top);
    break;
  }

  if (isLineDone()) {
    count++;
    s=0;
    if (count>3) {
      count=0;
      return true;
    }
  }

  return false;
}

boolean drawcross(float left, float top, float right, float bot) {
  //  float ux=(right-left);
  //  float uy=(bot-top);
  //  float x=0;
  //  float y=0;

  switch(count) {
  case 0:
    drawScaledSegment(left, top, right, bot);
    break;
  case 1:
    drawScaledSegment(right, top, left, bot);
    break;
  }
  //case 6:      
  //    drawScaledSegment(left, top+uy*0.5, right, top+uy*0.5);
  //    break;
  //  case 7:
  //    drawScaledSegment(left+ux*0.5, top, left+ux*0.5, bot);
  //    break;
  //  

  if (isLineDone()) {
    s=0;
    count++;
    doMessage("(next line in cross)");
    if (count>1) {
      doMessage("(done with cross)");
      count=0;
      return true;
    }
  }
  return false;
}

boolean drawcircle(float xcenter, float ycenter, float radius) {
  if (drawScaledArc(xcenter, ycenter, radius)) {
    count=0;
    s=0;
    return true;
  }
  return false;
}

boolean drawScaledArc(float xcenter, float ycenter, float radius) {
  float circum=TWO_PI*radius;
  int numsteps = int(circum/MAXARCLENGTH);
  float ang_inc=TWO_PI/numsteps;

  float theta=ang_inc*count;
  float theta2=theta+ang_inc;

  float x1, y1, x2, y2;

  x1 = xcenter + radius * sin(theta);
  y1 = ycenter + radius * cos(theta);
  x2 = xcenter + radius * sin(theta2);
  y2 = ycenter + radius * cos(theta2);

  s=0; // signalling something
  drawScaledSegment(x1, y1, x2, y2); 

  count++;
  if (count>numsteps)
    return true;

  return false;
}

boolean isLineDone() {
  //println("(checking segment "+(s-1)+" of line "+count+", at pattern "+x+","+y+"...)");
  if (s>=segs) {
    // finish multi-part line
    if (segs>1) {

      line(lx*onScreenScale, ly*onScreenScale, Endpoint.x*onScreenScale, Endpoint.y*onScreenScale);

      //println("finished long line");
      if (serialOn) {  
        //machine.penDown();
        machine.sendVal(Endpoint.x, Endpoint.y, PEN_DOWN);
        machine.waitForOk();
      }
      doMessage("(finished long line)");
    }

    s=0;
    // store current position
    segDone=true;

    //Last=Endpoint;
    Last.x=Endpoint.x;
    Last.y=Endpoint.y;
    //println("(done.)");
    return true;
  }
  //println("(not done.)");
  return false;
}


void drawScaledSegment(float x1, float y1, float x2, float y2) {

  if (s==0) {
    // starting a new line

    // jog to a new position
    stroke(240, 0, 0, 24);
    line(Last.x*onScreenScale-2, Last.y*onScreenScale-2, x1*onScreenScale-2, y1*onScreenScale-2);
    // only send if new start is not equal to last end
    if ((x1!=Last.x) || (y1!=Last.y)) {
      doJump=true;
      //doMessage("(JUMPING!)");
    }

    if (serialOn) {
      if (doJump)
        machine.penUp();
      machine.sendVal(x1, y1, PEN_UP);
      machine.waitForOk();
    }
    doMessage("("+repeats+":"+zoom+":"+count+":"+s+")");
    //println("\tdid jog");

    segDone=false;
    float d = dist(x1, y1, x2, y2);
    if (d>MAXLENGTH) {
      // line length needs to be broken down
      lx=x1;
      ly=y1;
      segs = d/MAXLENGTH;
      s=1;
      Endpoint.x=x2;
      Endpoint.y=y2;
      println("(starting a long line)");
      if (machine.xyToA(x1, y1)>LONGRAD || machine.xyToB(x1, y1)>LONGRAD) {
        println("(we are extended far, dwell for rest time "+RESTTIME+")");
        machine.DirectCommand("G4 P"+RESTTIME);
      }
    } 
    else {
      // line length is ok, draw it
      stroke(0);
      line(x1*onScreenScale, y1*onScreenScale, x2*onScreenScale, y2*onScreenScale);
      //println("(this is a short line)");
      //println("\tdrew short line");
      if (serialOn) {  
        if (doJump) {
          machine.penDown();
          doJump=false;
          //doMessage("(DONE JUMPING!)");
        }

        machine.sendVal(x2, y2, PEN_DOWN);
        machine.waitForOk();
      }

      // signal we drew a line, and we are done
      s=1;
      segs=1;
      segDone=true;

      // store current position
      Endpoint.x=x2;
      Endpoint.y=y2;
      Last.x=x2;
      Last.y=y2;
    }
  }

  if ((s>0) && !segDone) { 
    //    if (printToConsole)
    //      println("("+zoomLevel+":"+x+","+y+":"+count+":"+s+")");
    //    if (writeToFile) {
    //      writer.write("("+zoomLevel+":"+x+","+y+":"+count+":"+s+")\n");
    //      writer.flush();
    //    }

    // drawing a line segment
    stroke(0);
    float tx=lerp(x1, x2, (float)s/segs);
    float ty=lerp(y1, y2, (float)s/segs);

    line(lx*onScreenScale, ly*onScreenScale, tx*onScreenScale, ty*onScreenScale);
    if (serialOn) {  
      if (doJump) {
        machine.penDown();
        doJump=false;
        //doMessage("(DONE JUMPING!)");
      }
      machine.sendVal(tx, ty, PEN_DOWN);
      machine.waitForOk();
    }
    //println("\tdrew segment");
    doMessage("("+repeats+":"+zoom+":"+count+":"+s+")");

    lx=tx;
    ly=ty;

    s++;
  }
}

void drawCrossHairs(float x, float y) {
  float px, py;
  px=x*onScreenScale;
  py=y*onScreenScale;
  //  stroke(128, 0, 0, 128);
  //  line(px-3, py, px+3, py);
  //  line(px, py-3, px, py+3);

  line(px-5, py-5, px+5, py+5);
  line(px+5, py-5, px-5, py+5);
};

void stop() {
  machine.penUp();
  //machine.DirectCommand("G1 X"+HOMEA+" Y"+HOMEB+" Z0.0 F"+FASTFEED);
  machine.DirectCommand("G0 X"+HOMEA+" Y"+HOMEB+" Z0.0");
  machine.stop();
};


// keyboard interface

void keyPressed() {
  if (key == 'g') { 
    // start movement
    On = true;
    if (printToConsole)println("(GO!)");

    machine.DirectCommand("G100 S100"); //moves servo, push pen off page
    machine.waitForOk();
    doJump=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);
      machine.sendVal(MARGIN, MARGIN, 0);
      break;
    case '2':
      // top right
      //machine.sendVal(PAGEW, 0, 0);
      machine.sendVal(PAGEW-MARGIN, MARGIN, 0);
      break;
    case '3':
      // lower right
      //machine.sendVal(PAGEW, PAGEH, 0);
      machine.sendVal(PAGEW-MARGIN, PAGEH-MARGIN, 0);
      break;
    case '4':
      // lower left
      //machine.sendVal(0, PAGEH, 0);
      machine.sendVal(MARGIN, PAGEH-MARGIN, 0);
      break;      
    case '0':
      machine.penUp();
      if (printToConsole)println("(going to stored home position)");
      //machine.DirectCommand("G1 X"+HOMEA+" Y"+HOMEB+" Z0.0 F"+FASTFEED);
      machine.DirectCommand("G0 X"+HOMEA+" Y"+HOMEB+" Z0");
      break;
    case 'd':
      machine.penDown();
      break;
    case 'u':
      machine.penUp();
      break;
    case '-':
      SLOWFEED=SLOWFEED-10.0;
      doMessage("(speed decreased to "+SLOWFEED+")");
      break;
    case '+':
      SLOWFEED=SLOWFEED+10.0;
      doMessage("(speed increased to "+SLOWFEED+")");
      break;  
    default:
      break;
    }
  }
}



// -------- terminal output and logging -------- // 

void doMessage(String message) {
  if (printToConsole)
    println(message);
  if (writeToFile) {
    writer.write(message+"\n");
    writer.flush();
  }
}