#include "hmc58X3.h"

const Uint8 HMC58X3::i2c_address = (0x3C >> 1);

HMC58X3::HMC58X3(I2C &i2c_if) : I2C_MAG_SENSOR(i2c_if), mode(RAW) {}

static void take_average_of_8samples(HMC58X3 &hmc58x3, Int16 res[3]){
  Int16 tmp[3];
  for(int j(0); j < 3; j++){res[j] = 0;}
  for(int i(0); i < 8; i++){
    for(volatile unsigned long k(0); k < 1500000UL; k++); // wait, about 5ms
    hmc58x3.xyz(tmp);
    for(int j(0); j < 3; j++){res[j] += tmp[j];}
  }
  for(int j(0); j < 3; j++){res[j] /= 8;}
}

void HMC58X3::init(){
  // scale 1G => 2G(5843, 780/G) or 2.5G(5883, 660/G)
  const Uint8 scale_cmd[] = {0x01, 0x60};
  i2c.write(i2c_address, 
      (const char *)scale_cmd, sizeof(scale_cmd));
  // continuous mode
  const Uint8 mode_cmd[] = {0x02, 0x00};
  i2c.write(i2c_address, 
      (const char *)mode_cmd, sizeof(mode_cmd));

  // mag sensor detection, 5843 or 5883 by using comparison between values in normal and biased conditions
  {
    const Uint8 normal_cmd[] = {0x00, 0x14}, biased_cmd[] = {0x00, 0x15};
    // positive biased, HMC5883L <= averaging disabled(MA1:MA0=00)
    // Output rates are 20Hz(5843) and 30Hz(5883).
    // when positive biased, 5843 and 5883 measure 0.55G and 1.1G, respectively.
    Int16 normal[3], biased[3];
    i2c.write(i2c_address,
        (const char *)biased_cmd, sizeof(biased_cmd));
    take_average_of_8samples(*this, biased);
    i2c.write(i2c_address,
        (const char *)normal_cmd, sizeof(normal_cmd));
    take_average_of_8samples(*this, normal);

    int diff_average(0);
    for(int i(0); i < 3; i++){
      diff_average += (biased[i] - normal[i]);
    }
    diff_average /= 3;

    // 5843: 0.55G * 780 = 429; 5883: 1.1G * 660 = 726. => threshold 577 = (429 + 726) / 2
    mode = (diff_average < 577) ? HMC5843 : HMC5883L;
  }
}

void HMC58X3::raw(Uint8 (&raw_buf)[6]){
  const Uint8 idle_cmd[] = {0x02, 0x02};
  i2c.write(i2c_address,
      (const char *)idle_cmd, sizeof(idle_cmd));
  const Uint8 move_pointer_cmd[] = {0x03};
  i2c.write(i2c_address,
      (const char *)move_pointer_cmd, sizeof(move_pointer_cmd));
  i2c.read(i2c_address,
      (char *)raw_buf, sizeof(raw_buf));
  const Uint8 cont_cmd[] = {0x02, 0x00};
  i2c.write(i2c_address,
      (const char *)cont_cmd, sizeof(cont_cmd));
  switch(mode){
    case HMC5843 : {
      // Tiny Feather(HMC5843版)は地磁気X/Y軸が基板上定義と反転しているので、(*-1)して基板座標とあわせる
      raw_buf[0] = ~raw_buf[0];
      raw_buf[1] = ~raw_buf[1];
      raw_buf[2] = ~raw_buf[2];
      raw_buf[3] = ~raw_buf[3];
      break;
    }
    case HMC5883L : {
      // Tiny Feather(HMC5883版)は地磁気軸が基板上定義と異なり しかもレジスタの読み出し順がX/Z/Yである
      Uint8 tmp[2] = {raw_buf[2], raw_buf[3]};
      raw_buf[2] = raw_buf[0];
      raw_buf[3] = raw_buf[1];
      raw_buf[0] = ~raw_buf[4];
      raw_buf[1] = ~raw_buf[5];
      raw_buf[4] = tmp[0];
      raw_buf[5] = tmp[1];
      break;
    }
  }
}

void HMC58X3::get_id(char (&buf)[3]){
  const char move_pointer_cmd[] = {0x0A};
  i2c.write(i2c_address, move_pointer_cmd, sizeof(move_pointer_cmd));
  i2c.read(i2c_address, (char *)buf, sizeof(buf));
}