  *  =======================================================================================
  */
 
+//int hotheadtarget = 160;    //160 DEFAULT
+int hotheadtarget = 10;    //160 DEFAULT
+
 //physical movement scale (bigger == more);
-double tablexsteps    = 13000;     
-double tableysteps    = 13000;
-double tablezsteps    = 100000;    //257142 (100mm) from top (STEPS FROM TABLE SURFACE TO TOP) 0,0,0 is FAR LEFT BOTTOM
-double tableesteps  = -3600;
+double tablexsteps    = 23000;     
+double tableysteps    = 33000;
+double tablezsteps    = 203205;    //234000 (100mm) from top (STEPS FROM TABLE SURFACE TO TOP) 0,0,0 is FAR LEFT BOTTOM
+double tableesteps  = -1000000;
 
 //measured (1000 = 1mm)
-double tablexdistance = 250000;    //in microns (158mm )
-double tableydistance = 250000;    //in microns (200mm)
-double tablezdistance = 96000;    //in microns (96mm)
-double tableedistance = 1000;    //in microns (96mm)
-
-double maxmotorSpeed = 12600;
-double motorSpeed = 12600;                                           //default 9600
-double motorAccel = 160000; //steps/second/second to accelerate      //default 80000
+double tablexdistance = 200.000;    //in microns (158mm )
+double tableydistance = 200.000;    //in microns (200mm)
+double tablezdistance = 70.000;    //in microns (96mm)
+double tableedistance = 310.000;    //in microns (96mm)
+
 
 // Motor pins
-int motorpinXstep = 7;
-int motorpinXdir  = 6;
-int motorpinYstep = 9;
-int motorpinYdir  = 8;
-int motorpinZstep = 13;
-int motorpinZdir  = 12;
-int motorpinEstep = 11;
-int motorpinEdir  = 10;
+// step = pin for step on controller
+// dir  = pin for direction on controller
+// speed is minimum delay between pulses in microseconds
+int motorpinXstep = 24;
+int motorpinXdir  = 25;
+double stepperXcurrPosition = 0;
+double stepperXdestPosition = 0;
+double stepperXtimer = 0;
+int    stepperXtoggle = 0;
+double stepperXspeed = 60;
+
+int motorpinYstep = 26;
+int motorpinYdir  = 27;
+double stepperYcurrPosition = 0;
+double stepperYdestPosition = 0;
+double stepperYtimer = 0;
+int    stepperYtoggle = 0;
+double stepperYspeed = 120;  //slower for more power
+
+int motorpinZstep = 30;
+int motorpinZdir  = 31;
+double stepperZcurrPosition = 0;
+double stepperZdestPosition = 0;
+double stepperZtimer = 0;
+int    stepperZtoggle = 0;
+double stepperZspeed = 60;
+
+int motorpinEstep = 28;
+int motorpinEdir  = 29;
+double stepperEcurrPosition = 0;
+double stepperEdestPosition = 0;
+double stepperEtimer = 0;
+int    stepperEtoggle = 0;
+double stepperEspeed = 120;
+
 
 // Motor Zero switch pins
-int xZeropin = 5;        //INPUT pin
-int yZeropin = 4;        //INPUT pin
-int zZeropin = 3;        //INPUT pin
-
-int xZeroNC  =  1;        //Normally closed/open
-int yZeroNC  =  1;          //Normally closed/open
-int zZeroNC  =  0;          //Normally closed/open
+int xZeropin = 50;        //INPUT pin
+int yZeropin = 51;        //INPUT pin
+int zZeropin = 52;        //INPUT pin
+
+//thermo control
+int thermoswitch = 2;    //output switch for turning on melter
+int thermo = A0;         //input for reading temperature of melter
+
+//NC/NO toggles
+int xZeroNC  =  1;       //Normally closed/open
+int yZeroNC  =  1;       //Normally closed/open
+int zZeroNC  =  0;       //Normally closed/open
+
+
 
 // =========================================================================================
-
+// INTERNAL DO NOT CHANGE BELOW
 //latest position in microns (1000 = 1mm)
-double lastx = tablexdistance/2;
-double lasty = tableydistance/2;
+
+double lastx = 0;
+double lasty = 0;
 double lastz = 0;
 double laste = 0;
 
 
-
-
 // =========================================================================================
 
