/*
  https://randomnerdtutorials.com/esp32-dual-core-arduino-ide/
*/

TaskHandle_t Task1;

#include <pwmWrite.h>

const int servoPin = 5;

// units in degrees per second
float speed = 60;

// When easing constant (ke) < 1.0, return value is normalized, when 1.0, returns pulse width (μs)
// ke = 0.0 is linear, between 0.0 and 1.0 is tunable sigmoid, 1.0 is normal response
// Normalized Tunable Sigmoid: https://www.desmos.com/calculator/ejkcwglzd1
float ke = 0.5;
float ye;

// go to position (degrees)
uint8_t pos = 0;

// duration of travel in ms is degrees to move / speed * 1000
float dur = 180.0 / speed * 1000.0;

uint32_t prevMs;

Pwm pwm = Pwm();

void setup() {
  Serial.begin(115200);

  xTaskCreatePinnedToCore(
    Task1code,   /* Task function. */
    "Task1",     /* name of task. */
    10000,       /* Stack size of task */
    NULL,        /* parameter of the task */
    1,           /* priority of the task */
    &Task1,      /* Task handle to keep track of created task */
    0);          /* pin task to core 0 */
  delay(500);
}

void loop() {
  delay(5000);
}

//Task1code: pwmOut follows pwmIn
void Task1code( void * pvParameters ) {
  for (;;) {
    uint32_t ms = millis();
    if (ms - prevMs > dur) {
      prevMs = ms;
      pos = (pos == 0) ? 180 : 0;
    }
    ye = pwm.writeServo(servoPin, pos, speed, ke);
    Serial.println(ye);
    delay(15);
  }
}