#include <Wire.h>

#define AS5600_ADDRESS 0x36  // Endereço I2C do AS5600
int pino = 34;
int _Angulo_Offset = 0;

void setup() {
  Serial.begin(115200);
  pinMode(pino, INPUT);
  Wire.begin();
}

void loop() {
  int16_t angle = 0; //readAS5600Angle();
  double angle2 = AferirPosicao();
  Serial.print("Ângulo I2C: ");
  Serial.print(angle);
  Serial.print(", Ângulo PWM: ");
  Serial.println(angle2);
  
  delay(1000); // Espera 1 segundo antes de fazer a próxima leitura
}

int16_t readAS5600Angle() {
  Wire.beginTransmission(AS5600_ADDRESS);  // Inicia comunicação com o sensor
  Wire.write(0x0E); // Endereço do registro que contém o valor do ângulo
  Wire.endTransmission(false); // Termina a transmissão

  // Inicia a transmissão para leitura de dados
  Wire.requestFrom(AS5600_ADDRESS, 2); // Lê 2 bytes (16 bits) do registro de ângulo
  
  // Espera até que os dados estejam disponíveis
  while (Wire.available() < 2) {
    delay(1);
  }

  // Lê os dados
  uint16_t rawData = Wire.read();
  rawData = rawData << 8 | Wire.read(); // Junta os bytes para formar um inteiro

  // Converte o valor para um ângulo em graus
  float angle = rawData * 360.0 / 4096.0;

  return angle;
}

double AferirPosicao() {
  // Lê o valor analógico do pino conectado à saída analógica do AS5600
  int sensorValue = analogRead(pino);
  // Converte a leitura para ângulo (0V a 3.3V corresponde a 0 a 360 graus)
  double _angulo = (sensorValue / 4095.0) * 360.0;
  // Ajusta o ângulo com o offset
  double angulo_com_offset = _angulo - _Angulo_Offset; // Considerando que o offset pode ser positivo ou negativo
  // Normaliza o ângulo para o intervalo de -180 a 180 graus
  while (angulo_com_offset > 180) angulo_com_offset -= 360;
  while (angulo_com_offset < -180) angulo_com_offset += 360;
  return angulo_com_offset;
}