#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

const int MPU_addr = 0x68; // I2C address of MPU6500
const int Gyro_scale = 131; // Gyroscope full-scale range (+/-250 deg/s)

int16_t gx, gy, gz;
float ra, dec;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // Wake up MPU6500
  Wire.endTransmission(true);
  
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();
}

void loop() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x43); // Starting with register 0x43 (GYRO_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true); // Read 6 registers (gyroscope)
  gx = Wire.read()<<8 | Wire.read();
  gy = Wire.read()<<8 | Wire.read();
  gz = Wire.read()<<8 | Wire.read();
  
  // Convert raw values to deg/s
  float gyro_x = gx / Gyro_scale;
  float gyro_y = gy / Gyro_scale;
  float gyro_z = gz / Gyro_scale;

  // Calculate RA and Dec angles
  ra = atan2(gyro_y, gyro_x) * 180.0 / M_PI;
  if (ra < 0) {
    ra += 360.0;
  }
  dec = atan2(gyro_z, sqrt(gyro_x * gyro_x + gyro_y * gyro_y)) * 180.0 / M_PI;

  // Convert RA and Dec angles to sexagesimal notation
  int ra_hours = ra / 15.0;
  int ra_minutes = abs(ra / 15.0 - ra_hours) * 60.0;
  float ra_seconds = (abs(ra / 15.0 - ra_hours) * 60.0 - ra_minutes) * 60.0;
  int dec_degrees = dec;
  int dec_arcminutes = abs(dec - dec_degrees) * 60.0;
  float dec_arcseconds = (abs(dec - dec_degrees) * 60.0 - dec_arcminutes) * 60.0;

  // Display the results on the OLED screen
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.print("RA: ");
  display.print(ra_hours);
  display.print(":");
  display.print(ra_minutes);
  display.print(":");
  display.print(ra_seconds, 0);
  display.print("s");
  display.setCursor(0, 20);
  display.print("Dec: ");
  display.print(dec_degrees);
  display.print("o");
  display.print(dec_arcminutes);
  display.print("'");
  display.print(dec_arcseconds,0);
  display.print("\"");
  display.display();

  // Print the results in sexagesimal notation
  Serial.print("Gyroscope: ");
  Serial.print(gyro_x);
  Serial.print(", ");
  Serial.print(gyro_y);
  Serial.print(", ");
  Serial.print(gyro_z);
  Serial.print("\tRA: ");
  Serial.print(ra_hours);
  Serial.print(":");
  Serial.print(ra_minutes);
  Serial.print(":");
  Serial.print(ra_seconds, 0);
  Serial.print("s");
  Serial.print("\tDec: ");
  Serial.print(dec_degrees);
  Serial.print("°");
  Serial.print(dec_arcminutes);
  Serial.print("'");
  Serial.print(dec_arcseconds,0);
  Serial.println("\"");

  delay(1000);
}