#include  <Arduino.h>
  #include  "espnow.h"
  #include  "motor.h"
  #include  "servo.h"
  #include  "fuzzy.h"

  #define   terminal    Serial

  void setup() {

    // Terminal
    terminal.begin(115200);

    terminal.println("Hello");

    // ESPNOW Communications
    espnow_begin();

    // Initialize Motors
    motor1.begin();
    motor2.begin();

    // Initialize Servos
    servo1.begin();
    servo2.begin();

    // Print Terminal Message
    // terminal.println("Initialization complete.");
    
  }

  void loop() {

    terminal.print(data.X);   // 0% to 100%
    terminal.print(" ");
    terminal.print(data.Y);
    terminal.print(" ");
    terminal.println(data.B);

    delay(20);

    // // Non Blocking Motor control
    // if(millis() - motorStateStartTime >= MOTOR_ACTION_DELAY) {

    //   motorStateStartTime = millis();

    //   switch(currentMotorState) {

    //     case STOPPED:
    //       Serial.println("Moving motor 1 forward...");
    //       motor1.forward();
    //       motor1.setPercentage(75); //PWM set percentage
    //       motor2.stop();
    //       currentMotorState = FORWARD_M1;
    //       break;
    //     case FORWARD_M1:
    //       Serial.println("Moving motor 1 backward...");
    //       motor1.backward();
    //       motor1.setPercentage(50); //PWM set percentage
    //       motor2.stop();
    //       currentMotorState = BACKWARD_M1;
    //       break;
    //     case BACKWARD_M1:
    //       Serial.println("Moving motor 2 forward...");
    //       motor1.stop();
    //       motor2.forward();
    //       motor2.setPercentage(25); //PWM set percentage
    //       currentMotorState = FORWARD_M2;
    //       break;
    //     case FORWARD_M2:
    //       Serial.println("Moving motor 2 backward...");
    //       motor1.stop();
    //       motor2.backward();
    //       motor2.setPercentage(100); //PWM set percentage
    //       currentMotorState = BACKWARD_M2;
    //       break;
    //     case BACKWARD_M2:
    //       Serial.println("Stopping motors ...");
    //       motor1.stop();
    //       motor2.stop();
    //       currentMotorState = STOPPED;
    //       break;

    //   }

    // }

    // // Non Blocking Servos
    // static unsigned long servoTimer = 0;
    // static int servo1Pos = SERVO1_START;
    // static int servo2Pos = SERVO2_START;
    // static bool servoForward = true;
    // unsigned long currentTime = millis();

    // if(currentTime - servoTimer >= 20) { // Adjust for speed

    //   servoTimer = currentTime;

    //   if (servoForward) {
    //     servo1Pos++;
    //     if (servo1Pos >= SERVO1_END) {
    //       servoForward = false;
    //     }
    //   } else {
    //     servo1Pos--;
    //     if (servo1Pos <= SERVO1_START) {
    //       servoForward = true;
    //     }
    //   }

    //   servo1.write(servo1Pos);

    // }

    // // Non Blocking Servos
    // static unsigned long servoTimer2 = 0;
    // static int servo2Pos2 = SERVO2_START;
    // static bool servoForward2 = true;
    // unsigned long currentTime2 = millis();

    // if(currentTime2 - servoTimer2 >= 20) { // Adjust for speed

    //   servoTimer2 = currentTime2;

    //   if (servoForward2) {
    //     servo2Pos2++;
    //     if (servo2Pos2 >= SERVO2_END) {
    //       servoForward2 = false;
    //     }
    //   } else {
    //     servo2Pos2--;
    //     if (servo2Pos2 <= SERVO2_START) {
    //       servoForward2 = true;
    //     }
    //   }

    //   servo2.write(servo2Pos2);

    // }

  }