#include <Arduino.h>
#include <Wire.h>
#include <Servo.h>


/////////////////////////////////////
// Servo (output) e sensor (input) //
/////////////////////////////////////

Servo myservo;


/////////
// PID //
/////////

// Vars. para guardar resultados //

float proporcional = 0; // Erro do sistema
const float Kp = 1;

float integral = 0;    // Acumulo da correção do sistema
const float Ki = 0;//.01;

float derivada = 0;    // Variação do sistema
const float Kd = 0;//135;


// Vars. para guardar valores dos cálculos //

// Para P do PID //
float distancia;         // para verificar erro

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

// Para D do PID //
// (mesma variável de P)
float ultimaDistancia;   // para verficar variação de distância
float tempo;
float ultimoTempo = 0;   // para verificar variação de tempo

// PID total //
float total;



// prototipos das funções //
void atualizar();
void verificarProporcao();
void verificarIntegracao();
void verificarDerivacao();

void planos();


///////////////////////////////////
// Vars. para cálculo nos planos //
///////////////////////////////////

// declaração vars. plano fixo
float X4 = 0;      // = Xc4                    //*
float Y4 = 1;      // = Yc4                    //*

float Xb = 0; // = Xcb
float Yb = 0; // = Ycb

float HIP4;                                    //*

float sinAngle4;
float angleAxis4;

// declaração vars. bola (móvel) //

float HIPb;                                    //!

float sinAngleHIPb;                            //!

float angleHIPb;                               //!


double Xb4;                                     //!
double Yb4;                                     //!

void setup()
{
  Serial.begin(9600);

  HIP4 = sqrt(pow(X4, 2) + pow(Y4, 2));
  // Condicinoais de posição no plano para plano fixo
  if (X4 >= 0 && Y4 >= 0)
  {
    Serial.println("+, +");
    sinAngle4 = Y4 / HIP4;
    angleAxis4 = asin(sinAngle4) * 180 / PI;             //*
  }
  else if (X4 < 0 && Y4 >= 0)
  {
    Serial.println("-, +");
    sinAngle4 = X4 / HIP4;
    angleAxis4 = 90 + (asin(sinAngle4) * 180 / PI)*-1;   //*
  }
  else if (X4 < 0 && Y4 < 0)
  {
    Serial.println("-, -");
    sinAngle4 = Y4 / HIP4;
    angleAxis4 = 180 + (asin(sinAngle4) * 180 / PI)*-1;  //*
  }
  else if (X4 >= 0 && Y4 < 0)
  {
    Serial.println("+, -");
    sinAngle4 = X4 / HIP4;
    angleAxis4 = 270 + asin(sinAngle4) * 180 / PI;       //*
  }

  Serial.println("HIP4 = "+String(HIP4));
  Serial.println("sinAngle4 = "+String(sinAngle4));
  Serial.println("angleAxis4 = "+String(angleAxis4));


  // Servo setup
  myservo.attach(11);
  // teste servo
  myservo.write(180);
  delay(500);
  myservo.write(0);
  delay(500);
  myservo.write(90);
  delay(500);
}

void loop() {
  planos();
  delay(500);

  Yb4 = analogRead(A0);
  distancia = Yb4;

  if (!(distancia == 135 && distancia == ultimaDistancia)) // se a bola não está em inércia no meio da mesa:
  {
    atualizar();
  }
  else
  {
    integral = 0;
  }

  delay(25);


  delay(500);
}


void planos()
{
  //Xb = 1; // = Xcb
  //Yb = -sqrt(3); // = Ycb
  Serial.println("Yb = "+String(Yb));


  HIPb = sqrt(pow(Xb, 2) + pow(Yb, 2));
  Serial.println("HIPb = "+String(HIPb));

  // condicionais da bola móvel
  if (HIPb == 0)
  {
    Serial.println("~ 0 ~");
    sinAngleHIPb = 0;
    angleHIPb = 0;  //*
  }
  else if (Xb >= 0 && Yb >= 0)
  {
    Serial.println("+, +");
    sinAngleHIPb = Yb / HIPb;
    angleHIPb = asin(sinAngleHIPb) * 180 / PI;            //*
  }
  else if (Xb < 0 && Yb >= 0)
  {
    Serial.println("-, +");
    sinAngleHIPb = Xb / HIPb;
    angleHIPb = 90 + (asin(sinAngleHIPb) * 180 / PI)*-1;  //*
  }
  else if (Xb < 0 && Yb < 0)
  {
    Serial.println("-, -");
    sinAngleHIPb = Yb / HIPb;
    angleHIPb = 180 + (asin(sinAngleHIPb) * 180 / PI)*-1; //*
  }
  else if (Xb >= 0 && Yb < 0)
  {
    Serial.println("+, -");
    sinAngleHIPb = Xb / HIPb;
    angleHIPb = 270 + asin(sinAngleHIPb) * 180 / PI;      //*
  }

  Serial.println("sinAngleHIPb = "+String(sinAngleHIPb));
  Serial.println("angleHIPb = "+String(angleHIPb));

  Serial.println(String(angleHIPb) + " - " + String(angleAxis4) + " = " + String(angleHIPb - angleAxis4));
  Serial.println("HIPb: " + String(HIPb) + " * " + String(cos((angleHIPb - angleAxis4) *PI/180)));

  Yb4 = HIPb * cos((angleHIPb - angleAxis4) *PI/180/*para rad (porque ´cos()´ funciona com rad*/);
  Serial.println("Yb4 = " + String(Yb4));
}

