int steppers[][4] = {{2,3,4,5},{8,9,10,11}};
float perTurn = 15*2*3.1415926;
int stepsPerTurn = 2048;
float perStep = perTurn/stepsPerTurn;
float x = 0;
float y = 0;

int steps[] = {0,0};
float r[] = {1414.2, 1414.2}; // Aktuelle Radien
float t[] = {1414.2, 1414.2}; // Target Radien
float px[] = {0,0};   // Aktuelle Position



void step(int s, int d) {
  digitalWrite(steppers[s][steps[s]], LOW);
  steps[s]=(steps[s]+4+d)%4;
  r[s]+=perStep*d;
  digitalWrite(steppers[s][steps[s]], HIGH);
  /*Serial.print("r[0]=");
  Serial.print(r[0]);
  Serial.print(" r[1]=");
  Serial.println(r[1]);*/
}


void stop() {
  for (int s=0; s<2; s++) {
    for (int p=0; p<4; p++) {
      digitalWrite(steppers[s][p], LOW);
    }
  }
}

void gotoPos(float gx, float gy) {
  int steps[2];
  unsigned long dtime[2];
  unsigned long ntime[2];
  int dir[2];
  t[0] = sqrt((1000+gx)*(1000+gx)+(1000-gy)*(1000-gy));
  t[1] = sqrt((1000-gx)*(1000-gx)+(1000-gy)*(1000-gy));
  /*Serial.print("Target t[0]=");
  Serial.print(t[0]);
  Serial.print(" t[1]=");
  Serial.print(t[1]);
  Serial.print("   for x=");
  Serial.print(gx);
  Serial.print(" y=");
  Serial.println(gy);*/
  
  steps[0] = abs(t[0]-r[0])/perStep;
  steps[1] = abs(t[1]-r[1])/perStep;
  for (int s=0; s<2; s++) {
    if (t[s]>r[s]) {
      dir[s] = 1;
    } else {
      dir[s] = -1;
    }
  }

  /*Serial.print("steps[0]=");
  Serial.print(steps[0]);
  Serial.print("  steps[1]=");
  Serial.print(steps[1]);
  Serial.print("  dir[0]=");
  Serial.print(dir[0]);
  Serial.print(" dir[1]=");
  Serial.println(dir[1]);*/
  
  if (steps[0]>steps[1]) {
    dtime[0]=5000;
    if (steps[1]>0) {
      dtime[1]=5000L*(long)steps[0]/steps[1];
    } else {
      dtime[1] = 5000L*(long)steps[1];
    }
  } else {
    dtime[1]=5000;
    if (steps[0]>0) {
      dtime[0]=5000L*(long)steps[1]/steps[0];
    } else {
      dtime[0] = 5000L*(long)steps[1];
    }
  }
  /*Serial.print("dtime[0]=");
  Serial.print(dtime[0]);
  Serial.print("  dtime[1]=");
  Serial.println(dtime[1]); */
  ntime[0] = micros()+dtime[0];
  ntime[1] = micros()+dtime[1];
  while (steps[0]>0 && steps[1]>0) {
    for (int s=0; s<2; s++) {
      if (micros()>ntime[s]) {
        ntime[s]+=dtime[s];
        step(s,dir[s]);
        steps[s]--;
      }
    }
  }
  px[0] = gx;
  px[1] = gy;
  /*Serial.print("r[0]=");
  Serial.print(r[0]);
  Serial.print(" r[1]=");
  Serial.print(r[1]);
  Serial.print("  now at x=");
  Serial.print(gx);
  Serial.print(" y=");
  Serial.println(gy);*/
  
}

void approxTo(float gx, float gy) {
  float a = px[0];
  float b = px[1];
  float vx = gx-px[0];
  float vy = gy-px[1];
  float l = sqrt(vx*vx+vy*vy);
  int steps = (int)(l/2)+1;
  vx /= steps;
  vy /= steps;
  a+=vx;
  b+=vy;
  for (int i=0; i<steps; i++) {
    gotoPos(a,b);
    a+=vx;
    b+=vy;
  }  
  Serial.print("Finished going to x=");
  Serial.print(gx);
  Serial.print(" y=");
  Serial.println(gy);
  
}

void setup() {
  Serial.begin(115200);
  for (int s=0; s<2; s++) {
    for (int p=0; p<4; p++) {
      pinMode(steppers[s][p], OUTPUT);
      digitalWrite(steppers[s][p], LOW);
    }
  }
}


