// CNCHotwire - Tragflächen Schneidegerät
// Version 1.2
// Datum 15.Mai.14
// Versionskommentar: Es können nun auch senkrechte Vektoren geschnitten werden
//                    Die genauigkeit bei sehr steilen Winkeln wird besser
//                    Dadurch musste das Datenformat bei der seriellen Übertragung zum
//                    Arduino verändert werden. DAs erfordert ArduinoCNC ab Version 1.4.0
// Benötigt Processing 2.x
// Benötigt ArduinoCNC ab 1.4.0
// ----------------------------------------------------------------------------------
// Interface für den PC/Linux
// Gekoppelt über Serielle Schnittstelle mit Arduino
//
// Befehle, die über die Serielle Schnittstelle gesendet werden:
//  '5' - Links Y-Achse nach oben
//  '6' - Links Y-Achse nach unten
//  '7' - Links X-Achse nach vorne
//  '8' - Links X-Achse nach hinten
//  '1' - Rechts Y-Achse nach oben
//  '2' - Rechts Y-Achse nach unten
//  '3' - Rechts X-Achse nach vorne
//  '4' - Rechts X-Achse nach hinten
//  '0' - Stoppt die Bewegung aller Achsen
//  'S' - Startet den Schneidevorgang. Ab dann holt sich der Arduino die Daten ab
//  "<DIR><hex>" - bewegt beide Seiten um <hex> Schritte
//                 <DIR> = 'U'  Aufwärts
//                 <DIR> = 'D'  Abwärts
//                 <DIR> = 'F'  Vorwärts
//                 <DIR> = 'B'  Rückwärts
//                 Beispiel: F00F3
//  "<X/Y><sign><StepHex><sign><XaspectHex><sign>YaspectHex>"
//                 - Übergibt einen Vector an den Arduino. Der Arduino weiß, ob für die rechte
//                   oder linke Seite, weil er den Vector vorher entsprechend angefragt hat. 
//                   Die drei Vorzeichen geben die Richtung an.
//                   Die drei Werte sind unsigned Hex (4 Characters).
//                   StepHex - Anzahl der Schritte in X-Richtung
//                   XAspectHex - Alle wieviel Clocks wird ein X-Motorschritt gemacht.
//                   YAspectHex - Alle wieviel Clocks wird ein Y-Motorschritt gemacht.
//                   Beispiel: +024E+0041-003D
// "##----#----#----" - Keine weiteren Vectoren mehr d.h. Arduino hört auf danach zu fragen.
//
// Befehle, die empfangen werden:
// 'L' - Arduino fordert nächsten Vector für linke Seite an
// 'R' - Arduino fordert nächsten Vector für rechte Seite an
// 'A' - Rechter Endschalter in X-Richtung erreicht
// 'B' - Linker Endschalter in X-Richtung erreicht
// 'a' - Rechter Endschalter in Y-Richtung erreicht
// 'b' - Linker Endschalter in Y-Richtung erreicht

import controlP5.*;
import processing.serial.*;

final int TURNS_PER_MM = 48;  // Anzahl der Schritte pro Umdrehung des Schrittmotors 
final float MOTOR_DIST = 850;  // Abstand der beiden x/y-Führungen voneinander in mm
final int X_STOP = 7;       // Abstand des Endschalters vom Nullpunkt des Styropors
final int Y_STOP = 5;       // Abstand des Endschalters von der Grundplatte

String[] configLines;
String configPath, configSerial;

String FoilLeft, FoilRight;  
String[] profilLinks, profilRechts;
float[] plx, ply,prx,pry;   // Koordinaten für Arduino incl. Tiefe, Anstellwinkel etc.
float[] plxRaw, plyRaw,prxRaw,pryRaw;  // Koordinaten aus Profildatei

int idxLeft, idxRight;
int Heizleistung=70;

float lastXR=0, lastYR=0, lastXL=0, lastYL=0;
float mittlereTiefe;
float tiefeL0, tiefeR0;   // wahre Tiefe an den Motorpositionen (= programmierte Tiefe nur bei Rechteckflächen)

String[] fileList;

ControlP5 controlP5;

Textfield TLinks;
Textfield TRechts;
Textfield AWLinks;
Textfield AWRechts;
Textfield Pfeilung;
Textfield SegBreite;
Textfield xStart;
Textfield yStart;
Textfield Beplankung;

