//
// Recorder
//
// handles encoder input from suspended pulley pen mount with dual (A/B) encoders

// handles reading serial input from recorder

void serialEvent(Serial myPort) { 
  if (myPort==rec.getPort()) {

    // get the ASCII string:
    String inString = myPort.readStringUntil('\n');

    if (inString != null) {
      // trim off any whitespace:
      inString = trim(inString);
      // split the string on the commas and convert the 
      // resulting substrings into an integer array:
      float[] coords = float(split(inString, ","));
      // if the array has at least three elements, you know
      // you got the whole thing.  Put the numbers in the
      // color variables:
      if (coords.length >=2) {
        // map them to the range 0-255:
        //        redValue = map(colors[0], 0, 1023, 0, 255);
        //        greenValue = map(colors[1], 0, 1023, 0, 255);
        //        blueValue = map(colors[2], 0, 1023, 0, 255);
        rec.update(coords[0], coords[1]);
      }
    }
  }
}

// recorder class

class Recorder extends PApplet {

  Serial myPort; //declare serial port
  int prevZ = -1; 
  int LF = 10;    // Linefeed i  n ASCII
  int CR = 13;

  String myString = null;
  boolean writeToFile=true;

  PrintWriter writer;

  PVector Current;
  PVector Last;

  float PPI=800.0;

  // rec geometry
  float ENCODERSEP;// = 96.0;//72.0; // how far apart are the two steppers
  float HOMEA;// = 86.5;//81.0;//82.0;// 58.5225650924496; // 108
  float HOMEB;// = 86.5;//81.0;//82.0;//56.02580320709378; // 108

  // page setup
  float PAGEW;// = 13.0;//36.0;//17.0;//22.0;//44.0;  // width
  float PAGEH;// = 17.0;//24.0;//14.0;//30;//30.0;  // height
  float PAGE0Y;// = 32.0;//6.0-PAGEH;
  float PAGE0X;// = (MOTORSEP-PAGEW)/2.0;  // upper left

  Recorder(PApplet p, String portName, int portBaud) { //constructor

    if (serialOn) {
      if (printToConsole)println(Serial.list());
      //println(Serial.list());
      //String portName = Serial.list()[4];  // Change this Value to fit which Serial Port it is on.
      myPort = new Serial(p, portName, portBaud);
    }
    if (writeToFile) {
    }

    Current = new PVector(0, 0, 0);
    Last = new PVector(0, 0, 0);
  }

  Serial getPort() {
    return myPort;
  }
  
  void setGeometry(float encoderSep, float homeA, float homeB) {
    ENCODERSEP=encoderSep;
    HOMEA=homeA;
    HOMEB=homeB;
  }

  float getHomeA() {
    return HOMEA;
  }

  float getHomeB() {
    return HOMEB;
  }

  void setScale(float inPPI) {
    PPI = inPPI;
  }

  void setupWriting(PrintWriter _writer) {
    writer=_writer;
  }

  void stop() {
    if (writeToFile) {
      writer.flush(); // Write the remaining data
      writer.close(); // Finish the file
    }
  }

  void doZero() {
    String j = " ";
    if (printToConsole) {
      print("(zeroed encoders)");
      //println(j);
    }

    if (serialOn) 
      myPort.write(j+'\n');
  }

  float getXpos() {
    return Current.x;
  }

  float getYpos() {
    return Current.y;
  }

  float getLastXpos() {
    return Last.x;
  }

  float getLastYpos() {
    return Last.y;
  }

  void update(float inA, float inB) {
    float x, y;
    float rA, rB;
    rA=HOMEA+(inA/PPI);
    rB=HOMEB+(inB/PPI);
    x = (((pow(rA, 2) - pow(rB, 2))/ENCODERSEP) + ENCODERSEP) / 2;
    y = sqrt(pow(rA, 2) - pow(x, 2));
    Last.x=Current.x;
    Last.y=Current.y;
    Current.x=x;
    Current.y=y;
  }