void atualizar() // atualiza o valor de ´ultimaDistancia´ se necessário
{
  verificarProporcao();
  verificarIntegracao();
  verificarDerivacao();

  total = proporcional + integral + derivada;

  total = 90 + total; // default point

  if (total > 180)
  {
    total = 180;
  }
  else if (total < 0)
  {
    total = 0;
  }

  Serial.println("Output: " + String(total) + "°");
  myservo.write(total); // mover o servo de acordo com os valore encontrados
}

void verificarProporcao() // verificar o erro
{
  proporcional = Kp/*ConstanteP MoverServo*/ * map((distancia), -135, 135, -90, 90); // 100(mm) é o meio da mesa

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

void verificarIntegracao() // verificar o erro
{
  integral += Ki/*ConstanteI MoverServo*/ * map((distancia), -135, 135, -90, 90);

  Serial.println("I: " + String(integral));
}

void verificarDerivacao() // verificar a variação
{
  tempo = millis();

  derivada = Kd/*ConstanteD MoverServo*/ * /*velocidade*/(distancia - ultimaDistancia) / (tempo - ultimoTempo);

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

  ultimoTempo = tempo;

  if (distancia != ultimaDistancia)
  {
    Serial.println("Distância: " + String(distancia));
  }

  ultimaDistancia = distancia;
}


/*
  - Ver como se comporta com 0s
*/






/*
//versão incial gymbal 2 planos


//Gymbal control on 2 axis system

#include <Wire.h>
#include <Servo.h>


///////////
// Servo //
///////////

Servo myservos[2];
byte motorsPins[2] = {11, 10};

byte motorsAmmount = 2; // motors amount

/////////
// PID //
/////////

// angles and constants vars. //

float proportional;            // system error
const float Kp = 1;

float integral[2] = {0, 0};    // system correction accumulation
const float Ki = 0.01;

float derivative;              // system variation
const float Kd = 2;


// vars. to store calculus values //

// PID's Proportional //
float position[2]; // {X, Y}    // var. to verify error on axis (being controled by slide potentiometers)

// PID's Integral //
// (same as Proportional)

// PID's Derivative //
// (same as Proportional)
float lastPosition[2]; // to verify position variation
float time;            // to verify current time
float lastTime[2];   // to verify time variation

// PID total //
float total; // final angle output


// minimum and maximum angle and distances //
int angles[] = {-90, 90, 90}; // {minimum angle (difference to default), maximum angle (difference to default), default angle}
int distances[] = {-200, 200}; // {minimum distance (difference to point 0), maximum distance (difference to point 0)}

int angMin = angles[0];     // minimum angle
int angMax = angles[1];     // maximum angle
int angDefault = angles[2]; // default angle
int distMin = distances[0]; // minimum distance
int distMax = distances[1]; // maximum distance


// function prototypes //
void Update(int currentPos, byte index);
void Proportion(int pos);
void Integration(int pos, byte index);
void Derivation(int pos, byte index);
  
void setup()
{
  Serial.begin(9600);


  // Servo setup //
  for (int i = 0; i < motorsAmmount; i++)
  {
    myservos[i].attach(motorsPins[i]);
  
    // servo initial test
    myservos[i].write(angDefault+angMin);
    delay(400);
    myservos[i].write(angDefault+angMax);
    delay(400);
    myservos[i].write(angDefault);
    delay(400);
  }
}

void loop()
{
  position[0] = map(analogRead(A0), 0, 1023, -200, 200); // update ball X position
  position[1] = map(analogRead(A1), 0, 1023, -200, 200); // update ball Y position


  if (!(position[0] == 0 && position[1] == 0)) // if the ball is not on the center:
  {
    for (int i = 0; i < 2; i++)
    {
      Update(position[i], i); // calculate PID of given axis
    }
  }
  else
  {
    for (int i = 0; i < 2; i++)
    {
      integral[i] = 0; // reset integral var. value
    }
  }
}


void Update(int currentPos, byte index) // update servos
{
  Proportion(currentPos);
  Integration(currentPos, index);
  Derivation(currentPos, index);

  total = proportional + integral[index] + derivative;

  total =  total + angDefault; // calculated angle + default angle

  if (total > angDefault + angMax)
  {
    total = angDefault + angMax;
  }
  else if (total < angDefault + angMin)
  {
    total = angDefault + angMin;
  }

  Serial.println("Output axis [" + String(index) + "]: " + String(total) + "°");
  myservos[index].write(total);
}

void Proportion(int pos) // verify system error
{
  proportional = Kp * map(pos, distMin, distMax, angMin, angMax);

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

void Integration(int pos, byte index) // verify error in relation to time
{
  integral[index] += Ki * map(pos, distMin, distMax, angMin, angMax);

  Serial.println("I: " + String(integral[index]));
}

void Derivation(int pos, byte index) // verify system variation
{
  time = millis();

  derivative = Kd * (pos - lastPosition[index]) / (time - lastTime[index]);

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

  lastTime[index] = time;

  if (position[index] != lastPosition[index])
  {
    Serial.println("Distance: " + String(position[index]));
  }

  lastPosition[index] = position[index];
}
*/