Textlabel ProfNameL;
Textlabel ProfNameR;

ListBox listL, listR;

// The serial port:
Serial myPort;  

void setup() {
  int i;
  size(600,520);
  frameRate(25);
  
  File dataFolder = new File(sketchPath+"/profile_dat");
  fileList = dataFolder.list();
  fileList = sort(fileList);
  
  controlP5 = new ControlP5(this);

  listL=controlP5.addListBox("Profil Links",30,30,120,100);
  for (i=0; i<fileList.length; i++) listL.addItem(fileList[i],i);
  listR=controlP5.addListBox("Profil Rechts",320,30,120,100);
  for (i=0; i<fileList.length; i++) listR.addItem(fileList[i],i);

  TLinks = controlP5.addTextfield("Tiefe Links",30,140,100,20);
  TRechts = controlP5.addTextfield("Tiefe Rechts",320,140,100,20);
  AWLinks = controlP5.addTextfield("Anstellwinkel Links",150,140,100,20);
  AWLinks.setText("0");
  AWRechts = controlP5.addTextfield("Anstellwinkel Rechts",440,140,100,20);
  AWRechts.setText("0");
  Pfeilung = controlP5.addTextfield("Pfeilungswinkel",190,270,80,20);
  Pfeilung.setText("0");
  SegBreite = controlP5.addTextfield("Segmentbreite",190,320,80,20);
  xStart = controlP5.addTextfield("x-Start",310,270,80,20);
  xStart.setText("10");
  yStart = controlP5.addTextfield("y-Start",310,320,80,20);
  yStart.setText("20");
  Beplankung = controlP5.addTextfield("Beplankung",190,370,80,20);
  Beplankung.setText("0");
  controlP5.addSlider("Heizleistung",0,255,70,60,435,160,14).setId(7);
 
  controlP5.addButton("Home",0,30,480,100,19).setId(8);
  controlP5.addButton("Start",0,170,480,100,19).setId(9);
  controlP5.addButton("Heiz-Test",128,380,430,100,20).setId(12);
  controlP5.addButton("Cancel",255,310,480,100,19).setId(10);
  controlP5.addButton("Exit",128,450,480,100,19).setId(11);

  controlP5.addButton("UL",128,65,280,30,30).setId(21);
  controlP5.addButton("FL",128,30,320,30,30).setId(22);
  controlP5.addButton("BL",128,100,320,30,30).setId(23);
  controlP5.addButton("DL",128,65,360,30,30).setId(24);

  controlP5.addButton("UR",128,485,280,30,30).setId(31);
  controlP5.addButton("FR",128,450,320,30,30).setId(32);
  controlP5.addButton("BR",128,520,320,30,30).setId(33);
  controlP5.addButton("DR",128,485,360,30,30).setId(34);

  ProfNameL = controlP5.addTextlabel("labelL","",130,210);
  ProfNameR = controlP5.addTextlabel("labelR","",420,210);

  println("Serial Interfaces found!");
  println(Serial.list());

  configLines = loadStrings("PCHotwire.ini");
  configPath = configLines[0];
  configSerial = configLines[1];

  println("Config File found!");
  println("Serial Interface: "+configSerial);
  println("Profile Path: "+configPath);
  println("sketchPath="+sketchPath);
  
  myPort = new Serial(this, configSerial, 9600);

  background(100);
  fill(100);
  stroke(60);
  rect(10,10,280,240);
  rect(300,10,280,240);  
  rect(10,260,150,150);
  rect(170,260,250,150);
  rect(430,260,150,150);
  rect(10,420,570,40);
}


// Diese Funktion wird automatisch 30x pro Sekunde aufgerufen. Daher sollte das Polling des Ports 
// funktionieren.
// Hier fordert der Arduino jeweils rechts oder links einen neuen Vector an.
void draw() {
   char inByte='0';
   
   if (myPort.available() > 0) {    
      inByte = myPort.readChar();
      print(inByte);             // nur für debug
      
      if (inByte=='L') {  // Linke Seite braucht neuen Vector
         myPort.write(getNextVector('L'));  //Vector über serielle Schnittstelle senden
      }
      
      if (inByte=='R') {  // Rechte Seite braucht neuen Vector
         myPort.write(getNextVector('R'));  //Vector über serielle Schnittstelle senden
      }

   }   
   
   
}

