/*
 * AS5600 Magnetic Encoder with Analog Gauge Display
 * Adapted to show rotation from -89° to +89° with center at 12 o'clock
 * 
 * Libraries needed:
 * - AS5600 by Rob Tillaart
 * - U8g2 by oliver
 */

#include <Wire.h>
#include <AS5600.h>
#include <U8g2lib.h>

// U8g2 constructor for SSD1306 128x64 I2C display
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

AS5600 as5600;

char buffer[20];       // helper buffer for converting values
int string_width;      // helper value for string widths

float pixel_x = 0;     // x pos for pixel
float pixel_y = 0;     // y pos for pixel
float line_x = 0;      // x pos for line end
float line_y = 0;      // y pos for line end
float text_x = 0;      // x pos for text
float text_y = 0;      // y pos for text

int center_x = 64;     // x center of the dial
int center_y = 108;    // y center of the dial (outside of screen for arc effect)
int radius_pixel = 92; // radius for pixel tickmarks
int radius_line = 87;  // radius for line end
int radius_text = 75;  // radius for text

int angle;             // angle for the individual tickmarks
int tick_value;        // numeric value for the individual tickmarks

byte precalculated_x_radius_pixel[180]; // lookup table
byte precalculated_y_radius_pixel[180]; // lookup table

float current_angle = 0;      // current encoder angle (-89 to +89)
float displayed_angle = 0;    // smoothed angle for display
float last_raw_angle = 0;     // for zero crossing detection

void setup() {
  Serial.begin(115200);
  
  // Initialize I2C
  Wire.begin();
  Wire.setClock(400000);
  
  // Initialize U8g2 display
  u8g2.begin();
  u8g2.setDrawColor(1);
  
  // Initialize AS5600
  as5600.begin(3);
  as5600.setDirection(AS5600_CLOCK_WISE);
  
  // Pre-calculate x and y positions into lookup tables for performance
  for (int i = 0; i < 180; i++) {
    precalculated_x_radius_pixel[i] = sin(radians(i - 90)) * radius_pixel + center_x;
    precalculated_y_radius_pixel[i] = -cos(radians(i - 90)) * radius_pixel + center_y;
  }
  
  // Check if AS5600 is connected
  if (as5600.isConnected()) {
    Serial.println("AS5600 detected!");
  } else {
    Serial.println("AS5600 not found!");
  }
  
  // Get initial angle
  float raw_angle = as5600.readAngle() * AS5600_RAW_TO_DEGREES;
  last_raw_angle = raw_angle;
  current_angle = map_to_centered_range(raw_angle);
  displayed_angle = current_angle;
}

void loop() {
  // Read raw angle from AS5600 (0-360°)
  float raw_angle = as5600.readAngle() * AS5600_RAW_TO_DEGREES;
  
  // Convert to centered range (-89 to +89)
  current_angle = map_to_centered_range(raw_angle);
  
  // Smooth the displayed angle for less jitter
  displayed_angle = displayed_angle * 0.8 + current_angle * 0.2;
  
  // Read magnet status
  uint8_t magnetDetect = as5600.detectMagnet();
  
  // Draw the display
  draw_display(magnetDetect);
  
  // Serial output
  Serial.print("Raw: ");
  Serial.print(raw_angle, 1);
  Serial.print("° | Centered: ");
  Serial.print(current_angle, 1);
  Serial.print("° | Mag: ");
  Serial.println(magnetDetect ? "OK" : "NONE");
  
  delay(20);  // ~50 FPS
}

// Convert 0-360° range to -89 to +89° centered at 0°
float map_to_centered_range(float raw_angle) {
  // Map 0-360 to -89 to +89 with 0° at top (12 o'clock)
  // 0° raw = 0° centered
  // 90° raw = +89° centered (max right)
  // 270° raw = -89° centered (max left)
  
  float centered;
  if (raw_angle <= 90) {
    // 0-90° raw maps to 0-89° centered
    centered = min(raw_angle, 89.0);
  } else if (raw_angle >= 270) {
    // 270-360° raw maps to -89-0° centered
    centered = max(raw_angle - 360.0, -89.0);
  } else if (raw_angle < 180) {
    // 90-180° clamp to +89°
    centered = 89.0;
  } else {
    // 180-270° clamp to -89°
    centered = -89.0;
  }
  
  return centered;
}

