//#include "esp_system.h"
//#include "Arduino.h"
//#include <wiring_private.h>

int period = 1; 
unsigned long time_trig = 0; 
unsigned long currentTime = 0;

int state_machine = 0; 
int pin_state; 

// Motor A
#define motorAPin1 16 
#define motorAPin2  19 

// Motor B
#define motorBPin1 25 
#define motorBPin2 26 



void setup() {
  // sets the pins as outputs:
  pinMode(motorAPin1, OUTPUT);
  pinMode(motorAPin2, OUTPUT);

  pinMode(motorBPin1, OUTPUT);
  pinMode(motorBPin2, OUTPUT);

  Serial.begin(115200);
  Serial.print("Testing DC Motor...");
}

void loop() {
  while(millis() > (time_trig + period)){
    //Serial.println(state_machine);
    state_machine++; 

    switch (state_machine) {
      case 1: 
        //Serial.println("Moving Forward");
        digitalWrite(motorAPin1, 1);
        digitalWrite(motorAPin2, 0); 
        digitalWrite(motorBPin1, 1);
        digitalWrite(motorBPin2, 0); 
        break; 

      case 2:
        //Serial.println("Motor stopped");
        digitalWrite(motorAPin1, 1);        
        digitalWrite(motorAPin2, 1);
        digitalWrite(motorBPin1, 1);
        digitalWrite(motorBPin2, 1);
        pin_state = digitalReadOutputPin(motorAPin1);
        //Serial.print("Pin state is ");
        //Serial.print(pin_state);
        //Serial.println();
        break;

      case 3:
        //Serial.println("Motor backwards");
        digitalWrite(motorAPin1, 0);
        digitalWrite(motorAPin2, 1);
        digitalWrite(motorBPin1, 0);
        digitalWrite(motorBPin2, 1);
        break;

      case 4:
          //Serial.println("Motor FULL STOP");
          digitalWrite(motorAPin1, 0);
          digitalWrite(motorAPin2, 0);
          digitalWrite(motorBPin1, 0);
          digitalWrite(motorBPin2, 0);
          break;

    } //end of state_machine SWITCH
    if(state_machine>=5){
      state_machine = 0; 
    }
    time_trig = millis(); 
  }  //end of while_millis


} //end of main_loop

int digitalReadOutputPin(uint8_t pin)
{
  uint8_t bit = digitalPinToBitMask(pin);
  uint8_t port = digitalPinToPort(pin);
  if (port == NOT_A_PIN) 
    return LOW;

  return (*portOutputRegister(port) & bit) ? HIGH : LOW;
}