//======================= Support functions ========================

String getNextVector(char side) {
  float x,y,dx,dy;
  float stepsX, stepsY;
  float aspectX, aspectY;
  float tiefe;
  float speedFactor;
  int aspX, aspY, stp;
  String outStr, aspXhex, aspYhex, stpHex;
  char aspXsign, aspYsign, stpSign;
  int ende=0;
  int linDist;
  char xySelect;   // Neu: Gibt an, ob relativ zur X- oder Y-Achse gerechnet wird 

  if (side=='L') {
    tiefe = tiefeL0;  // wahre Tiefe an der linken Motorposition 
    x=plx[idxLeft];    
    y=ply[idxLeft];
    dx=(x-lastXL);
    dy=(y-lastYL);
    lastXL=x;
    lastYL=y;
    if (idxLeft < (plx.length-1)) idxLeft++; else ende=1;
    
  } else {
    tiefe= tiefeR0;  // wahre Tiefe an der rechten Motorposition
    x=prx[idxRight];  
    y=pry[idxRight];
    dx=(x-lastXR);
    dy=(y-lastYR);
    lastXR=x;
    lastYR=y;
    if (idxRight < prx.length-1) idxRight++; else ende=1;
  }  

  stepsX=dx*TURNS_PER_MM;
  stepsY=dy*TURNS_PER_MM;
  linDist = int(sqrt(stepsX*stepsX + stepsY*stepsY));     // Vektorlänge in steps (Pytagoras)
  
  // Je größer die Tiefe desto kleiner der Abstand zwischen zwei Schritten
  // Der SpeedFactor ist für x und y gleich aber unterscheidet sich bei Trapezflächen zwischen rechts und links 
  speedFactor = mittlereTiefe / tiefe;    
  
  //Ursrünglich wurde hier mit 20 multipliziert. Nun wird mit 50 multipliziert, was die Genauigkeit um den
  // Faktor 2.5 verbessert. Auf der Arduino Seite muss daher der Delay verkelinert werden, damit die Fahr-
  // geschwindigkeit nicht auch um den Faktor 2.5 heruntergeht. 
  if (stepsX!=0) aspectX=speedFactor*linDist*50/stepsX; else aspectX=linDist*100;   // Berechnung der cycles pro Schritt 
  if (stepsY!=0) aspectY=speedFactor*linDist*50/stepsY; else aspectY=linDist*100;

  if (abs(aspectX)>32000) aspectX=32000;   // Sicherstellen, dass es keinen Überlauf gibt
  if (abs(aspectY)>32000) aspectY=32000; 
  
  aspX= int(aspectX);
  aspY= int(aspectY);
  
  //Abhängig davon, ob mehr X- oder Y-Schritte gebraucht werden, wird die Referenz für die Total-Steps ausgewählt
  if (abs(stepsX)>abs(stepsY)) {
    stp= int(stepsX);
    xySelect = 'X';
  }
  else {
    stp = int(stepsY);
    xySelect = 'Y';
  } 
  
  if (stp>=0) stpSign='+'; else stpSign='-';
  if (aspX>=0) aspXsign='+'; else aspXsign='-';
  if (aspY>=0) aspYsign='+'; else aspYsign='-';
  
  println(ende+" # "+idxLeft+"-"+plx.length+" | "+idxRight+"-"+prx.length+" # "+y+" * "+stp+" - "+aspX+" * "+aspY+" * "+xySelect);
  
  stpHex = hex(abs(stp));
  aspXhex = hex(abs(aspX));
  aspYhex = hex(abs(aspY));

  // Neu seit 1.5.0: xySelect wird am Anfang ausgegeben, damit der Arduino weiß, auf welche Achse sich die Steps beziehen
  outStr= stpSign + stpHex.substring(4) +  aspXsign + aspXhex.substring(4) + aspYsign + aspYhex.substring(4);
  outStr = xySelect + outStr; 
  println("--> "+outStr);

  if (ende==0) return(outStr); else return("##----#----#----");
}

