// start

// PINS DEFINITION

const int encoder_pin_A = 25,  // pulled up externally, and a 100nF cap to Gnd
          encoder_pin_B = 33,  // pulled up externally, and a 100nF cap to Gnd

          motor_input_A1 = 26,
          motor_input_A2 = 27,
          


          not_used_pin_here = 99;


// END PINS


#ifndef CONFIG_IDF_TARGET_ESP32
#error "Please Choose ESP32 Dev Board !!!"
#endif

// variables

// encoder variables

uint32_t interval_to_calculate_rpm = 200; // in millisecs
float rpm_multiplier = 1.0;

float actual_rpm = 0;
//

int previous_output;
int encoder_count;
int going_clockwise = 0;



uint32_t rpm_counter = 0;
uint32_t snap_start_timer = 0;

// motor variables
// Setting PWM properties
uint16_t freq = 50000; // 50kHz
int resolution = 8; // this gives 0-255, 12 gives 0-4095
int channel_input_1 = 1;
int channel_input_2 = 2;

const bool motor_cw = false,
           motor_ccw = !motor_cw;

// task handlers
TaskHandle_t encoder_task,
             motor_task;

void setup() {
  Serial.begin(115200);
  Serial.println("\n\nBooting...");


  create_tasks_here();

}


void loop() {
  encoder_debug_task();
}

void create_tasks_here() {
  // disable watchdogs on both CPUs so multitasking could run properly
  disableCore0WDT();
  disableCore1WDT();

  // create tasks
  xTaskCreate(encoder_task_loop, "encoder_task_loop", 1 * 1024, NULL, 1, &encoder_task);
  delay(20);

  xTaskCreate(motor_task_loop, "motor_task_loop", 1 * 1024, NULL, 2, &motor_task);
  delay(20);
}


void motor_task_loop(void *parameter) {
  pinMode(motor_input_A1, OUTPUT);
  pinMode(motor_input_A2, OUTPUT);

  ledcSetup(channel_input_1 /* LEDChannel */, freq /* freq */, resolution /* resolution */);
  ledcAttachPin(motor_input_A1, channel_input_1 /* LEDChannel */);

  ledcSetup(channel_input_2 /* LEDChannel */, freq /* freq */, resolution /* resolution */);
  ledcAttachPin(motor_input_A2, channel_input_2 /* LEDChannel */);

  set_motor_speed_and_direction(0, motor_cw);

  while (1) {
    delay(1);

    Serial.println("Going Clockwise accelerating");
    for (int x = 0; x < 100; x++) { //
      set_motor_speed_and_direction(x, motor_cw);
      delay(100); // total of 10000ms == 10 seconds
    }


    Serial.println("Stay at maximum speed for 5 secs");
    set_motor_speed_and_direction(100, motor_cw);
    delay(5000);

    Serial.println("Going Counter Clockwise decelerating");
    for (int x = 100; x > 0; x--) { //
      set_motor_speed_and_direction(x, motor_ccw);
      delay(100); // total of 10000ms == 10 seconds
    }

    Serial.println("Stay at 0 speed for 5 secs");
    set_motor_speed_and_direction(0, motor_ccw);
    delay(5000);


  }
}

// eg: set_motor_speed_and_direction(45,motor_cw) means 45% of maximuam speed in motor_cw == clockwise direction, motor_ccw == counter clockwise
void set_motor_speed_and_direction(int speed_perc, bool direction_) {
  //ledcWrite(1 /* LEDChannel */, 0); /* 0-255 */
  int duty_cycle = map(0, 255, 0, 100, speed_perc);
  // ledcWrite(channel_input_1, duty_cycle);
  // ledcWrite(channel_input_2, duty_cycle);
  if (speed_perc == 0) { // both LOW
    ledcWrite(channel_input_1, 0);
    ledcWrite(channel_input_2, 0);
    return;
  }

  if (direction_) {
    ledcWrite(channel_input_1, duty_cycle);
    ledcWrite(channel_input_2, 0);
  }
  else {
    ledcWrite(channel_input_1, 0);
    ledcWrite(channel_input_2, duty_cycle);
  }
}



void encoder_task_loop(void *parameter) {
  pinMode(encoder_pin_A, INPUT);
  pinMode(encoder_pin_B, INPUT);
  previous_output = digitalRead(encoder_pin_A); //Read the inital value of Output A

  while (1) {
    delay(1);
    static int flag = 0;
    static uint32_t encoder_moved_timer = 0;
    static uint32_t debounce_last = 0;
    uint32_t current_millis = millis();

    //  if (digitalRead(encoder_pin_A) != previous_output && current_millis - debounce_last >=10  ) {
    //   debounce_last = current_millis;

    if (digitalRead(encoder_pin_A) != previous_output) {
      if (digitalRead(encoder_pin_B) != previous_output) {
        encoder_count++;
        going_clockwise = 0;
        if ( flag != 3 ) {
          flag = 3;
          rpm_counter = 0; // reset this once as change in direction detected
          snap_start_timer = current_millis; // capture time
        }
        rpm_counter++; // increment
      }
      else {
        encoder_count--;
        going_clockwise = 1;
        if ( flag != 4 ) {
          flag = 4;
          rpm_counter = 0; // reset this once as change in direction detected
          snap_start_timer = current_millis; // capture time
        }
        rpm_counter++; // increment
      }
      encoder_moved_timer = current_millis;
    }
    else { // encoder hasn't moved in a long time, reset these
      if ( current_millis - encoder_moved_timer >= 1000UL ) {
        rpm_counter = 0;
        flag = 0;
      }
    }

    previous_output = digitalRead(encoder_pin_A);

    // getting RPM here


    if ( current_millis - snap_start_timer >= interval_to_calculate_rpm ) {
      actual_rpm = 1000.0 * rpm_multiplier * rpm_counter / (1.0 * interval_to_calculate_rpm); // that 1000 is because we have a intervbal in ms, to change to secs
      actual_rpm = 60 * actual_rpm; // change from RPS to RPM


      // reset these
      snap_start_timer = current_millis;
      rpm_counter = 0;
    }

  }

}

void encoder_debug_task() { // for serial printing
  static int prev_count = 0;
  static const char* direction_[] = {"clockwise", "anti-clockwise"};
  if ( prev_count != encoder_count ) {
    prev_count = encoder_count;
    Serial.printf("counter: %d\ngoing: %s\nRPM: %0.1f\n",
                  encoder_count, direction_[going_clockwise],
                  actual_rpm
                 );
  }
  uint32_t current_millis = millis();
  static uint32_t aa = current_millis;
  if ( current_millis - aa >= 200 ) {
    aa = current_millis;
    Serial.printf("RPM: %0.1f\n",
                  actual_rpm
                 );
  }
}









// end