/*
  Uno with Scope https://github.com/Dlloydev/Wokwi-Chip-Scope
        and https://github.com/Dlloydev/Wokwi-Chip-PWM


  Wokwi Uno https://wokwi.com/projects/390819301187622913
  Wokwi Mega: https://wokwi.com/projects/390819455604080641
  Wokwi ESP32S3 https://wokwi.com/projects/404720144387009537
  Wokwi ESP32  https://wokwi.com/projects/408508221519641601

  See also https://wokwi.com/projects/359331973918199809

  ESP32:
  https://docs.espressif.com/projects/esp-idf/en/stable/esp32s3/hw-reference/esp32s3/user-guide-devkitc-1.html

*/

/* 
//plainEncoder_motor.ino
This sketch is for driving a Encoder Metal Gearmotor 12V DC 80 RPM Gear Motor with Encoder
with a TB6612FNG dual-H-bridge driver.

IRL the motor switches speeds from full CW to full CCW, as the PWM and 
INA1 & INA2 signals change.

*/

long count;

#if 1 
// TB6612FNG pins per 
// https://learn.sparkfun.com/tutorials/tb6612fng-hookup-guide/all
//
const int AI1pin = 22; // LL off, HH brake, LH or HL move
const int AI2pin = 23; //
const int APWMpin = 21; // active HIGH
const int StandbyPin = 16; // RX activeHIGH
const int Apin = 18;  // ESP32 DevKit V1 general pins
const int Bpin = 19;
#endif

#if 0 
const int AI1pin = 12;
const int AI2pin = 13;
const int APWMpin = 11;
const int StandbyPin = 10; // activeHIGH

const int Apin = 18;  // ESP32 DevKit V1 general pins
const int Bpin = 19;
#endif



void setup() {
  Serial.begin(115200);
  while (!Serial)
    ;
  pinMode(Apin, INPUT_PULLUP);
  pinMode(Bpin, INPUT_PULLUP);

  pinMode(AI1pin, OUTPUT);
  pinMode(AI2pin, OUTPUT);
  pinMode(APWMpin, OUTPUT);
  pinMode(StandbyPin, OUTPUT);
  digitalWrite(StandbyPin,HIGH);

  for (int ii = 0; ii <= 2000; ii += 200) {
    // delay for IDE Serial monitor to connect
    delay(200);
    Serial.println(millis());
  }
  Serial.println("ESP32 encoder plain loop");
}

void loop() {
  static int lastA = -1;
  static uint32_t lastEncUs;
  int nowA = digitalRead(Apin);
  uint32_t nowUs = micros();
  if (nowA != lastA && nowUs - lastEncUs > 0) {
    if (nowA == LOW) {  // falling
      if (digitalRead(Bpin) == LOW) {
        ++count;
      } else {
        --count;
      }
      Serial.print(count);
      Serial.print(' ');
    }
    lastEncUs = nowUs;
    lastA = nowA;
  }
  cycleMotor();
  report();
}

void report() {
  const uint32_t interval = 1000;
  static uint32_t last = -interval;
  uint32_t now = millis();
  if (now - last >= interval) {
    last += interval;
    Serial.print(now);
    Serial.print('r');
    Serial.print(digitalRead(Apin) == HIGH ? "A" : "a");
    Serial.print(digitalRead(Bpin) == HIGH ? "B" : "b");
    Serial.print(count);
    Serial.println();
  }
}


void cycleMotor() {
  static int motorState = 0, lastMotorState = 0;
  static int motForward = 1;
  const uint32_t interval = 20;
  static uint32_t last = -interval;
  uint32_t now = millis();
  if (now - last >= interval) {
    last += interval;
    switch (motForward) {
      case 1:
        if (motorState >= 255) {
          motorState = 255;
          motForward = 0;
        }
        ++motorState;
        break;
      case 0:
        if (motorState <= -255) {
          motorState = -255;
          motForward = 1;
        }
        --motorState;
        break;
    }
    if (lastMotorState == 0 && motorState > 0) {
      Serial.print("forewards");
      digitalWrite(AI1pin, LOW);
      digitalWrite(AI2pin, HIGH);
    }
    if (lastMotorState == 0 && motorState < 0) {
      Serial.print("backwards");
      digitalWrite(AI1pin, HIGH);
      digitalWrite(AI2pin, LOW);
    }
    analogWrite(APWMpin, abs(motorState));
    digitalWrite(APWMpin,abs(motorState)>10);
    lastMotorState = motorState;
    Serial.print("p");Serial.print(motorState);Serial.print(' ');
  }
}