#include <stdint.h>
#include <stdio.h>
#include "MMC56x3.h"
#include "MMC56x3_hal.h"
#include "esp_err.h"
#include "esp_log.h"

#define PRODUCT_ID_1 0x39
#define TEMPERATURE 0x09
#define IC0 0x1B
#define STATUS1 0x18
#define XOUT0 0x00
#define XOUT2 0x06

#define TAKE_MEAS_T 1
#define MEAS_T_DONE 7
#define TAKE_MEAS_M 0
#define MEAS_M_DONE 6
#define DO_SET 3
#define DO_RESET 4

esp_err_t MMC56x3_get_product_ID(uint8_t* id){
  ESP_ERROR_CHECK(i2c_read_reg(PRODUCT_ID_1, id, 1));
  return ESP_OK;
}

esp_err_t MMC56x3_get_temperature(uint8_t* temp){
  uint8_t startMeas = 1 << TAKE_MEAS_T;
  ESP_ERROR_CHECK(i2c_write_reg(IC0, &startMeas, 1));
  for(int i=0; i<5; i++){ // wait for meas_t_done
    uint8_t status1_content;
    ESP_ERROR_CHECK(i2c_read_reg(STATUS1, &status1_content, 1));
    ESP_LOGI("MMC56x3", "status 1 %d", status1_content);
    if(status1_content & (1 << MEAS_T_DONE)) break;
    i2c_hal_wait_ms(1);
  }
  ESP_ERROR_CHECK(i2c_read_reg(TEMPERATURE, temp, 1));
  return ESP_OK;
}

esp_err_t MMC56x3_get_mag_field(int32_t* x, int32_t* y, int32_t* z){
  uint8_t startMeas = 1 << TAKE_MEAS_M;
  ESP_ERROR_CHECK(i2c_write_reg(IC0, &startMeas, 1));
  for(int i=0; i<5; i++){ // wait for meas_m_done
    i2c_hal_wait_ms(10);
    uint8_t status1_content;
    ESP_ERROR_CHECK(i2c_read_reg(STATUS1, &status1_content, 1));
    if(status1_content & (1 << MEAS_M_DONE)) break;
  }
  uint8_t buff[3*3];
  ESP_ERROR_CHECK(i2c_read_reg(XOUT0, buff, sizeof(buff)));
  *x=(buff[0] << 12) | (buff[1] << 4) | (buff[6] >> 4);
  *y=(buff[2] << 12) | (buff[3] << 4) | (buff[7] >> 4);
  *z=(buff[4] << 12) | (buff[5] << 4) | (buff[8] >> 4);
  *x-=1 << 19;
  *y-=1 << 19;
  *z-=1 << 19;
  return ESP_OK;
}

esp_err_t MMC56x3_set(){
  uint8_t set = 1 << DO_SET;
  ESP_ERROR_CHECK(i2c_write_reg(IC0, &set, 1));
  return ESP_OK;
}
esp_err_t MMC56x3_reset(){
  uint8_t set = 1 << DO_RESET;
  ESP_ERROR_CHECK(i2c_write_reg(IC0, &set, 1));
  return ESP_OK;
}