#include <Servo.h>

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

const int servoPin = 13;
const int trigPin = 33;
const int echoPin = 32;
int pos = 0;
int delayms = 30; 
int distance;
int rpm;

TaskHandle_t Task1;
TaskHandle_t Task2;

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT);
  myservo.attach(servoPin);  // attaches the servo on pin 9 to the servo object

  xTaskCreatePinnedToCore(
    servoSweep,          // name of the task function
    "Task1",  // name of the task
    1000,           // memory assigned for the task
    NULL,           // parameter to pass if any
    1,              // priority of task, starting from 0(Highestpriority) *IMPORTANT*( if set to 1 and there is no activity in your 2nd loop, it will reset the esp32)
    &Task1,         // Reference name of taskHandle variable
    0);             // choose core (0 or 1)

  xTaskCreatePinnedToCore(
    distCalc,          // name of the task function
    "Task2",  // name of the task
    1000,           // memory assigned for the task
    NULL,           // parameter to pass if any
    2,              // priority of task, starting from 0(Highestpriority) *IMPORTANT*( if set to 1 and there is no activity in your 2nd loop, it will reset the esp32)
    &Task2,         // Reference name of taskHandle variable
    1);             // choose core (0 or 1)
}

void loop() {
  
}

void servoSweep(void * parameter){
  Serial.print("Task1 running on core ");
  Serial.println(xPortGetCoreID());
  long timecheck1;
  for(;;){
    for (pos = 20; pos <= 160; pos += 1) { // goes from 0 degrees to 180 degrees
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      Serial.print("Servo Pos : ");
      Serial.println(pos);
      timecheck1 = millis()%delayms;
      delay(delayms-timecheck1);                       // waits 15ms for the servo to reach the position
    }
    for (pos = 160; pos >= 20; pos -= 1) { // goes from 180 degrees to 0 degrees
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      Serial.print("Servo Pos : ");
      Serial.println(pos);
      timecheck1 = millis()%delayms;
      delay(delayms-timecheck1);                       // waits 15ms for the servo to reach the position
    }
  }
}

void distCalc(void * parameter){
  Serial.print("Task2 running on core ");
  Serial.println(xPortGetCoreID());
  int dist; long duration; long timecheck2; long calctime;

  for(;;){
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);

    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW); 

    duration = pulseIn(echoPin, HIGH);

    dist = duration*0.034/2;  
    Serial.print("Dist : ");
    Serial.println(dist);
    timecheck2 = millis()%delayms;
    delay(delayms-timecheck2);
  }
}

  // timeperstep = millis();
  // Serial.print(", timeperstep : ");
  // Serial.println(timeperstep);
// int rpmcalc(int delayms,int degstart,int degend){
//   int degtotal; int rpm, long timetotal;
//   degtotal = abs(degend-degstart);
//   timetotal = delayms*degtotal;
//   rpm = (degtotal/360) 
// }