#include <Arduino.h>
#include <Wire.h>
#include <HardwareSerial.h>
#include <math.h>

#define ADDRESS 0x1E
#define CONFIG_A 0x00
#define CONFIG_B 0x01
#define MODE 0x02
#define X_MSB 0x03
#define Z_MSB 0x05
#define Y_MSB 0x07

#define COMPASS_READ_INTERVAL 100

uint32_t timePassed = millis(),
  prevTimeHeartbeat = millis(),
  prevTimeCompassRead = millis();

HardwareSerial SerialPort(2);

void gpsRead() {
  if (SerialPort.available() > 0) {
    Serial.print((char)SerialPort.read());
  }
}

int readRawData(int addr) {
  Wire.beginTransmission(ADDRESS);
  Wire.write(addr);
  Wire.endTransmission();
  
  Wire.requestFrom(ADDRESS, 2);
  
  int value = Wire.read() << 8 | Wire.read();
  
  if (value > 32768) {
    value -= 65536;
  }
  
  return value;
}

void compassRead() {
  if (timePassed - prevTimeCompassRead < 
    COMPASS_READ_INTERVAL) return;

  int x, y, z;
  
  x = readRawData(X_MSB);
  y = readRawData(Y_MSB);
  z = readRawData(Z_MSB);

  float heading = atan2(y, x);

  // float declinationAngle = 1.46607;
  // heading += declinationAngle;

  // Correct for when signs are reversed.
  if (heading < 0) heading += 2*PI;

  // Check for wrap due to addition of declination.
  if (heading > 2*PI) heading -= 2*PI;

  // Reverse rotation
  heading = 2*PI - heading;

  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI; 

  // Show Values
  Serial.print((float)micros() / 1000, 3);
  Serial.print("\t");
  Serial.print("Raw: ");
  Serial.print(x);
  Serial.print("  ");   
  Serial.print(y);
  Serial.print("  ");   
  Serial.print(z);
  Serial.print("   Heading: ");
  Serial.println(headingDegrees);

  // Serial.print(x, 6); Serial.print(",");
  // Serial.print(y, 6); Serial.print(",");
  // Serial.println(z, 6);

  prevTimeCompassRead = timePassed;
}

void printHeartbeat() {
  if (timePassed - prevTimeHeartbeat < 1000) return;
  
  float us = (float)micros() / 1000.0;
  Serial.print("> ");
  Serial.println(us, 3);
  prevTimeHeartbeat = timePassed;
}

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

  SerialPort.begin(9600, SERIAL_8N1, 16, 17);
  SerialPort.print("abc");

  Wire.begin();

  Wire.beginTransmission(ADDRESS);
  Wire.write(CONFIG_A);
  Wire.write(0x70);
  Wire.endTransmission();
  Wire.beginTransmission(ADDRESS);
  Wire.write(CONFIG_B);
  Wire.write(0x20);
  Wire.endTransmission();
  Wire.beginTransmission(ADDRESS);
  Wire.write(MODE);
  Wire.write(0x00);
  Wire.endTransmission();
}

void loop() {
  timePassed = millis();

  // printHeartbeat();
  // gpsRead();
  compassRead();

  delay(1);
}