#include <Wire.h>  // I2C communication library

//---------------------------------------------------------------------------
// Magnetic sensor variables (AS5600 or similar angle sensor)

int magnetStatus = 0;     // Stores magnet detection status

int lowbyte;              // Lower 8 bits of angle data
word highbyte;            // Higher 8 bits of angle data
int rawAngle;             // Combined 12-bit raw angle value
float degAngle;           // Angle converted to degrees

// Variables for tracking continuous rotation
int quadrantNumber, previousquadrantNumber;
float numberofTurns = 0;  // Counts full rotations
float correctedAngle = 0; // Angle adjusted relative to start
float startAngle = 0;     // Initial reference angle
float totalAngle = 0;     // Final continuous angle output

//---------------------------------------------------------------------------

void setup()
{
  Serial.begin(115200);   // Start serial communication for plotting/debugging
  Wire.begin();           // Initialize I2C communication
  Wire.setClock(800000L); // Set high-speed I2C (800 kHz)

  checkMagnetPresence();  // Ensure magnet is detected before starting

  ReadRawAngle();         // Read initial angle
  startAngle = degAngle;  // Set this as reference (zero point)
}

//---------------------------------------------------------------------------

void loop()
{
  ReadRawAngle();   // Get current angle from sensor
  correctAngle();   // Adjust angle relative to starting position
  checkQuadrant();  // Track full rotations

  // SERIAL PLOTTER OUTPUT (NO TEXT)
  // First value: angle within 0–360°
  // Second value: continuous angle (accounts for multiple turns)
  Serial.print(correctedAngle);
  Serial.print(" ");
  Serial.println(totalAngle);

  delay(50); // Small delay for smoother plotting
}

//---------------------------------------------------------------------------

// Function to read raw angle data from sensor via I2C
void ReadRawAngle()
{
  // Read LOW byte (register 0x0D)
  Wire.beginTransmission(0x36);
  Wire.write(0x0D);
  Wire.endTransmission();
  Wire.requestFrom(0x36, 1);

  while (Wire.available() == 0);
  lowbyte = Wire.read();

  // Read HIGH byte (register 0x0C)
  Wire.beginTransmission(0x36);
  Wire.write(0x0C);
  Wire.endTransmission();
  Wire.requestFrom(0x36, 1);

  while (Wire.available() == 0);
  highbyte = Wire.read();

  // Combine high and low bytes into a single 12-bit value
  highbyte = highbyte << 8;
  rawAngle = highbyte | lowbyte;

  // Convert raw value to degrees (360° / 4096 steps)
  degAngle = rawAngle * 0.087890625;
}

//---------------------------------------------------------------------------

// Adjust angle so that starting position becomes 0°
void correctAngle()
{
  correctedAngle = degAngle - startAngle;

  // Handle wrap-around (if negative, bring back into 0–360 range)
  if (correctedAngle < 0)
    correctedAngle += 360;
}

//---------------------------------------------------------------------------

// Track which quadrant the angle is in to detect full rotations
void checkQuadrant()
{
  // Divide circle into 4 quadrants
  if (correctedAngle >= 0 && correctedAngle <= 90)
    quadrantNumber = 1;
  else if (correctedAngle <= 180)
    quadrantNumber = 2;
  else if (correctedAngle <= 270)
    quadrantNumber = 3;
  else
    quadrantNumber = 4;

  // Detect transitions between quadrants
  if (quadrantNumber != previousquadrantNumber)
  {
    // Completed one full clockwise rotation
    if (quadrantNumber == 1 && previousquadrantNumber == 4)
      numberofTurns++;

    // Completed one full counter-clockwise rotation
    if (quadrantNumber == 4 && previousquadrantNumber == 1)
      numberofTurns--;

    previousquadrantNumber = quadrantNumber;
  }

  // Calculate total angle including multiple turns
  totalAngle = (numberofTurns * 360) + correctedAngle;
}

//---------------------------------------------------------------------------

// Check if magnet is properly detected by the sensor
void checkMagnetPresence()
{
  while ((magnetStatus & 32) != 32) // Loop until magnet detected
  {
    magnetStatus = 0;

    Wire.beginTransmission(0x36);
    Wire.write(0x0B); // Status register
    Wire.endTransmission();
    Wire.requestFrom(0x36, 1);

    while (Wire.available() == 0);
    magnetStatus = Wire.read();
  }
}