#include <Arduino.h> // if using platformio


#include <Servo.h>


// servo
  Servo servo[2];
  const byte servoPin[] = {11, 10};
  const byte qntServos = 2;

  byte ang_def[] = { 90,  90},
       ang_min[] = {  0,   0},
       ang_max[] = {180, 180},
       ang_velocidade[] = {4, 4};

  int ang_current[] = {ang_def[0], ang_def[1]};


// bluetooth
  char BTvalue;


// leds
const byte led[] = {6, 5};
const byte ledsN = 2;


void setup()
{
  inicializar();
}

void loop()
{
  BT();
  
  delay(1);
}



void inicializar()
{
  // serials setup
  Serial.begin(9600);
  while (!Serial)
    ;
  Serial.println("Start");


  // servos setup
  for (int i = 0; i < qntServos; i++)
  {
    servo[i].attach(servoPin[i]);

    servo[i].write(ang_min[i]);
    delay(550);
    servo[i].write(ang_max[i]);
    delay(550);
    servo[i].write(ang_def[i]);
    delay(550);
  }


  // led setup
  for (int i = 0; i < ledsN; i++)
  {
    pinMode(led[i], OUTPUT);
  }
}

void BT()
{
  while (Serial.available() > 0)
  {
    BTvalue = Serial.read();

    Serial.println("SerialBT: " + String(BTvalue));

    switch (BTvalue)
    {
      //# X axis
      case '1':
        control(0, 1);
        break;
      case '3':
        control(0, -1);
        break;

      //# Y axis
      case '4':
        control(1, 1);
        break;
      case '2':
        control(1, -1);
        break;

      default:
        break;
    }

    Serial.println("X: " + String(ang_current[0]) + " -- Y: " + String(ang_current[1]));

    // clear the buffer
    while (Serial.available() > 0)
    {
      Serial.read();
    }
  
    delay(4);
  }
}

void control(byte index, int sign)
{
  ang_current[index] += ang_velocidade[index] * sign;
  ang_current[index] =  constrain(ang_current[index], ang_min[index], ang_max[index]);
  servo[index].write(ang_current[index]);
  analogWrite(led[index], map(ang_current[index], 0, 180, 0, 255));
}