-AccelStepper stepper5(1, motorpinXstep, motorpinXdir);          //CROSSBEAM      X AXIS
-AccelStepper stepper4(1, motorpinYstep, motorpinYdir);          //TABLE IN/OUT   Y AXIS
-AccelStepper stepper1(1, motorpinZstep, motorpinZdir);          //UP DOWN        Z AXIS
-AccelStepper stepper2(1, motorpinEstep, motorpinEdir);          //EXSTRUDER FEED E AXIS
 
 void setup() {
+  analogReadResolution(16);
+  analogWriteResolution(16);
   Serial.begin(115200);
   Serial.print("{ \"log\" : \"Connecting...\"}");
   //ZERO SWITCHES

      Runs each motor towards the zero switch and resets its position. Must be run at start.
  */

///////////////////////////////////////////////////////ZERO X OVERHANG SIDE TO SIDE     
    
    //stepper5.setSpeed(6400);
    //stepper5.setAcceleration(999999);          
    
    while (digitalRead(xZeropin) != xZeroNC) {   
      stepper5.moveTo(stepper5.currentPosition()-10);      //change to +4 for zero on opposite side      
      stepper5.run();      
    }
    
    stepper5.stop();
    stepper5.setCurrentPosition(-1000);
    stepper5.setSpeed(motorSpeed);   
    stepper5.setAcceleration(motorAccel); 
    stepper5.moveTo(7000);
    while (stepper5.distanceToGo() != 0) {
      stepper5.run(); 
    }    

  
    ///////////////////////////////////////////////////////ZERO Y TABLE
    stepper4.setSpeed(6400);
    stepper4.setAcceleration(999999);       
    while (digitalRead(yZeropin) != yZeroNC) {   
      stepper4.moveTo(stepper4.currentPosition()+10);            
      stepper4.run();      
    }
    delay(100);
    stepper4.stop();
    stepper4.setCurrentPosition(tableysteps+1000);
    stepper4.moveTo(0);
    while (stepper4.distanceToGo() != 0) {
      stepper4.run(); 
    }
    ////////////////////////////////////////////////////////ZERO Z - UPDOWN
    stepper1.setSpeed(8000);
    stepper1.setAcceleration(999999);          

    while (digitalRead(zZeropin) != zZeroNC) {   
      stepper1.moveTo(stepper1.currentPosition()+320);          
      stepper1.run();
    }
    stepper1.stop();
    stepper1.setCurrentPosition(tablezsteps);
    stepper1.setSpeed(motorSpeed);   
    stepper1.setAcceleration(motorAccel);     
    stepper1.moveTo(0);
    while (stepper1.distanceToGo() != 0) {
      stepper1.run(); 
    }    

    //////////////////////////////////////////////////////////E zero - feeder    
    stepper2.setSpeed(6400);
    stepper2.setAcceleration(999999);
    stepper2.moveTo(0);
    while (stepper2.distanceToGo() != 0) {
      stepper2.runSpeedToPosition(); 
    }
  
}
// END ZEROAXIS()

// ==============================================================================================================================================================================
// START TESTAXIS()
void testaxis() {
  /* 
    Runs all axis to extremes and also gives extruder a test spin 
  */
    //HIGH
    stepper5.moveTo(tablexsteps);
    stepper4.moveTo(tableysteps);    
    stepper1.moveTo(tablezsteps);
    stepper2.moveTo(tableesteps); 
    while ((stepper5.distanceToGo() != 0) || (stepper4.distanceToGo() != 0) || (stepper1.distanceToGo() != 0) || (stepper2.distanceToGo() != 0)) {
      stepper5.run();
      stepper4.run();
      stepper1.run();
      stepper2.run();
    }  
    //ZERO
    stepper5.moveTo(0);
    stepper4.moveTo(0);    
    stepper1.moveTo(0);
    stepper2.moveTo(0); 
    while ((stepper5.distanceToGo() != 0) || (stepper4.distanceToGo() != 0) || (stepper1.distanceToGo() != 0) || (stepper2.distanceToGo() != 0)) {
      stepper5.run();
      stepper4.run();
      stepper1.run();
      stepper2.run();
    }      
}
// END TESTAXIS();

// ==============================================================================================================================================================================