// Verfährt beide Seiten um distance in mm
void drive(char axis, int distance) { 
  String outStr, distHex;
  char sign;
  
  if (distance>0) sign='+'; else sign='-';
  distHex = hex(abs(distance*TURNS_PER_MM));
  outStr = axis + sign + distHex.substring(4);
  println("Drive: "+outStr);
  myPort.write(outStr);
}

void applySettings()  // Berechne Koordinaten aus Raw-Koordinaten
{
   int i;
   float tiefeL, tiefeR;
   float alphaOrg, alphaNew, deltaAlpha, len;
   float alphaSweep;
   float segmentLength, scaleL, scaleR;
   float d, dir, x1, x2, y1, y2;
   
   int lastx=0, lasty=0; 
   
   plx = new float [plxRaw.length];
   ply = new float [plyRaw.length];
   prx = new float [prxRaw.length];
   pry = new float [pryRaw.length];
         
   //=================== Ab hier wird die Tiefe verrechnet ========================
   tiefeL= float(TLinks.getText());
   for (i=0; i<plxRaw.length; i++) {
     plx[i]=(1-plxRaw[i])*tiefeL;
     ply[i]=plyRaw[i]*tiefeL;
   }     
   tiefeR= float(TRechts.getText());
   for (i=0; i<prxRaw.length; i++) {
     prx[i]=(1-prxRaw[i])*tiefeR;
     pry[i]=pryRaw[i]*tiefeR;
   }     
   
   //========== mittlere Tiefe um später die relativen Geschwindigkeiten zu berechnen =======
   mittlereTiefe = (tiefeL + tiefeR) /2;
   
   //================= Ab hier wird der Anstellwinkel verrechnet
   deltaAlpha = float(AWLinks.getText()); 
   for (i=0; i<plx.length; i++) {   
     len = sqrt(sq(plx[i]) + sq(ply[i]));   // Abstand von der Endleiste zum Koordinatenpaar;
     if (len!=0) alphaOrg = asin(ply[i]/len) / PI * 180; else alphaOrg=0;   // Division durch Null vermeiden
     alphaNew = alphaOrg + deltaAlpha; 
     plx[i] = len * cos(alphaNew / 180 * PI);
     ply[i] = len * sin(alphaNew / 180 * PI); 
     // stroke(70);
     // line(280-lastx,220-lasty,280-int(plx[i]),220-int(ply[i]));
     //     lastx=int(plx[i]); lasty=int(ply[i]);
   }  
   
   deltaAlpha = float(AWRechts.getText());
   for (i=0; i<prx.length; i++) {
     len = sqrt(sq(prx[i]) + sq(pry[i]));   // Abstand von der Endleiste zum Koordinatenpaar;
     if (len!=0) alphaOrg = asin(pry[i]/len) / PI * 180; else alphaOrg=0;
     alphaNew = alphaOrg + deltaAlpha; 
     prx[i] = len * cos(alphaNew / 180 * PI);
     pry[i] = len * sin(alphaNew / 180 * PI); 
     // stroke(70);
     // line(570-lastx,220-lasty,570-int(prx[i]),220-int(pry[i]));
     // lastx=int(prx[i]); lasty=int(pry[i]);
   }   
   
//============ Ab hier wird der Pfeilungswinkel verrechnet (= Stauchung in x-Richtung) ================
   alphaSweep = float(Pfeilung.getText());
   for (i=0; i<plx.length; i++) {
     plx[i]= plx[i] * cos(alphaSweep / 180 * PI);
     // stroke(70);
     // line(280-lastx,220-lasty,280-int(plx[i]),220-int(ply[i]));
     // lastx=int(plx[i]); lasty=int(ply[i]);
   }  
   for (i=0; i<prx.length; i++) {
     prx[i]= prx[i] * cos(alphaSweep / 180 * PI);
     // stroke(70);
     // line(570-lastx,220-lasty,570-int(prx[i]),220-int(pry[i]));
     // lastx=int(prx[i]); lasty=int(pry[i]);
   }  
   
   //========= Ab hier wird die Segmentlänge verrechnet um die tatsächliche X/Y-Bewegung zu erhalten =========
   segmentLength = float(SegBreite.getText());
   tiefeL0 = tiefeL + (MOTOR_DIST-segmentLength) * (tiefeL - tiefeR) / 2 / segmentLength;
   tiefeR0 = tiefeR + + (MOTOR_DIST-segmentLength) * (tiefeR - tiefeL) / 2 / segmentLength;
   scaleL = tiefeL0/tiefeL;
   scaleR = tiefeR0/tiefeR;
   
   for (i=0; i<plx.length; i++) {
     plx[i] = plx[i] * scaleL;  
     ply[i] = ply[i] * scaleL; 
     // stroke(70);
     // line(280-lastx,220-lasty,280-int(plx[i]),220-int(ply[i]));
     // lastx=int(plx[i]); lasty=int(ply[i]);
   }   
   for (i=0; i<prx.length; i++) {
     prx[i] = prx[i] * scaleR;
     pry[i] = pry[i] * scaleR;
     // stroke(70);
     // line(570-lastx,220-lasty,570-int(prx[i]),220-int(pry[i]));
     // lastx=int(prx[i]); lasty=int(pry[i]);
   }   
   
   //=================================== Beplankungsabzug ==========================================
   d = float(Beplankung.getText());
   dir = 0; alphaNew=PI/2; 
   for (i=0; i<plx.length-1; i++) {
      x1=plx[i]; x2=plx[i+1];
      y1=ply[i]; y2=ply[i+1];
      alphaOrg = sin((y1-y2) / sqrt(sq(y1-y2)+sq(x1-x2)));
      alphaNew = alphaOrg-PI/2;   //90 Grad drehen um Senkrechte auf der Linie zu erhalten
      if (x1-x2>=0) dir=1; else dir=-1;
      plx[i] = plx[i] - d * cos(alphaNew);
      ply[i] = ply[i] - d * dir * sin(alphaNew);
      stroke(170);
      line(280-lastx,220-lasty,280-int(plx[i]),220-int(ply[i]));
      lastx=int(plx[i]); lasty=int(ply[i]);
   }
   i = plx.length-1;
   plx[i] = plx[i] - d * cos(alphaNew);    // für den letzten Punkt extra ausführen 
   ply[i] = ply[i] - d * dir * sin(alphaNew);

   for (i=0; i<prx.length-1; i++) {
      x1=prx[i]; x2=prx[i+1];
      y1=pry[i]; y2=pry[i+1];
      alphaOrg = sin((y1-y2) / sqrt(sq(y1-y2)+sq(x1-x2)));
      alphaNew = alphaOrg-PI/2;   //90 Grad drehen um Senkrechte auf der Linie zu erhalten
      if (x1-x2>=0) dir=1; else dir=-1;
      prx[i] = prx[i] - d * cos(alphaNew);
      pry[i] = pry[i] - d * dir * sin(alphaNew);
      stroke(170);
      line(570-lastx,220-lasty,570-int(prx[i]),220-int(pry[i]));
      lastx=int(prx[i]); lasty=int(pry[i]);
   }
   i = prx.length-1;
   prx[i] = prx[i] - d * cos(alphaNew);    // für den letzten Punkt extra ausführen 
   pry[i] = pry[i] - d * dir * sin(alphaNew);    
}

