#include <math.h> // (no semicolon)
int pins[]={
  9,8,11,10,13,12};  // x step,x dir, y step, y dir, z step, z dir
// x,y,z =0 when the head on the left upper corner with high z
//format: x,y,z,
int xyz[]={
  0,0,0};
int current[]={
  0,0,0};

void setup() {
  for (int i =0;i<=5;i++){
    pinMode(pins[i], OUTPUT);
  }
  Serial.begin(9600); 
} 


void loop()
{
  getSerial();

}

void getSerial()
{
  long serialdata = 0;
  int inbyte=0;
  int neg=0;
  int i=0;
  while (Serial.available())
  {
    inbyte = Serial.read();  
    if (inbyte > 0 && inbyte != ',')
    { 
      if (inbyte=='-') {
        neg=1;
      } 
      else {
        serialdata = serialdata * 10 + inbyte - '0';
      }
    } 
    else if (inbyte == ','){
      if (!neg) serialdata*=-1;
      xyz[i]=serialdata;
      i++;
      neg=0;
      serialdata = 0;
    }
  }
  if (i>0){

  //  int dx=current[0]-xyz[0];
   // int dy=current[1]-xyz[1];
   // int dz=current[2]-xyz[2];

   // current[0]-=xyz[0];
    //current[1]-=xyz[1];
    //current[2]-=xyz[2];

    if (xyz[0]!=0) rotateDeg(0,xyz[0] ,0.6);     
 //   Serial.println(current[0]);
    if (xyz[1]!=0) rotateDeg(2,xyz[1] ,0.2);     
   // Serial.println(current[1]);
    if (xyz[2]!=0) rotateDeg(4,xyz[2] ,0.6);     
    //Serial.println(current[2]);

    Serial.println('d');

  }
xyz[0]=0;
xyz[1]=0;
xyz[2]=0;
  inbyte = 0;
  i=0; 
}

void gotopoint(int x,int y,int z){ 
  //handle x,y :


  //handle z:

}


void rotateDeg(int axis,float deg, float speed){
  //rotate a specific number of degrees (negitive for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger
  int dir = (deg > 0)? HIGH:LOW;
  digitalWrite(pins[axis+1],dir); 

  int steps = abs(deg)*(1/0.225);
  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){
    digitalWrite(pins[axis], HIGH);
    delayMicroseconds(usDelay); 

    digitalWrite(pins[axis], LOW);
    delayMicroseconds(usDelay);
  }
}


/*
void rotate(int axis,int steps, float speed){
 //rotate a specific number of microsteps (8 microsteps per step) - (negitive for reverse movement)
 //speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger
 int dir = (steps > 0)? HIGH:LOW;
 steps = abs(steps);
 digitalWrite(pins[axis+1],dir); 
 float usDelay = (1/speed) * 70;
 
 for(int i=0; i < steps; i++){
 digitalWrite(pins[axis], HIGH);
 delayMicroseconds(usDelay); 
 
 digitalWrite(pins[axis], LOW);
 delayMicroseconds(usDelay);
 }
 } 
 */