#include <Arduino_FreeRTOS.h>
#include <semphr.h>
#include "Servo.h"

QueueHandle_t queue1;
QueueHandle_t queue2;

// -----------------------iniciacao dos pinos-----------------------------
// -----------------------------sonar----------------------------------

#define ECHO_PIN 12
#define TRIG_PIN 13

// -----------------------------servo----------------------------------

#define pot  A1
#define servo  11   

Servo servoMotor;
int potReading = 0 ;

// -----------------------------stepper-motor----------------------------------

typedef struct {
  int STEP_PIN;
  int DIR_PIN;
  SemaphoreHandle_t semaphore;
} StepperMotor;

SemaphoreHandle_t motorA_semaphore;
SemaphoreHandle_t motorB_semaphore;

static StepperMotor motorA = {18, 19, NULL};
static StepperMotor motorB = {16, 17, NULL};


void setup() {

  
  Serial.begin(9600);

  queue1 = xQueueCreate(10, sizeof(double));
  queue2 = xQueueCreate(10, sizeof(float));

  motorA.semaphore = xSemaphoreCreateBinary();
  motorB.semaphore = xSemaphoreCreateBinary();


  xTaskCreate(vGetPotentiomenterTask, "Task 1 para leitura do potenciômetro", 1000, NULL, 1, NULL);
  xTaskCreate(vSetServoTask, "Task 3 para o acionamento do servomotor", 1000, NULL, 2, NULL);

  xTaskCreate(vSonarTask, "Task 2 para leitura do sensor ultrassônico", 1000, NULL, 1, NULL);
  xTaskCreate(vTaskControl, "Task 4 para o acionamento dos dois motores de passo", 1000, NULL, 2, NULL);

  xTaskCreate(vStepperMotorTask, "Stepper_A", 1000, &motorA, 3, NULL);
  xTaskCreate(vStepperMotorTask, "Stepper_B", 1000, &motorB, 3, NULL);

}

void loop() {

// Tudo é executado nas tarefas. Não há nada a ser feito aqui.

}

// -----------------------------functions---------------------------------

float readDistanceCM(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  int duration = pulseIn(echo, HIGH);

  return duration * 0.034 / 2;
}

/*PotentiometerToServo getPotentiometer () {

  PotentiometerToServo p;
  p.potReading = analogRead(pot); 
  p.servoValue = p.potReading * (180 / 1023.0); 
  servoMotor.write(p.servoValue);

  return p;
}*/

void ControlStepperMotor (int stepPin, int dirPin ){

  digitalWrite(dirPin, LOW);
  // faz 200 pulsos para fazer uma rotação de ciclo completo
  
  for(int x = 0; x < 200; x++) {
    digitalWrite(stepPin,HIGH);
    delayMicroseconds(500);
    digitalWrite(stepPin,LOW);
    delayMicroseconds(500);
  }
  
}

// -----------------------------tasks---------------------------------

void vSonarTask(void *pvParameters) {
  
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  float distance;

  while (1) {
    distance = readDistanceCM(TRIG_PIN,ECHO_PIN);

    xQueueSend(queue2, &distance, portMAX_DELAY);

    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void vGetPotentiomenterTask(void *pvParameters) {
    
  pinMode(pot, INPUT); 
  int potReading;

  while (1) {

    potReading  = analogRead(pot); 
    xQueueSend(queue1, &potReading, portMAX_DELAY);

    Serial.print("Analog Reading: ");
    Serial.print(potReading);

    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void vSetServoTask(void *pvParameters) {
    
  servoMotor.attach(servo);
  int potReading;
  float servoValue;
  while (1) {
    if (xQueueReceive(queue1, &potReading, portMAX_DELAY) == pdTRUE) {
      servoValue = potReading * (180 / 1023.0);
      servoMotor.write(servoValue);

      Serial.print(", Degree Of Servo: ");
      Serial.println( servoValue);
      Serial.print("\n"); 

    }
    
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void vStepperMotorTask(void *pvParameters) {
  
  StepperMotor *stepperMotor = (StepperMotor *)pvParameters;
  pinMode(stepperMotor->STEP_PIN, OUTPUT);
  pinMode(stepperMotor->DIR_PIN, OUTPUT);

  while (1) {
     if (xSemaphoreTake(stepperMotor->semaphore, portMAX_DELAY) == pdTRUE) {
        ControlStepperMotor(stepperMotor->STEP_PIN, stepperMotor->DIR_PIN);
     }
  }
}

void vTaskControl(void *pvParameters) {

  float distance;

  while (1) {
    if (xQueueReceive(queue2, &distance, portMAX_DELAY) == pdTRUE) {
      Serial.print("distance: ");
      Serial.print(distance);
      Serial.print("  cm.\n");
      if(distance > 50) {
        xSemaphoreGive(motorA.semaphore);
        xSemaphoreGive(motorB.semaphore);
      }
    }
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}