#ifndef COMPASS_HDMM01_h
#define COMPASS_HDMM01_h

#include <Arduino.h>
#include "TypesLib.h"


class CompassHDMM01
{
  public:
    static const int HDMM01_1 = 0x30;
    static const int HDMM01_2 = 0x32;
    static const int HDMM01_3 = 0x34;
    static const int HDMM01_4 = 0x36;
    
    static const byte TAKE_MEASURE = 0x01;
    static const byte SET_COIL = 0x02;
    static const byte RESET_COIL = 0x04;
    
    int i2addr;
    Vec2i offs;
    double declination;
    
    CompassHDMM01(int i2addr);
    void resetSetCoil();
    Vec2i getReading(bool resetCoil);
    void calibrateMinMax(long num, long t, bool resetCoil);
    void calibrateAvg(long num, long t, bool resetCoil);
    void calibrateLS(long num, long t, bool resetCoil);
    
    // calculate the angle of the magnetic field in grad
    // 0 Deg = North
    // 90 Deg = East
    // +-180 Deg = South
    // -90 Deg = West
    template <typename T> 
    double calcAngleDeg(Vec2<T> const &reading)
    {
      double angle = atan2(reading.y, reading.x) * 180.0 / 3.1415927410 + declination;
      return angle;
    }

    // calculate the angle of the magnetic field in rads
    // 0 Rad = North
    // PI/2 Rad = East
    // +-PI Rad = South
    // -PI/2 Rad = West
    template <typename T> 
    double calcAngleRad(Vec2<T> const &reading)
    {
      double angle = atan2(reading.y, reading.x) + declination * PI / 180.0;
      return angle;
    }

    // caclulate the strenght of the magnetic field in gauss
    static double calcStrength(Vec2i const &reading)
    {
      double x = (double)reading.x / 512.0;
      double y = (double)reading.y / 512.0;   
      double strength = sqrt(x*x + y*y);
      return strength;
    }
    
    // converts the reading to gauss
    static Vec2d convertGauss(Vec2i const &reading)
    {
      Vec2d gauss;
      gauss.x = (double)reading.x / 512.0;
      gauss.y = (double)reading.y / 512.0;   
      return gauss;
    }
};

#endif