  void waitForOk() {
    if (serialOn) {
      boolean okd=false;
      //print("waiting...");
      while (!okd) {
        myString = myPort.readStringUntil(LF);
        if (myString != null) {
          //print("RX: "+myString);
          if (myString.startsWith("ok")) {
            okd = true; // when drawRecorder has parsed a command it sends "ok"
          } 
          else {
            //println("received something not ok");
          }
        } 
        else {
          //print(".");
        }
      }
      //println("done.");
    }
  }

  void waitForStart() {
    String startString;
    // GRBL
    startString="ok";
    // gcode-firmware
    //startString="start";

    println("waiting for start.");
    boolean started=false;
    if (serialOn) {
      while (true) {
        if (myPort.available() > 0)
          break;
      }
      println("reading.");
      while ( (myPort.available () > 0) && (!started)) {
        myString = myPort.readStringUntil(LF);
        if (myString != null) {
          print("RX: "+myString);
          if (myString.startsWith(startString)) {
            started = true; // when drawRecorder stops at point, it sends "ok" and triggers atPoint
          } 
          else {
            println("received something not ok");
          }
        }
      }
    }
  }


  //  void sendVal(float inx, float iny, float upDown) { 
  //    String xsend;
  //    String ysend;
  //    String feedRate;
  //    String xy;
  //
  //    // calc A and B string lengths from x,y coords (in inches)
  //    xsend=str(xyToA(inx, iny)*INCH_TO_MM);
  //    ysend=str(xyToB(inx, iny)*INCH_TO_MM);
  //
  //    feedRate="";
  //
  //    if (upDown==PEN_DOWN)
  //      //feedRate="F"+SLOWFEED; // pen is on, drawing
  //      DirectCommand("G1 X"+xsend+" Y"+ysend+" F"+SLOWFEED);
  //    else
  //      //feedRate="F"+FASTFEED; // pen is off, jogging
  //      DirectCommand("G0 X"+xsend+" Y"+ysend);
  //
  //
  //    //DirectCommand("G1 X"+xsend+" Y"+ysend+" "+feedRate);
  //
  //    atPoint=false;
  //  }

  //  void DirectCommand(String j) {
  //    if (printToConsole)
  //      println(j);
  //    //print(j);
  //    if (serialOn) { 
  //      myPort.write(j+'\n');
  //      //println("sent: "+j);
  //    } 
  //    if (writeToFile) {
  //      //println("writing "+j);
  //      //println(j);
  //      writer.write(j+"\n");
  //
  //      writer.flush();
  //    }
  //  }

  //  void penDown() {
  //    // gcode-firmware
  //    //DirectCommand("G100 S130"); //moves servo, push pen on page
  //    
  //    // GRBL
  //    DirectCommand("M3");
  //    waitForOk();
  //  }
  //
  //  void penUp() {
  //    // gcode-firmware
  //    //DirectCommand("G100 S150"); //moves servo, lift pen off page
  //    
  //    // GRBL
  //    DirectCommand("M5");
  //    waitForOk();
  //  }

  //  void storeHome() {
  //    if (printToConsole) println("(switching machine to ABSOLUTE positioning)");
  //    DirectCommand("G90");
  //    if (serialOn) waitForOk();
  //
  //    if (printToConsole)println("(storing current position as home position)");
  //    DirectCommand("G92 X"+HOMEA+" Y"+HOMEB);
  //    if (serialOn) waitForOk();
  //
  //    DirectCommand("M3"); // pen down
  //    if (serialOn) waitForOk();
  //  }
  //
  //  void setInches() {
  //    if (printToConsole) println("(setting machine to inches)");
  //    DirectCommand("G20");
  //    if (serialOn) waitForOk();
  //  }
  //
  //  void goHome() {
  //    if (printToConsole)println("(going to stored home position)");
  //    //DirectCommand("G1 X"+HOMEA+" Y"+HOMEB+" Z0.0 F"+FASTFEED);
  //    DirectCommand("G0 X"+HOMEA+" Y"+HOMEB+" Z0.0");
  //  }
}