void draw_display(uint8_t magnet_status) {
  u8g2.clearBuffer();  // Clear the internal memory
  
  u8g2.setDrawColor(1);
  u8g2.setFont(u8g2_font_6x10_tr);  // Small font for tickmarks
  
  // Convert displayed_angle to a scaled value (multiply by 10 to match original code pattern)
  // This allows smooth scrolling between tickmarks
  int scaled_value = displayed_angle * 10;
  
  // Calculate tickmarks with smooth scrolling
  for (int i = -48; i <= 48; i = i + 3) {
    // Calculate the visual angle position with smooth scrolling offset
    angle = i + ((scaled_value * 3) / 10) % 3;
    
    // Calculate the actual degree value this tickmark represents
    tick_value = round((scaled_value / 10.0) + angle / 3.0);
    
    // Get positions from lookup table
    int lookup_angle = angle + 90;
    if (lookup_angle >= 0 && lookup_angle < 180) {
      pixel_x = precalculated_x_radius_pixel[lookup_angle];
      pixel_y = precalculated_y_radius_pixel[lookup_angle];
      
      // Only draw inside screen bounds
      if (pixel_x > 0 && pixel_x < 128 && pixel_y > 0 && pixel_y < 64) {
        
        // Only draw tickmarks in valid range (-89 to +89)
        if (tick_value >= -89 && tick_value <= 89) {
          
          if (tick_value % 10 == 0) {
            // Draw major tickmark (line + text)
            line_x = sin(radians(angle)) * radius_line + center_x;
            line_y = -cos(radians(angle)) * radius_line + center_y;
            u8g2.drawLine(pixel_x, pixel_y, line_x, line_y);
            
            text_x = sin(radians(angle)) * radius_text + center_x;
            text_y = -cos(radians(angle)) * radius_text + center_y;
            itoa(tick_value, buffer, 10);
            string_width = u8g2.getStrWidth(buffer);
            u8g2.drawStr(text_x - string_width / 2, text_y, buffer);
            
          } else {
            // Draw minor tickmark (pixel)
            u8g2.drawPixel(pixel_x, pixel_y);
          }
        }
      }
    }
  }
  
  // Draw the centered value display at top
  u8g2.setFont(u8g2_font_8x13_tr);  // Larger font
  dtostrf(displayed_angle, 1, 1, buffer);
  sprintf(buffer, "%s%s", buffer, "\xb0");  // Add degree symbol
  
  string_width = u8g2.getStrWidth(buffer);
  
  // Draw background rounded box
  u8g2.setDrawColor(1);
  u8g2.drawRBox(64 - (string_width + 4) / 2, 0, string_width + 4, 11, 2);
  u8g2.drawTriangle(64 - 3, 11, 64 + 4, 11, 64, 15);
  
  // Draw text in inverse (black on white)
  u8g2.setDrawColor(0);
  u8g2.drawStr(64 - string_width / 2, 10, buffer);
  
  // Draw magnet status indicator in corner
  u8g2.setDrawColor(1);
  u8g2.setFont(u8g2_font_5x7_tr);
  if (magnet_status == 1) {
    u8g2.drawStr(100, 8, "MAG");
  } else {
    u8g2.drawStr(98, 8, "NOMAG");
  }
  
  // Draw center reference line at 12 o'clock
  u8g2.setDrawColor(1);
  u8g2.drawLine(64, 15, 64, 25);
  u8g2.drawDisc(64, 26, 2);  // Filled circle
  
  u8g2.sendBuffer();  // Transfer internal memory to display
}