#include<Arduino.h>
#include <Wire.h>


#include "WEMOS_Motor.h"

//follow firmware flash guide for new wemos motor shield v1.0  https://github.com/thomasfredericks/wemos_motor_shield

#define BUTTON_DEBOUNCE 200
#define PIN_BUTTON D5
unsigned long last_pin_button=0;
bool pin_button_down=0;
bool last_pin_button_down=0;

bool manual_drive_direction=false;

#define PIN_SENSE A0
unsigned long last_print=0;
#define SENSE_CLEAR_LOWER 40 //adc value lower limit for clear part. clear is around 70
#define SENSE_CLEAR_UPPER 100 //adc value upper limit for clear part

#define SENSE_OPAQUE_LOWER 600 //adc value lower limit for opaque part. opaque is around 675
#define SENSE_OPAQUE_UPPER 750 //adc value upper limit for opaque part

#define SENSE_END_LOWER 850 //adc value lower limit for end marker
#define SENSE_END_UPPER 1024 //adc value upper limit for end marker

//define directions for motors
#define _M1_UP = _CCW;
#define _M1_DOWN = _CW;

#define _M2_UP = _CCW;
#define _M2_DOWN = _CW;

int pwm;

//Motor shield default I2C Address: 0x30
//PWM frequency: 1000Hz(1kHz)
Motor M1(0x30, _MOTOR_A, 1000); //Motor A
Motor M2(0x30, _MOTOR_B, 1000); //Motor B


void setup() {
  
  
  Serial.begin(57600);
  Serial.println("Starting demo");

  pinMode(PIN_BUTTON, INPUT_PULLUP);
  pinMode(PIN_SENSE, INPUT);

  M1.setmotor(_STOP);
  M2.setmotor(_STOP);

  pwm=100;
  
}


void loop() {

  if (millis() > last_pin_button + BUTTON_DEBOUNCE) {
    bool new_pin_button_down=!digitalRead(PIN_BUTTON);
    if (pin_button_down != new_pin_button_down) { //changed
      pin_button_down = new_pin_button_down; //update
      last_pin_button=millis(); //delay next check
    }    
  }

  if (pin_button_down) { //button currently pressed
    if (last_pin_button_down!=pin_button_down) { //value changed. executed one time when entering if
      if (manual_drive_direction) {
        M1.setmotor( _CW, pwm);
        Serial.print("CW PWM: ");
      }else{
        M1.setmotor( _CCW, pwm);
        Serial.print("CCW PWM: ");
      }
      Serial.println(pwm);
      manual_drive_direction=!manual_drive_direction; //switch direction every new press
    }
  }else{
    if (last_pin_button_down!=pin_button_down) { //value changed
      Serial.println("Motor STOP");
      M1.setmotor(_STOP);
    }
  }
  last_pin_button_down=pin_button_down;

  if (millis() > last_print + 100) {
    int sense=analogRead(PIN_SENSE);
    Serial.println(sense);
    last_print=millis();
  }
}

/*
void loop() {

  int pwm;

  for (pwm = 0; pwm <= 100; pwm++)
  {
    M1.setmotor( _CW, pwm);
    M2.setmotor(_CW, pwm);
    Serial.print("Clockwise PWM: ");
    Serial.println(pwm);
    delay(100);
  }

  Serial.println("Motor STOP");
  M1.setmotor(_STOP);
  M2.setmotor( _STOP);

  delay(1000);

  
  for (pwm = 0; pwm <= 100; pwm++)
  {
    M1.setmotor(_CCW, pwm);
    //delay(1);
    M2.setmotor(_CCW, pwm);
    Serial.print("Counterclockwise PWM: ");
    Serial.println(pwm);
    delay(100);

  }

  Serial.println("Motor A&B STANDBY");
  M1.setmotor(_STANDBY);
  M2.setmotor( _STANDBY);
  delay(1000);

}
*/