// STM32 Nucleo-C031C6 SPI LCD Example
// Simulation: https://wokwi.com/projects/365549388158011393

#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"
#include "STM32_ISR_Servo.h"

//SPI TFT
#define TFT_DC 2
#define TFT_CS 3
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

//Srvo
#define SERVO_PIN       D6

typedef struct
{
  int     servoIndex;
  uint8_t servoPin;
} ISR_servo_t;

#if ( defined(STM32L0) || defined(STM32L1) || defined(STM32L4) || defined(STM32L5) )

#define NUM_SERVOS        1

ISR_servo_t ISR_servo[NUM_SERVOS] =
{
  { -1, SERVO_PIN}};
#else
#endif


void setup() {
  Serial.begin(115200);
  Serial.println("Hello, WOKWI");

  //TFT Assign
  tft.begin();

  //Servo Attach
  //Select STM32 timer USE_STM32_TIMER_NO
  STM32_ISR_Servos.useTimer(USE_STM32_TIMER_NO);

  for (int index = 0; index < NUM_SERVOS; index++)
  {
    ISR_servo[index].servoIndex = STM32_ISR_Servos.setupServo(ISR_servo[index].servoPin, MIN_MICROS, MAX_MICROS);

    if (ISR_servo[index].servoIndex != -1)
    {
      Serial.print(F("Setup OK Servo index = "));
      Serial.println(ISR_servo[index].servoIndex);
    }
    else
    {
      Serial.print(F("Setup Failed Servo index = "));
      Serial.println(ISR_servo[index].servoIndex);
    }
  }

}

//Main Code
void loop() { 
  
  //operate();
  servOp();

}

//State Condition
void operate() {
  tft.setCursor(20, 100);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(3);
  tft.println("STOP");

  tft.setCursor(20, 180);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(3);
  tft.println("START");
}

volatile uint8_t counter = 0;
SIGNAL(TIMER0_COMPA_vect) {
  // this gets called every 2 milliseconds
  counter += 2;
  // every 20 milliseconds, refresh the servos!
  if (counter >= 20) {
    counter = 0;
    myServo1.refresh();
    myServo2.refresh();
  }

void printServoInfo(int indexServo)
{
  Serial.print(F("Servos idx = "));
  Serial.print(indexServo);
  Serial.print(F(", act. pos. (deg) = "));
  Serial.print(STM32_ISR_Servos.getPosition(ISR_servo[indexServo].servoIndex) );
  Serial.print(F(", pulseWidth (us) = "));
  Serial.println(STM32_ISR_Servos.getPulseWidth(ISR_servo[indexServo].servoIndex));
}

void servOp(){
    Serial.println(F("Servos sweeps from 0-180 degress"));

  for (position = 0; position <= 180; position += 1)
  {
    // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    for (int index = 0; index < NUM_SERVOS; index++)
    {
      STM32_ISR_Servos.setPosition(ISR_servo[index].servoIndex, position );
    }

    // waits 50ms for the servo to reach the position
    delay(50);
  }

  // waits 5s between test
  delay(5000);

  Serial.println(F("Servos sweeps from 180-0 degress"));

  for (position = 180; position >= 0; position -= 1)
  {
    // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    for (int index = 0; index < NUM_SERVOS; index++)
    {
      STM32_ISR_Servos.setPosition(ISR_servo[index].servoIndex, position );
    }

    // waits 50ms for the servo to reach the position
    delay(50);
  }

  // waits 5s between test
  delay(5000);

  Serial.println(F("Servos, index depending, be somewhere from 0-180 degress"));

  for (position = 0; position <= 180; position += 1)
  {
    // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    for (int index = 0; index < NUM_SERVOS; index++)
    {
      STM32_ISR_Servos.setPosition(ISR_servo[index].servoIndex, (position + index * (180 / NUM_SERVOS)) % 180 );
    }

    // waits 50ms for the servo to reach the position
    delay(50);
  }

  delay(5000);

  Serial.println(F("Servos, index depending, be somewhere from 180-0 degress"));

  for (position = 180; position >= 0; position -= 1)
  {
    // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    for (int index = 0; index < NUM_SERVOS; index++)
    {
      STM32_ISR_Servos.setPosition(ISR_servo[index].servoIndex, (position + index * (180 / NUM_SERVOS)) % 180 );
    }

    // waits 50ms for the servo to reach the position
    delay(50);
  }

  // waits 5s between test
  delay(5000);
}