#include <Arduino.h>


#include <AccelStepper.h>

// https://learn.watterott.com/de/silentstepstick/pinconfig/tmc2130/


#define EN_PIN D0
#define TILT_STEP_PIN D7
#define TILT_DIR_PIN D4
#define ENDSTOP_TILT D5

#define PAN_STEP_PIN D8
#define PAN_DIR_PIN D3
#define ENDSTOP_PAN A0

#define PIN_LIGHT D6

AccelStepper stepperPan(AccelStepper::DRIVER, PAN_STEP_PIN, PAN_DIR_PIN);

AccelStepper stepperTilt(AccelStepper::DRIVER, TILT_STEP_PIN, TILT_DIR_PIN);

unsigned long stepsPerRotationPan=16*42*400/12; //12z to 42z, 16x microstepping

unsigned long stepsPerRotationTilt=16*85*400/17;//17z to 85z, 16x microstepping

void setEnable(bool t){
  digitalWrite(EN_PIN,!t); //active low
}

void setup() {
  Serial.begin(115200);

  pinMode(EN_PIN,OUTPUT);
  pinMode(ENDSTOP_TILT,INPUT_PULLUP);
  pinMode(ENDSTOP_PAN,INPUT_PULLUP);
  pinMode(PIN_LIGHT, OUTPUT);
  digitalWrite(PIN_LIGHT, LOW);
  setEnable(false);

  stepperPan.setMaxSpeed(10000); //tested w/o load 15000
  stepperPan.setAcceleration(20000.0); //tested w/o load 80000
  stepperPan.moveTo(stepsPerRotationPan/2);

  stepperTilt.setMaxSpeed(10000); //tested w/o load 15000
  stepperTilt.setAcceleration(20000.0); //tested w/o load 80000
  stepperTilt.moveTo(stepsPerRotationTilt/8);

  setEnable(true);
}

void loop() {
  static unsigned long last_change;

  if (stepperPan.distanceToGo() == 0) {
    if (stepperPan.currentPosition()>100) {
      Serial.print("Pan Moving to: "); Serial.println(0);
      stepperPan.moveTo(0);
    }else{
      Serial.print("Pan Moving to: "); Serial.println(stepsPerRotationPan/2);
      stepperPan.moveTo(stepsPerRotationPan/2);
    }
  }

  if (stepperTilt.distanceToGo() == 0) {
    if (stepperTilt.currentPosition()>100) {
      Serial.print("Tilt Moving to: "); Serial.println(0);
      stepperTilt.moveTo(0);
    }else{
      Serial.print("Tilt Moving to: "); Serial.println(stepsPerRotationTilt/8);
      stepperTilt.moveTo(stepsPerRotationTilt/8);
    }
  }
  
  bool resultPan = stepperPan.run();
  bool resultTilt = stepperTilt.run();

  
  if ( millis() > last_change+500 ) {
    last_change=millis();
    //Serial.print("run="); Serial.print(result);
    //Serial.print(" dist="); Serial.print(stepperPan.distanceToGo());
    //Serial.print(" pos="); Serial.print(stepperPan.currentPosition());
    //Serial.println();
  }
  
}