#include <Wire.h>
#include "CompassHDMM01.h"

// Constructor
CompassHDMM01::CompassHDMM01(int i2addr)
{
  this->i2addr = i2addr;
  
  // set offset do default value
  // 2048 is the value for zero field.
  offs.x = 2048;
  offs.y = 2048;
  
  declination = 0.0;
}

// Reset and Set coils
void CompassHDMM01::resetSetCoil()
{   
  // reset coil
  Wire.beginTransmission(i2addr);
  Wire.write(0x00);
  Wire.write(RESET_COIL);
  Wire.endTransmission();
  delay(1);
  
  // set coil 
  Wire.beginTransmission(i2addr);
  Wire.write(0x00);
  Wire.write(SET_COIL);
  Wire.endTransmission();
  delay(1);  
}

// Readout compass values
Vec2i CompassHDMM01::getReading(bool resetCoil)
{
  Vec2i reading;
  byte MsbX,LsbX,MsbY,LsbY;
  int x,y;
  
  if (resetCoil)
  {
    resetSetCoil();
  }
  
  // take measurement
  Wire.beginTransmission(i2addr);
  Wire.write(0x00); // write to address 0x00
  Wire.write(TAKE_MEASURE);
  Wire.endTransmission();
   
  delay(5);
  
  // read data
  Wire.beginTransmission(i2addr);
  Wire.write(0x01); // read from address 0x01
  Wire.requestFrom(i2addr, 4);

  while(Wire.available() < 4);
  MsbX = Wire.read();
  LsbX = Wire.read();
  MsbY = Wire.read();
  LsbY = Wire.read();
  Wire.endTransmission();
  
  reading.x=(((MsbX & 0x0f) << 8) | LsbX) - offs.x;
  reading.y=(((MsbY & 0x0f) << 8) | LsbY) - offs.y;  
  
  return reading;
}

// Calibrate between min and max values.
void CompassHDMM01::calibrateMinMax(long num, long t, bool resetCoil)
{
  Vec2<int> min, max;
  Vec2i reading;
  
  min = max = getReading(resetCoil);
  
  for (int i=0; i < num; i++)
  {
    reading = getReading(resetCoil);
    if (reading.x < min.x)
    {
       min.x = reading.x;
    }  
    else if (reading.x > max.x)
    {
      max.x = reading.x;
    }
    
    if (reading.y < min.y)
    {
       min.y = reading.y;
    }  
    else if (reading.y > max.y)
    {
      max.y = reading.y;
    }
    delay(t);
  }
  
  offs.x += (min.x + max.x) / 2;
  offs.y += (min.y + max.y) / 2;  
}

// Calibreate compass by averaging all points.
// Not good. Works only with constant rotation speed.
void CompassHDMM01::calibrateAvg(long num, long t, bool resetCoil)
{
  Vec2<long> sum;
  Vec2i reading;
  
  sum.x = 0;
  sum.y = 0;
  
  for (int i=0; i < num; i++)
  {
    reading = getReading(resetCoil);
    sum.x += reading.x;
    sum.y += reading.y;
    delay(t);
  }
  
  offs.x += (sum.x) / num;
  offs.y += (sum.y) / num;
}

// Calibrating compass with Least Sqare Method for circles.
// Yes, it is black magic.
void CompassHDMM01::calibrateLS(long num, long t, bool resetCoil)
{
  Vec2i reading;
  
  /* Matrix B
  a b c
  b e f
  c f i  
  */
  long long Ba = 0;
  long long Bb = 0;
  long long Bc = 0;
  long long Be = 0;
  long long Bf = 0;
  long long Bi = 0;
  long long y;
  
  // Vector C [x y z]
  long long Cx = 0;
  long long Cy = 0;
  long long Cz = 0;
  
  // Vector Z [x y z]
  long long Zx;
  long long Zy;
  long long Zz;
  
  // start meassurement
  for (int i=0; i < num; i++)
  {
    reading = getReading(resetCoil);
    /* DEBUG 
    Serial.print("new Vector<double>(");
    Serial.print(reading.x);
    Serial.print(",");
    Serial.print(reading.y);
    Serial.println("),");
    */
    
    Ba += reading.x * reading.x;
    Bb += reading.x * reading.y;
    Bc += reading.x;
    Be += reading.y * reading.y;
    Bf += reading.y;
    			
    y = reading.x * reading.x + reading.y * reading.y;
    Cx += reading.x * y;
    Cy += reading.y * y;
    Cz += y;
    
    delay(t);
  }
  Bi = num;
  
  // Solve B * z = c
  // Calculate det(B)
  long long det = Ba * (Be * Bi - Bf * Bf)
            -Bb * (Bb * Bi - Bf * Bc)
            +Bc * (Bb * Bf - Be * Bc);            
  /* DEBUG
  Serial.print("det:");
  Serial.println((long)(det/1000000));
  */
  
  // Calculate z = B^-1 * c
  Zx = (( Be * Bi - Bf * Bf) * Cx
        +(Bc * Bf - Bb * Bi) * Cy
        +(Bb * Bf - Bc * Be) * Cz) / det;
  Zy = (( Bf * Bc - Bb * Bi) * Cx
        +(Ba * Bi - Bc * Bc) * Cy
        +(Bc * Bb - Ba * Bf) * Cz) / det;
  Zz = (( Bb * Bf - Be * Bc) * Cx
        +(Bb * Bc - Ba * Bf) * Cy
        +(Ba * Be - Bb * Bb) * Cz) / det;
        
  offs.x += Zx / 2;
  offs.y += Zy / 2;
  //double r = sqrt(Zz + (Zx * Zx + Zy * Zy) / 4);
  /* DEBUG
  Serial.print("r:");
  Serial.println(r, 3);
  */
}