#include <Arduino.h>


#include <AccelStepper.h>

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



// Motor / Hardware Configuration
int microsteppingPan = 4;
long stepsPerRotationPan=microsteppingPan*85*400/17; //17z to 85z
long homepos_pan=-stepsPerRotationPan/360*4; //positive number=homed position is too far clockwise

int microsteppingTilt = 4;

long stepsPerRotationTilt=microsteppingTilt*42*400/12; //12z to 42z
long homepos_tilt=0; //positive number=homed position is too far clockwise


enum Mode{
  PROGRAM,
  HOME,
  RUN,
  IDLE
};
enum Mode mode=PROGRAM;



enum Stepperpreset{
  HOMECOARSE,
  HOMEFINE,
  SLOW,
  NORMAL,
  FAST
};

enum Stepper{
  PAN,
  TILT
};

bool homedPan=false;
bool homedTilt=false;


AccelStepper stepperPan(AccelStepper::DRIVER, PAN_STEP_PIN, PAN_DIR_PIN);
AccelStepper stepperTilt(AccelStepper::DRIVER, TILT_STEP_PIN, TILT_DIR_PIN);

void setOutput(bool o);
bool getOutput();
void setEnable(bool t);
bool getEnable();
void setStepperPreset(Stepperpreset preset,Stepper s);
void setMode(Mode newmode);
bool home(bool direction,bool endstop, uint8_t& homestage, Stepper s);

void testMovementPan();
void testMovementTilt();
bool isEndstopPan();
bool isEndstopTilt();

void setOutput(bool o) {
  digitalWrite(PIN_OUTPUT, o);
}
bool getOutput() {
  return digitalRead(PIN_OUTPUT);
}

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

void setStepperPreset(Stepperpreset preset,Stepper s)
{
  float pan_speed=0;
  float pan_accel=0;
  float tilt_speed=0;
  float tilt_accel=0;
  switch (preset){
    case HOMECOARSE:
      pan_speed=stepsPerRotationPan*1/10;
      pan_accel=stepsPerRotationPan*1/4;
      tilt_speed=stepsPerRotationTilt*1/10;
      tilt_accel=stepsPerRotationTilt*1/4;
      break;
    case HOMEFINE:
      pan_speed=stepsPerRotationPan*1/400;
      pan_accel=stepsPerRotationPan*1/4;
      tilt_speed=stepsPerRotationTilt*1/400;
      tilt_accel=stepsPerRotationTilt*1/4;
      break;
    case SLOW:
      pan_speed=stepsPerRotationPan*1/10;
      pan_accel=stepsPerRotationPan*1/4;
      tilt_speed=stepsPerRotationTilt*1/10;
      tilt_accel=stepsPerRotationTilt*1/4;
      break;
    case NORMAL:
      pan_speed=stepsPerRotationPan*1/4;
      pan_accel=stepsPerRotationPan*1/2;
      tilt_speed=stepsPerRotationTilt*1/4;
      tilt_accel=stepsPerRotationTilt*1/2;
      break;
    case FAST:
      //abs. max. speed w/o load: stepsPerRotationPan*2/3 @19V 
      //abs. max. speed w/o load: stepsPerRotationTilt*1/2 @19V 
      pan_speed=stepsPerRotationPan*1/2;
      pan_accel=stepsPerRotationPan*2/1;
      tilt_speed=stepsPerRotationTilt*1/2;
      tilt_accel=stepsPerRotationTilt*2/1;
      break;
  }

  if (s==PAN) {
    stepperPan.setMaxSpeed(pan_speed);
    stepperPan.setAcceleration(pan_accel);
    Serial.print("Set Pan Preset. Speed=");Serial.print(pan_speed); Serial.print(" acc="); Serial.println(pan_accel);
  }else if (s==TILT){
    stepperTilt.setMaxSpeed(tilt_speed);
    stepperTilt.setAcceleration(tilt_accel);
    Serial.print("Set Tilt Preset. Speed=");Serial.print(tilt_speed); Serial.print(" acc="); Serial.println(tilt_accel);
  }
}

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

  pinMode(ENDSTOP_TILT,INPUT_PULLUP);
  pinMode(ENDSTOP_PAN,INPUT_PULLUP);
  pinMode(PIN_OUTPUT, OUTPUT);
  setOutput(false);
  pinMode(EN_PIN,OUTPUT);
  setEnable(false);

  
  //stepperPan.moveTo(stepsPerRotationPan/2);

  //stepperTilt.moveTo(stepsPerRotationTilt/4);

  delay(1000); //wait until terminal ready
  if (isEndstopPan()) {
    delay(500);
    setMode(PROGRAM);
    
  }else{
    setMode(HOME);
  }

  Serial.println("Starting");
  
}

