#include <Arduino.h>

#include <Wire.h>
#include <stdint.h>
#include "TouchScreen.h"
#include <Servo.h>

///////////////////
//* Touch Screen //
///////////////////
#define YP 26 // must be an analog pin, use "An" notation!  //' Fio branco
#define XM 27 // must be an analog pin, use "An" notation!  //! Fio Azul
#define YM 14                                               //# Fio Marrom
#define XP 12                                               //? Fio Vermelho

TouchScreen ts = TouchScreen(XP, YP, XM, YM, 246);

TSPoint p; // a point object that holds x and y coordinates

int min_max_touch[2][2] = {
  {0, 1023}, // valores mínimos e máximos de leitura do eixo X do touch //? *wokwi
  {0, 1023}  // valores mínimos e máximos de leitura do eixo X do touch //? *wokwi
};

int centro[2] = {(min_max_touch[0][1] - min_max_touch[0][0]) / 2,  // centro do eixo X do touch
                 (min_max_touch[1][1] - min_max_touch[1][0]) / 2}; // centro do eixo Y do touch

int touch_default[2] = {1023, 1023}; // valores do padrão touch, quando não há nada o pressionando //? *wokwi

/////////////////////
//* Servo (output) //
/////////////////////
byte qnt_servos = 2,           // quantidade de servos 
     servo_pins[2] = {11, 10}; // pinos dos servos //? *wokwi

Servo servo[2];

//////////
//* PID //
//////////
float proporcional,         // erro no sistema
      integral[2] = {0, 0}, // acumulo da correção no sistema (de cada eixo)
      derivada;             // variação no sistema
      
float Kp[2] = {1, 1}, // constantes P
            Ki[2] = {0, 0}, // constantes I 
            Kd[2] = {0, 0}; // constantes D
//. 1, .01, 135;

//' Vars. para guardar valores dos cálculos //
//# Para P do PID //
float posicao[2]; // X e Y no touch

//! Para I do PID //
// (mesma variável de P)

//* Para D do PID //
// (mesma variável de P) +
float ultima_posicao[2],  // verficar variação da posição (de cada eixo)
      tempo_atual,        // guardar tempo atual
      ultimo_tempo[2];    // variação de tempo

//? PID total //
float total = 0; // angulo final dos cálculos


//' Constantes do hardware //
int ang_default[2] = {45, 135}, // angulo padrão d cada eixo (mínimo e máximo são em relação ao padrão) //? *wokwi
    ang_min[2] = {-45, -45},     // angulos máximo de cada eixo (padrão - mínimo)                       //? *wokwi
    ang_max[2] = {45, 45};   // angulos mínimo de cada eixo (mínimo - padrão)                           //? *wokwi

//' Prototipos das funções //
void PegarPosicao(), Atualizar(int posicao, byte index), Proporcao(int pos, byte index), Integracao(int pos, int index), Derivacao(int pos, byte index), LimparBuffer(), Calibrar();

void setup()
{
  Serial.begin(9600);
  while (!Serial)
    ;
  Serial.println("Iniciando...");
  delay(500);

  //' Servos setup //
  servo[0].attach(servo_pins[0]);
  servo[1].attach(servo_pins[1]);

  for (int i = 0; i < qnt_servos; i++)
  {
    servo[i].write(ang_default[i] + ang_min[i]);
    delay(1000);

    servo[i].write(ang_default[i] + ang_max[i]);
    delay(1000);

    servo[i].write(ang_default[i]);
    delay(1000);
  }
}

void loop()
{
  Calibrar();

  PegarPosicao();

  if (!(posicao[0] == centro[0] && posicao[1] == centro[1]) && !(posicao[0] == touch_default[0] && posicao[1] == touch_default[1])) // se a bola não está no meio da touch, mas está na touch:
  {
    for (int i = 0; i < 2; i++)
    {
      Atualizar(posicao[i], i); // calcular PID no eixo do index (0 = X, 1 = Y)
    }
  }

  else
  {
    for (int i = 0; i < 2; i++)
    {
      integral[i] = 0;
    }
  }
}


void PegarPosicao()
{
  p = ts.getPoint();

  posicao[0] = analogRead(A0);  // atualizar posição X da bola //? *wokwi
  posicao[1] = analogRead(A1);  // atualizar posição Y da bola //? *wokwi

  Serial.println("Pos: [0] " + String(posicao[0]) + " [1] " + String(posicao[1])); //! *4
}

void Atualizar(int posicao_atual, byte index) // atualiza o valor de ´ultimaPosicao´ se necessário
{
  Proporcao(posicao_atual, index);
  Integracao(posicao_atual, index);
  Derivacao(posicao_atual, index);

  total = ang_default[index] + proporcional + integral[index] + derivada;
  total = constrain(total, ang_default[index] + ang_min[index], ang_default[index] + ang_max[index]);
  
  Serial.println("Output[" + String(index) + "]: " + String(proporcional + integral[index] + derivada + ang_default[index]) + "°");
  
  servo[index].write(proporcional + integral[index] + derivada + ang_default[index]);
  
}

void Proporcao(int pos, byte index) // verificar o erro
{
  proporcional = Kp[index] * map(pos, min_max_touch[index][0], min_max_touch[index][1], ang_min[index], ang_max[index]);

  Serial.println("P: " + String(proporcional));
}