public void homeWire() {
int xr, xl, yr, yl;
int xDist, yDist;
char inByte;
String hexNumber;

   xr=0; xl=0;
   myPort.write('8');    // Die beiden x-Achsen werden rückwärts bewegt
   myPort.write('4');
   do {
      if (myPort.available() > 0) { 
      inByte=myPort.readChar();
        if (inByte=='A') xr=1;
        if (inByte=='B') xl=1;
      }  
   } while (xr*xl==0);   // Ende, wenn beide Seiten am Anschlag sind
   yr=0; yl=0;
   myPort.write('6');    // Die beiden y-Achsen werden nach unten bewegt
   myPort.write('2');
   do {
      if (myPort.available() > 0) { 
        inByte=myPort.readChar();
        if (inByte=='a') yr=1;
        if (inByte=='b') yl=1;
      }  
   } while (yr*yl==0);    // Ende, wenn beide Seiten am Anschlag sind
       
   yDist = int(yStart.getText());      // Draht auf die y-Startposition fahren
   hexNumber=hex((yDist-Y_STOP)*TURNS_PER_MM);   
   println("Schritte fahren: "+'U'+hexNumber.substring(4));
   myPort.write('U'+hexNumber.substring(4));

   hexNumber=hex(X_STOP*TURNS_PER_MM);  // Draht an den Anfang des Styroblocks fahren 
   println("Schritte fahren: "+'F'+hexNumber.substring(4));
   myPort.write('F'+hexNumber.substring(4));
}