void nikolaus() {
  approxTo(0,100);
  approxTo(100,100);
  approxTo(50,150);
  approxTo(0,100);
  approxTo(100,0);
  approxTo(100,100);
  approxTo(0,0);
  approxTo(100,0);
}

void techlab() {
  approxTo(247.0, 32.3);
  approxTo(237.4, 40.4);
  approxTo(243.6, 46.1);
  approxTo(246.1, 55.2);
  approxTo(238.1, 68.9);
  approxTo(221.3, 73.5);
  approxTo(195.9, 73.5);
  approxTo(195.9, 1.5);
  approxTo(223.3, 1.5);
  approxTo(242.1, 7.2);
  approxTo(250.0, 22.2);
  approxTo(250.0, 22.2);
  approxTo(230.5, 47.3);
  approxTo(221.6, 44.4);
  approxTo(207.7, 44.4);
  approxTo(207.7, 64.0);
  approxTo(220.3, 64.0);
  approxTo(229.7, 61.5);
  approxTo(234.1, 54.3);
  approxTo(234.1, 54.3);
  approxTo(232.9, 14.4);
  approxTo(222.7, 11.0);
  approxTo(207.7, 11.0);
  approxTo(207.7, 34.8);
  approxTo(222.7, 34.8);
  approxTo(232.8, 31.6);
  approxTo(237.3, 23.0);
  approxTo(237.3, 23.0);
  approxTo(158.3, 73.5);
  approxTo(144.5, 73.5);
  approxTo(114.8, 1.5);
  approxTo(128.5, 1.5);
  approxTo(135.3, 19.5);
  approxTo(167.5, 19.5);
  approxTo(174.3, 1.5);
  approxTo(188.0, 1.5);
  approxTo(139.2, 29.1);
  approxTo(151.6, 61.1);
  approxTo(163.7, 29.1);
  approxTo(111.0, 11.1);
  approxTo(74.1, 11.1);
  approxTo(74.1, 73.5);
  approxTo(62.3, 73.5);
  approxTo(62.3, 1.5);
  approxTo(111.0, 1.5);
  approxTo(14.2, 73.5);
  approxTo(2.3, 73.5);
  approxTo(2.3, 44.6);
  approxTo(-32.1, 44.6);
  approxTo(-32.1, 73.5);
  approxTo(-43.9, 73.5);
  approxTo(-43.9, 1.5);
  approxTo(-32.1, 1.5);
  approxTo(-32.1, 34.9);
  approxTo(2.3, 34.9);
  approxTo(2.3, 1.5);
  approxTo(14.2, 1.5);
  approxTo(-55.1, 19.2);
  approxTo(-66.6, 12.3);
  approxTo(-79.9, 9.8);
  approxTo(-98.9, 19.2);
  approxTo(-105.0, 37.3);
  approxTo(-98.9, 55.2);
  approxTo(-79.9, 64.9);
  approxTo(-66.3, 62.3);
  approxTo(-55.1, 55.1);
  approxTo(-55.1, 67.7);
  approxTo(-66.6, 72.8);
  approxTo(-79.9, 74.8);
  approxTo(-108.5, 61.7);
  approxTo(-117.7, 37.3);
  approxTo(-108.4, 12.8);
  approxTo(-79.9, 0.0);
  approxTo(-66.7, 1.8);
  approxTo(-55.1, 7.4);
  approxTo(-55.1, 7.4);
  approxTo(-125.0, 11.1);
  approxTo(-163.6, 11.1);
  approxTo(-163.6, 34.9);
  approxTo(-127.6, 34.9);
  approxTo(-127.6, 44.6);
  approxTo(-163.6, 44.6);
  approxTo(-163.6, 63.7);
  approxTo(-125.9, 63.7);
  approxTo(-125.9, 73.5);
  approxTo(-175.3, 73.5);
  approxTo(-175.3, 1.5);
  approxTo(-125.0, 1.5);
  approxTo(-184.0, 73.5);
  approxTo(-250.0, 73.5);
  approxTo(-250.0, 63.7);
  approxTo(-222.8, 63.7);
  approxTo(-222.8, 1.5);
  approxTo(-210.9, 1.5);
  approxTo(-210.9, 63.7);
  approxTo(-184.0, 63.7);
  
}


void loop3() {
  step(0,-1);
  step(1,1);
  delay(5);
  
}
void loop() {
  techlab();
  stop();
  while (true);
  //step(1,-1);
  //delay(5);
}