void Integracao(int pos, int index) // verificar o erro em relação ao tempo
{
  Serial.println("I[" + String(index) + "]: " + String(integral[index]) + " + " + String(Ki[index] * map(pos, min_max_touch[index][0], min_max_touch[index][1], ang_min[index], ang_max[index])));

  integral[index] += Ki[index] * map(pos, min_max_touch[index][0], min_max_touch[index][1], ang_min[index], ang_max[index]);
}

void Derivacao(int pos, byte index) // verificar a variação
{
  tempo_atual = millis();

  derivada = Kd[index] * (pos - ultima_posicao[index]) / (tempo_atual - ultimo_tempo[index]);

  Serial.println("D: " + String(derivada));

  ultimo_tempo[index] = tempo_atual;

  if (posicao[index] != ultima_posicao[index])
  {
    Serial.println("Distância: " + String(posicao[index]));

    ultima_posicao[index] = posicao[index];
  }

}

void Calibrar()
{
  if (Serial.available() > 0 && Serial.read() == 'c')
  {
    LimparBuffer();

    for (int i = 0; i < qnt_servos; i++)
    {
      servo[i].write(ang_default[i]);
      delay(1000);
    }

    while (1)
    {
      //# Selecionar eixo
      Serial.println("Selecione um eixo: \n1 para eixo X [0] \n2 para eixo Y [1]");

      while (Serial.available() == 0)
        ;

      byte axis = Serial.parseInt();

      if (axis != 1 && axis != 2)
      {
        Serial.println("Eixo inválido");
        return;
      }

      axis--;

      Serial.println("Eixo " + String(axis) + " selecionado");

      Serial.println("Kp <valor> → mudar valor da constante P \nKi <valor> → mudar valor da constante I \nKd <valor> → mudar valor da constante D");
      Serial.println("touch min <valor> → mudar valor de input mínimo do touch \ntouch max <valor> → mudar valor de input máximo do touch");
      Serial.println("ang → mudar os valores de ângulo\n");

      LimparBuffer();

      while (Serial.available() == 0)
        ;

      //# selecionar variável
      while (Serial.available() > 0)
      {
        String input = Serial.readStringUntil('\n');
        // input.trim(); // Remove any leading or trailing whitespace

        //# constantes do PID
        if (input.startsWith("Kp"))
        {
          Kp[axis] = input.substring(3).toFloat();

          Serial.println("Kp alterada para " + String(Kp[axis]) + " no eixo " + String(axis));
          return;
        }
        else if (input.startsWith("Ki"))
        {
          Ki[axis] = input.substring(3).toFloat();

          Serial.println("Ki alterada para " + String(Ki[axis]) + " no eixo " + String(axis));
          return;
        }
        else if (input.startsWith("Kd"))
        {
          Kd[axis] = input.substring(3).toFloat();

          Serial.println("Kd alterada para " + String(Kd[axis]) + " no eixo " + String(axis));
          return;
        }

        //# valores do touch
        else if (input.startsWith("touch min"))
        {
          min_max_touch[axis][0] = input.substring(10).toInt();

          Serial.println("Touch min alterado para " + String(min_max_touch[axis][0]) + " no eixo " + String(axis));
          return;
        }
        else if (input.startsWith("touch max"))
        {
          min_max_touch[axis][1] = input.substring(10).toInt();

          Serial.println("Touch max alterado para " + String(min_max_touch[axis][1]) + " no eixo " + String(axis));
          return;
        }

        //# ângulos
        else if (input.startsWith("ang"))
        {
          LimparBuffer();

          //' angulo mínimo
          Serial.println("angulo mínimo:");
          while (Serial.available() == 0)
            ;

          if (Serial.readStringUntil('\n').toInt() > ang_default[axis])
          {
            Serial.println("Valor inválido");
            return;
          }
          
          input = Serial.readStringUntil('\n');

          ang_min[axis] = input.toInt();

          LimparBuffer();

          //' angulo padrão
          Serial.println("angulo padrão:");
          while (Serial.available() == 0)
            ;

          input = Serial.readStringUntil('\n');

          ang_default[axis] = input.toInt();

          LimparBuffer();

          //' angulo máximo
          Serial.println("angulo máximo:");
          while (Serial.available() == 0)
            ;

          if (Serial.readStringUntil('\n').toFloat() < ang_default[axis])
          {
            Serial.println("Valor inválido");
            return;
          }

          input = Serial.readStringUntil('\n');

          ang_max[axis] = input.toInt();


          Serial.println("Angulos alterados \nmínimo: " + String(ang_min[axis]) + "°, padrão: " + String(ang_default[axis]) + "°, máximo: " + String(ang_max[axis]) + "° no eixo " + String(axis));
          return;
        }

        else
        {
          Serial.println("Comando inválido");
        }
      }
    }
  }
}


void LimparBuffer()
{
  while (Serial.available() > 0)
  {
    Serial.read();
  }
}



/*
! *4 build diz que ´Serial.print("..." + String(var))´ é possível sem ´String()´
  ! testar (não funciona no wokwi)

* derivada não tem mínimo e máximo por não se saber um mínimo ou máximo da velocidade

* servos estão girando no sentido anti-horário, por isso os valores dos ângulos estão "invertidos"

# fazer map2() para levar em conta valores decimais

# add WriteMicroseconds() para melhor precisão
*/