// slightly cleaned up from harvey moon's drawing machine
//
// gridding off space available in the image plane
// repeatibility
// accuracy
// precision
//
// 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

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

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

// offsets
// line A, at limit switch, is 2.0 inches out. 
// line B, at limit switch, is 1.25 inches out. 

// paper size
float PAGEW = 84.0;//36.0;//17.0;//22.0;//44.0;  // width
float PAGEH = 60.0;//14.0;//30;//30.0;  // height
float MARGIN = 2.0;

// page location relative to motors
float PAGE0X = (MOTORSEP-PAGEW)/2.0;  // upper left
float PAGE0Y = 6.0;//6.0-PAGEH;  // top of paper //12.0;//40.0;

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

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

// feed rates
float FASTFEED = 200.0;//200.0;
float SLOWFEED = 100.0;//15.0;

static int PEN_UP = 0;
static int PEN_DOWN = 1;

// limit of the process
int MAXDEPTH=4;

int PATTERN_A = 1;
int PATTERN_B= 2;
int PATTERN_C = 3;

//-----------------------------------
// 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 jumping = true;
boolean atPoint = true; // if the arduino returns that it is at the point, turn true.
boolean atHome=true;

int scale=0;
int x=0;
int y=0;
int state=PATTERN_A;
PVector Last, Current;

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

  // 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);

  // 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();
  Current = new PVector(MOTORSEP/2, sqrt(81*81-(MOTORSEP/2*MOTORSEP/2)), 0);
  println("(at "+Current.x+","+Current.y+" relative to PAGE(0,0))");
 
  noCursor();
}


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

  if (On && !done) {
    drawGridAtScale(MARGIN, MARGIN, PAGEW-MARGIN, PAGEH-MARGIN, int(pow(2, scale)));
    if (incrementPosition(int(pow(2, scale))))
    {
      scale++;

      if(printToConsole)
        println("(grid "+scale+":"+MAXDEPTH+")");
        
      if (scale>MAXDEPTH) {
        drawBox(MARGIN, MARGIN, PAGEW-MARGIN, PAGEH-MARGIN);
        done=true;
      }
    }
  }
}

boolean incrementPosition(int scale) {
  x++;  
  if (x>=scale) {
    x=0;
    y++;
    
    if(printToConsole)
        println("(line "+y+":"+scale+")");

    if (y>=scale) {
      y=0;
      return true;
    }
  } else {
    if(state==PATTERN_B) {
      state=PATTERN_C; 
    } else {
      state=PATTERN_B;
    }
  }
  return false;
}

void drawGridAtScale(float left, float top, float right, float bot, int scale) {
  float ux=(right-left)/(float)scale;
  float uy=(bot-top)/(float)scale;

//  print(ux);
//  print(" ");
//  println(uy);

  //  for(int x=0;x<scale;x++) {
  //    for(int y=0;y<scale;y++) {
  
  if(state!=PATTERN_B)  
    drawScaledSegment(left+ux*x, top+uy*y, left+ux*(x+1), top+uy*(y+1));
  if(state!=PATTERN_C)
    drawScaledSegment(left+ux*x, top+uy*(y+1), left+ux*(x+1), top+uy*y);
  drawScaledSegment(left+ux*x, top+uy*(y+0.5), left+ux*(x+1), top+uy*(y+0.5));
  drawScaledSegment(left+ux*(x+0.5), top+uy*y, left+ux*(x+0.5), top+uy*(y+1));

  if(state==PATTERN_A)
    state=PATTERN_B;
    
  //      print("*");
  //      print(x);
  //      print(" ");
  //      println(y);
  delay(100);

  //    }
  //  }
}

void drawBox(float left, float top, float right, float bot) {
  drawScaledSegment(left, top, right, top);
  drawScaledSegment(right, top, right, bot);
  drawScaledSegment(right, bot, left, bot);
  drawScaledSegment(left, bot, left, top);
}

// 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();
    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();
      if (printToConsole)println("(going to stored home position)");
      machine.DirectCommand("G1 X"+HOMEA+" Y"+HOMEB+" Z0.0 F"+FASTFEED);
      break;
    case 'd':
      machine.penDown();
      break;
    case 'u':
      machine.penUp();
      break;
    default:
      break;
    }
  }
}

void drawScaledSegment(float x1, float y1, float x2, float y2) {
  Last=Current;
  
  machine.penUp();
  machine.sendVal(x1, y1, PEN_UP);
  
  stroke(240,240);
  line(Last.x*onScreenScale, Last.y*onScreenScale, x1*onScreenScale, y1*onScreenScale);
  
  machine.penDown();
  machine.sendVal(x2, y2, PEN_DOWN);
  
  stroke(0);
  line(x1*onScreenScale, y1*onScreenScale, x2*onScreenScale, y2*onScreenScale);

  Current.x=x2;
  Current.y=y2;
};

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);

  stroke(192, 0, 0, 128);
  line(px-1, py, px+1, py);
  line(px, py-1, px, py+1);
};

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