// Hier werden die GUI Events verarbeitet

public void controlEvent(ControlEvent theEvent) {
int i;
String profil;
String hexNumber;

  if (theEvent.isGroup()) {
      println(theEvent.group().value()+" from "+theEvent.group());
      i = int(theEvent.group().value());
      profil=fileList[i];
      if (theEvent.group().name()=="Profil Links") loadFoil('L',profil);
      if (theEvent.group().name()=="Profil Rechts") loadFoil('R',profil);
  } else 
  {  
    println("got a control event from controller with id "+theEvent.controller().id());
    switch(theEvent.controller().id()) {
    case(8): // Home
      println("Home");
      homeWire();
    break;
    case(9): // Start
      println("Start");
      applySettings();
      hexNumber = hex(Heizleistung); 
      myPort.write('H'+hexNumber.substring(6));   // Heizdraht einschalten
      delay(500);  // Warten bis aufgeheizt
      myPort.write('S');   // Der Arduino wird gestartet und holt sich dann selbst immer 
                           // wieder neue Vektoren ab (siehe Funktion "draw")  
    break;
    case(10): // Cancel
      println("Cancel");
      myPort.write('0');
    break;
    case(11): // Exit
      println("Exit");
      exit();
    break;
    case(12): // Start Heizung
      hexNumber = hex(Heizleistung); 
      println("Heizung an:"+'H'+hexNumber.substring(6));
      myPort.write('H'+hexNumber.substring(6));
    break;
    case(21):
      println("UL");
      myPort.write('5');
    break;
    case(22):
      println("FL");
      myPort.write('7');
    break;
    case(23):
      println("BL");
      myPort.write('8');
    break;
    case(24):
      println("DL");
      myPort.write('6');
    break;
    case(31):
      println("UR");
      myPort.write('1');
    break;
    case(32):
      println("FR");
      myPort.write('3');
    break;
    case(33):
      println("BR");
      myPort.write('4');
    break;
    case(34):
      println("DR");
      myPort.write('2');
    break;
    }
  }
}

public void loadFoil(char id, String profil) {
  int i;
  float x,y,lastx=0,lasty=0;
  String[] pieces;
  
  if (id=='L') {
    profilLinks = loadStrings(sketchPath+"/profile_dat/"+profil);
    plxRaw = new float [profilLinks.length];
    plyRaw = new float [profilLinks.length];
    
    fill(100);
    stroke(60);
    rect(10,10,280,230);
    ProfNameL.setValue(profil);

    for (i=1; i<profilLinks.length; i++) {
      pieces = splitTokens(profilLinks[i], " ");  
      x = float(pieces[0]);
      y = float(pieces[1]);
      plxRaw[i-1] = x;
      plyRaw[i-1] = y;
      stroke(255);
      if (i==1) {lastx=x*250; lasty=y*250;};
      line(30+lastx,220-lasty,30+x*250,220-y*250);
      lastx=x*250; lasty=y*250;
    }
  }  
  
  if (id=='R') {
    profilRechts = loadStrings(sketchPath+"/profile_dat/"+profil);
    prxRaw = new float [profilRechts.length];
    pryRaw = new float [profilRechts.length];

    fill(100);
    stroke(60);
    rect(300,10,280,230); 
    ProfNameR.setValue(profil);
    
    for (i=1; i<profilRechts.length; i++) {
      pieces = splitTokens(profilRechts[i], " ");  
      x = float(pieces[0]);
      y = float(pieces[1]);
      prxRaw[i-1] = x;
      pryRaw[i-1] = y;
      stroke(255);
      if (i==1) {lastx=x*250; lasty=y*250;};
      line(320+lastx,220-lasty,320+x*250,220-y*250);
      lastx=x*250; lasty=y*250;
    }  
  }
  idxLeft=1;   // =1 damit der erste Wert übersprungen wird. Der ist immer 1.0  0.0
  idxRight=1;
  lastXR=0; lastYR=0; lastXL=0; lastYL=0;
  println(pry);
}  

