//WeMos D1 R2 & mini

//D8 is not working??

//Serial
long last_serialdebug=0;
#define INTERVAL_SERIALDEBUG 200

//Inputs
#define PIN_BUTTON D3 //D3 should not be pulled to gnd while booting
#define PIN_ENCA D1
#define PIN_ENCB D2
#define BUTTON_RELEASE_DEBOUNCE 100 //minimum time after button release to reenable triggering

boolean button_flag=false; //true if button pressed
boolean button_released=true;
long last_button_released=0; //last time button has been released (for debounce)

#include <Encoder.h>
Encoder volEnc(PIN_ENCA,PIN_ENCB);

//Servo stuff
#define PIN_MOTOR_IN1 D6 //L293 Motor IN1
#define PIN_MOTOR_IN2 D5 //L293 Motor IN2

#define PIN_POT A0 //reference potentiometer wiper
#define DEADZONE_POTI 5 //maximum allowed error. stop when reached this zone
#define POT_MIN 5 //minimum value pot can reach
#define POT_MAX 1010 //maximum value pot can reach

int poti_set=512; //set value
int poti_read=0; //read value from poti
boolean poti_reachedposition=true; //set to true if position reached. after that stop turning

#define MOTOR_STOP(); digitalWrite(PIN_MOTOR_IN1,LOW); digitalWrite(PIN_MOTOR_IN2,LOW);
#define MOTOR_LEFT(); digitalWrite(PIN_MOTOR_IN1,LOW); digitalWrite(PIN_MOTOR_IN2,HIGH);
#define MOTOR_RIGHT(); digitalWrite(PIN_MOTOR_IN1,HIGH); digitalWrite(PIN_MOTOR_IN2,LOW);
#define MOTOR_TURNING() (digitalRead(PIN_MOTOR_IN1) != digitalRead(PIN_MOTOR_IN2))

//Motorcheck
long last_motorcheck=0;
#define INTERVAL_MOTORCHECK 100 //check motor movement every x ms
int poti_read_last=0;
int motor_vel=0; //analog read units per second
#define MINIMUM_MOTORVEL 2 //minimum velocity motor should turn wenn active
#define MOTOR_FAILTIME 500 //in ms. if motor did not turn fox x amount of time at least with MINIMUM_MOTORVEL an error will initiate
long last_motorTooSlow=0; //typically 0

//error
uint8_t error=0;
#define NOERROR 0
#define MOTORDIDNOTTURN 1

void setup() {
  pinMode(LED_BUILTIN,OUTPUT);

  Serial.begin(115200);

  pinMode(PIN_BUTTON,INPUT_PULLUP);

  pinMode(PIN_MOTOR_IN1,OUTPUT);
  pinMode(PIN_MOTOR_IN2,OUTPUT);

  digitalWrite(PIN_MOTOR_IN1,LOW);
  digitalWrite(PIN_MOTOR_IN2,LOW);

  pinMode(PIN_POT,INPUT);

  Serial.println("Setup finished");

}

void loop() {
  long loopmillis=millis();

  //Serial Input ##############################################
  while (Serial.available() > 0) {
    int _value = Serial.parseInt();
    if (Serial.read() == '\n') {
      Serial.print("value=");
      Serial.println(_value);
      poti_set=_value;
      poti_reachedposition=false; //aim for new position
    }
    
  }

  //Inputs ###################################################
  poti_read=analogRead(PIN_POT); //read poti
  if (!digitalRead(PIN_BUTTON)){ //button pressed
    if (button_released){
      button_released=false; //flag: not released
      if(loopmillis-last_button_released > BUTTON_RELEASE_DEBOUNCE){
        button_flag=true;
      }
    }
  }else if(!button_flag && !button_released){ //button released and flag has been cleared
    last_button_released=loopmillis;
    button_released=true;
  }

  //Read Encoder to velocity "volEncVel"
  int volEncVel=0;
  int _volEnc=volEnc.read();
  if (_volEnc!=0){ //encoder moved
    volEncVel=_volEnc;
    volEnc.write(0); //reset value
  }
  

  //Input Handling
  if (volEncVel!=0){ //knob moved
    poti_set+=volEncVel; //change poti set value
    poti_set=constrain(poti_set, POT_MIN,POT_MAX);
    poti_reachedposition=false;
  }


  //Motor Movement Routine #################
  if (error==0){ //no errors
      
    if (!poti_reachedposition && abs(poti_read-poti_set)>DEADZONE_POTI){ //error too high
      digitalWrite(LED_BUILTIN,HIGH);
      if (poti_read-poti_set < 0){
        MOTOR_LEFT();
      }else{
        MOTOR_RIGHT();
      }
    }else{
      MOTOR_STOP();
      poti_reachedposition=true; //position reached
      digitalWrite(LED_BUILTIN,LOW);
    }
  
    
    if ( loopmillis > last_motorcheck+INTERVAL_MOTORCHECK){
      last_motorcheck=loopmillis;
  
      motor_vel=(poti_read-poti_read_last)*1000 /INTERVAL_MOTORCHECK ; //calculate current motor velocity
      poti_read_last=poti_read;
  
      //motor fail check
      if (MOTOR_TURNING() && abs(motor_vel)<MINIMUM_MOTORVEL){ //motor is turning too slow
        if (last_motorTooSlow==0){ //first time slow motor recognized
          last_motorTooSlow=loopmillis; 
        }else if (loopmillis-last_motorTooSlow > MOTOR_FAILTIME){
          error=MOTORDIDNOTTURN;
          Serial.println("MOTORDIDNOTTURN");
        }
        
      }else if (last_motorTooSlow>0){ //was recognized too slow but is now turning fast again
        last_motorTooSlow=0; //reset
      }
      
    }
  }else{ //an error occured. error!=0
    MOTOR_STOP();
  }
  

  if ( loopmillis > last_serialdebug+INTERVAL_SERIALDEBUG){
    last_serialdebug=loopmillis;


    Serial.print(" set=");
    Serial.print(poti_set);
    Serial.print(" is=");
    Serial.print(poti_read);
    Serial.print(" vel=");
    Serial.print(motor_vel);
    Serial.println("");

    if (button_flag){ //TODO: remove hier if correct behaviour implemented
      Serial.println("BUTTON Pressed");
      button_flag=false; //clear flag to reenable button triggering.
    }
    
  }

}