// START RAPID()
void rapid(int x, int y, int z, int e) {
  /* 
    Allows you to position the printer and control the feeder
    dimensions are in microns. 1000 = 1mm
    Moves proportionally so all motors arrive at destination at the same time.
  */
    double nx = tablexsteps*x/tablexdistance;
    double ny = tableysteps*y/tableydistance;    
    
    stepper5.moveTo(nx);
    stepper4.moveTo(ny);    
    stepper1.moveTo(z);
    stepper2.moveTo(e); 

    while ((stepper5.distanceToGo() != 0) || (stepper4.distanceToGo() != 0) || (stepper1.distanceToGo() != 0) || (stepper2.distanceToGo() != 0)) {
      stepper5.runSpeedToPosition();
      stepper4.runSpeedToPosition();
      stepper1.runSpeedToPosition();
      stepper2.runSpeedToPosition();
    }        
}
// END RAPID();
// ==============================================================================================================================================================================
// START CIRCLE()
void circle(int x, int y,int z,int e, int rad) {
  double nx = tablexsteps*x/tablexdistance;
  double ny = tableysteps*y/tableydistance;  
  
  for (int i = 0; i < 3600; i++) 
  { 
    float angle = i*2*3.14/1000; 
    double cx = nx + (cos(angle) * (tablexsteps*rad/tablexdistance)); 
    double cy = ny + (sin(angle) * (tableysteps*rad/tableydistance)); 
    stepper5.moveTo(cx);
    stepper4.moveTo(cy);
    stepper1.moveTo(z);
    stepper2.moveTo(e); 
    while ((stepper5.distanceToGo() != 0) || (stepper4.distanceToGo() != 0) || (stepper1.distanceToGo() != 0) || (stepper2.distanceToGo() != 0)) {
          stepper5.runSpeedToPosition();
          stepper4.runSpeedToPosition();
          stepper1.runSpeedToPosition();
          stepper2.runSpeedToPosition();
        }      
  } 
}
// END CIRCLE()
// ==============================================================================================================================================================================
// START LINE()
void line(double x, double y, double z, double e) {
  /* 
    Allows you to position the printer and control the feeder
    dimensions are in microns. 1000 = 1mm
    Moves proportionally so all motors arrive at destination at the same time.
  */
/*
      Serial.print("RAWLINE:X");      Serial.print(x);
      Serial.print(" Y");             Serial.print(y);      
      Serial.print(" Z");             Serial.print(z);            
      Serial.print(" E");             Serial.print(e);  
      */

   double steps = 100;
   
        //calculate new step positions from micron position
   double nx = tablexsteps*(x/tablexdistance);
   double ny = tableysteps*(y/tableydistance);  
   double nz = tablezsteps*(z/tablezdistance);      
   double ne = tableesteps*(e/tableedistance);         

   double cx = float(stepper5.currentPosition());
   double cy = float(stepper4.currentPosition());   
   double cz = float(stepper1.currentPosition());      
   double ce = float(stepper2.currentPosition());         
   
   double dx = ((nx-cx)/steps);
   double dy = ((ny-cy)/steps);   
   double dz = ((nz-cz)/steps);      
   double de = ((ne-ce)/steps);         

   lastx = x;
   lasty = y;
   lastz = z;
   laste = e;

   for (int i = 0; i <= steps; i++) {   
     double newx = cx + (dx*i);
     double newy = cy + (dy*i);     
     double newz = cz + (dz*i);          
     double newe = ce + (de*i);     
      
     stepper5.moveTo(int(newx));
     stepper4.moveTo(int(newy));
     stepper1.moveTo(int(newz));
     stepper2.moveTo(int(newe));      
     while ((stepper5.distanceToGo() != 0) || (stepper4.distanceToGo() != 0) || (stepper1.distanceToGo() != 0) || (stepper2.distanceToGo() != 0)) {
       stepper5.run();
       stepper4.run();
       stepper1.run();
       stepper2.run();
     }  
   }

}
// END LINE();
// ==============================================================================================================================================================================

  pinMode(yZeropin, INPUT);                  //Y SWITCH
  pinMode(zZeropin, INPUT);                  //Z SWITCH

  //Thermo
  pinMode(thermo, INPUT);              //Analogue temperature measure
  pinMode(thermoswitch, OUTPUT);       //Heater mosfet switch
  digitalWrite(thermoswitch, LOW);     //Defaults to OFF/LOW
  
  pinMode(motorpinXstep, OUTPUT);
  pinMode(motorpinXdir, OUTPUT);  
  pinMode(motorpinYstep, OUTPUT);
  pinMode(motorpinYdir, OUTPUT);    
  pinMode(motorpinZstep, OUTPUT);
  pinMode(motorpinZdir, OUTPUT);    
  pinMode(motorpinEstep, OUTPUT);
  pinMode(motorpinEdir, OUTPUT);    

  thermistor();
  zeroaxis();                                
  thermistor();
  
  
  Serial.print("{ \"log\" : \"Success\"}");
  Serial.print("{ \"version\" : \""+version+"\"}");  
  pinMode(13, OUTPUT);
  Serial.print("{ \"status\" : \"waiting\"}"); 
}

aJsonStream serial_stream(&Serial);

double lastcmd = 0;
double tempcounter = 0;

void loop() {
    double lastloop = millis();
    while (serial_stream.available()) {
        aJsonObject *msg = aJson.parse(&serial_stream);
        processMessage(msg);
        lastcmd = millis();
        aJson.deleteItem(msg);                

        tempcounter++;
        if (tempcounter > 5) {
            thermistor();
            tempcounter = 0;
        }

        Serial.print("{ \"status\" : \"waiting\"}"); 
    } 
    
    
    if (!serial_stream.available()) {
      
    }    

    
    if (lastloop > lastcmd+1000) {
        pinMode(13, HIGH);
        delay(250);        
        pinMode(13, LOW);                
        delay(250);                
        pinMode(13, HIGH);
        delay(250);        
        pinMode(13, LOW);                
        delay(250);                
        Serial.print("{ \"status\" : \"waiting\"}"); 
        pinMode(13, HIGH);
        delay(250);        
        pinMode(13, LOW);                
        delay(250);                
        pinMode(13, HIGH);
        delay(250);        
        pinMode(13, LOW);                
        delay(250);        
    }

}
//=======================================================================================

void thermistor() {
   Serial.print("{ \"thermistor\" : \"");
   Serial.print(analogRead(thermo));
   Serial.print("\"}");   
}

//=======================================================================================