void loop() {
  static unsigned long last_change;

  // ### Mode handling ###
  switch(mode){
    case PROGRAM:
      break;

    case HOME:
      static uint8_t homestagePan=0;
      static uint8_t homestageTilt=0;
      homedPan=home(1,isEndstopPan(),homestagePan,PAN);
      homedTilt=home(1,isEndstopTilt(),homestageTilt,TILT);
      if (homedPan && homedTilt) { //finished homing
        setMode(RUN);
      }
      break;

    case RUN:
      testMovementPan();
      testMovementTilt();
      break;

    case IDLE:
      break;

  }


  
  if (getEnable()){
    //bool resultPan = stepperPan.run();
    stepperPan.run();
    //bool resultTilt = stepperTilt.run();
    stepperTilt.run();
  }


  // ### TIMED PRINTING ###
  
  if ( millis() > last_change+250 ) {
    last_change=millis();
    

    switch(mode){
      case PROGRAM:
        Serial.print("PAN Endstop="); Serial.print(digitalRead(ENDSTOP_PAN));
        Serial.print(" TILT Endstop="); Serial.print(analogRead(ENDSTOP_TILT));
        Serial.println();
        break;

      case HOME:
        break;

      case RUN:
        testMovementPan();
        
        Serial.print("Pan pos="); Serial.print(stepperPan.currentPosition());
        Serial.print(" Tilt pos="); Serial.print(stepperTilt.currentPosition());
        Serial.println();
        break;

      case IDLE:
        break;

    }
  }
  
}


void setMode(Mode newmode) {
  switch(newmode){
    case PROGRAM:
      setEnable(false);
      homedPan=false;
      homedTilt=false;
      mode=newmode;
      Serial.println("Mode set to PROGRAM");
      break;

    case HOME:
      setStepperPreset(HOMECOARSE,PAN); //set Pan
      setStepperPreset(HOMECOARSE,TILT); //set Tilt
      setEnable(true);
      mode=newmode;
      Serial.println("Mode set to HOME");
      break;

    case RUN:
      setStepperPreset(FAST,PAN); //set Pan
      setStepperPreset(FAST,TILT); //set Tilt
      setEnable(true);
      mode=newmode;
      Serial.println("Mode set to RUN");
      break;

    case IDLE:
      mode=newmode;
      Serial.println("Mode set to IDLE");
      break;

  }
}


bool home(bool direction,bool endstop, uint8_t& homestage, Stepper s){
  //PAN direction = 1 -> clockwise (viewed from top)
  //TILT direction = 1 -> clockwise (viewed pcb side)
  AccelStepper* stepper;
  if (s==PAN) {
    stepper=&stepperPan;
  }else if (s==TILT) {
    stepper=&stepperTilt;
  }else{
    return false;
  }

  switch (homestage){
    case 0: //move fast until endstop triggered
      if (!endstop) {
        stepper->move(direction*2*stepper->acceleration());
      }else{ //endstop reached
        stepper->stop();
        setStepperPreset(HOMECOARSE,s);
        homestage=1;
        Serial.println("Endstop Reached with coarse");
      }
      
      break;
    case 1:
      if (endstop) {
        stepper->move(-direction*2*stepper->acceleration()); //move away
      }else{ //endstop released
        stepper->stop();
        setStepperPreset(HOMEFINE,s);
        homestage=2;
        Serial.println("Endstop Released");
      }
      
      break;
    case 2:
      if (!endstop) {
        stepper->move(direction*2*stepper->acceleration()); //move towards endstop
      }else{ //endstop reached
        stepper->stop();
        Serial.println("Endstop Reached with fine");
        if (s==PAN) {
          stepper->setCurrentPosition(homepos_pan+stepsPerRotationPan);
        }else if(s==TILT) {
          stepper->setCurrentPosition(homepos_tilt+stepsPerRotationTilt);
        }
        
        homestage=3;
        return true;
      }
      break;
    default:
      return true;
  }
  return false;
}


void testMovementPan()
{
  static long last_positionReached=0;
  if (stepperPan.distanceToGo() == 0 && last_positionReached==0) {
    last_positionReached=millis();
  }
  if (last_positionReached!=0 && millis()-last_positionReached>200) {
    last_positionReached=0;

    if (stepperPan.currentPosition()<stepsPerRotationPan-100) {
      stepperPan.moveTo(stepsPerRotationPan); //move to home
      setOutput(false); //test output
    }else{
      stepperPan.moveTo(stepsPerRotationPan-random(200,stepsPerRotationPan)); //move somewhere else
      setOutput(true); //test output
    }
  }
}
void testMovementTilt()
{

  static long last_positionReached=0;
  if (stepperTilt.distanceToGo() == 0 && last_positionReached==0) {
    last_positionReached=millis();
  }
  if (last_positionReached!=0 && millis()-last_positionReached>1000) {
    last_positionReached=0;

    if (stepperTilt.currentPosition()<stepsPerRotationTilt-100) {
      stepperTilt.moveTo(stepsPerRotationTilt); //move to home
    }else{
      stepperTilt.moveTo(stepsPerRotationTilt-random(200,stepsPerRotationTilt)); //move somewhere else
      
      
    }
  }
}


bool isEndstopPan()
{
  //PAN 0V when magnet is close, High when not
  return !digitalRead(ENDSTOP_PAN); //active Low
}
bool isEndstopTilt()
{
  // Tilt Hall sensor is U18. 0V when powered on. switches when magnetic field changes (keeps state when magnet not close).
  return analogRead(ENDSTOP_TILT)>=512; //active when magnet passed by (field changed to north? "red marking").
}