#include <SoftwareSerial.h>
#include <Wire.h>

#define HMC5883_ADDRESS_MAG            (0x3C >> 1)         // 0011110x



typedef enum {
  HMC5883_REGISTER_MAG_CRA_REG_M = 0x00,
  HMC5883_REGISTER_MAG_CRB_REG_M = 0x01,
  HMC5883_REGISTER_MAG_MR_REG_M = 0x02,
  HMC5883_REGISTER_MAG_OUT_X_H_M = 0x03,
  HMC5883_REGISTER_MAG_OUT_X_L_M = 0x04,
  HMC5883_REGISTER_MAG_OUT_Z_H_M = 0x05,
  HMC5883_REGISTER_MAG_OUT_Z_L_M = 0x06,
  HMC5883_REGISTER_MAG_OUT_Y_H_M = 0x07,
  HMC5883_REGISTER_MAG_OUT_Y_L_M = 0x08,
  HMC5883_REGISTER_MAG_SR_REG_Mg = 0x09,
  HMC5883_REGISTER_MAG_IRA_REG_M = 0x0A,
  HMC5883_REGISTER_MAG_IRB_REG_M = 0x0B,
  HMC5883_REGISTER_MAG_IRC_REG_M = 0x0C,
  HMC5883_REGISTER_MAG_TEMP_OUT_H_M = 0x31,
  HMC5883_REGISTER_MAG_TEMP_OUT_L_M = 0x32
} hmc5883MagRegisters_t;

void setup() {
  Serial.begin(9600);
}

void loop() {
      Wire.beginTransmission((byte)HMC5883_ADDRESS_MAG);
      Wire.write(HMC5883_REGISTER_MAG_OUT_X_H_M);
      Wire.endTransmission(false);
      Wire.requestFrom((byte)HMC5883_ADDRESS_MAG, (byte)6, true);  
      uint8_t xa = Wire.read();
      uint8_t xb = Wire.read();
      uint8_t ya = Wire.read();
      uint8_t yb = Wire.read();
      uint8_t za = Wire.read();
      uint8_t zb = Wire.read();
      int magx = (int16_t)(xb | ((int16_t)xa << 8));
      int magy = (int16_t)(yb | ((int16_t)ya << 8));
      int magz = (int16_t)(zb | ((int16_t)za << 8));
      /*Serial.print("x = "); Serial.print(magx);
      Serial.print(",Y = "); Serial.print(magy);
      Serial.print(",Z = "); Serial.print(magz);*/

      float heading = atan2(magy, magx) * 180 / PI;
      if (heading <0) heading = 360 + heading;
      Serial.print("Heading = "); Serial.println(heading);


        //Wire.endTransmission(true);

  delay(250);
}