/**!  @brief Class for managing storing "Adafruit" sensor calibration 
             in internal Flash memory with KVStore
             for Arduino Nano 33 BLE
 * **/

//https://os.mbed.com/docs/mbed-os/v6.12/apis/kvstore.html
//KVStore. TDBStore - Default implementation of the KVStore API. It provides static wear-leveling and quick access for when you have a small number of KV pairs.
//https://os.mbed.com/docs/mbed-os/v6.12/apis/data-architecture.html

#include "KVStore.h"
#include "kvstore_global_api.h"

  /**! XYZ vector of offsets for zero-g, in m/s^2 */
  float accel_zerog[3] = {0, 0, 0};

  /**! XYZ vector of offsets for zero-rate, in rad/s */
  float gyro_zerorate[3] = {0, 0, 0};

  /**! XYZ vector of offsets for hard iron calibration (in uT) */
  float mag_hardiron[3] = {0, 0, 0};

  /**! The 3x3 matrix for soft-iron calibration (unitless) */
  float mag_softiron[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};

  /**! The magnetic field magnitude in uTesla */
  float mag_field = 50;

const char* const ASC_KV_Key0 = "store_flag";
const char* const ASC_KV_Key1 = "store_offsets";

void setGyroCalibration(float x, float y, float z) {
    gyro_zerorate[0]=x;	  
    gyro_zerorate[1]=y;	  
    gyro_zerorate[2]=z;	  
}

float getGyroXCal() {
  return gyro_zerorate[0];
}

float getGyroYCal() {
  return gyro_zerorate[1];
}

float getGyroZCal() {
  return gyro_zerorate[2];
}

bool saveCalibration(void) {
  Serial.println("Save Cal");

  kv_reset("/kv/");

  uint8_t flag=42;
  
  kv_set(ASC_KV_Key0,&flag,sizeof(flag),0);
  
  float offsets[16];
  memcpy(offsets, accel_zerog, 12);       // 3 x 4-byte floats
  memcpy(offsets + 3, gyro_zerorate, 12); // 3 x 4-byte floats
  memcpy(offsets + 6, mag_hardiron, 12);  // 3 x 4-byte floats

  offsets[9] = mag_field;

  offsets[10] = mag_softiron[0];
  offsets[11] = mag_softiron[4];
  offsets[12] = mag_softiron[8];
  offsets[13] = mag_softiron[1];
  offsets[14] = mag_softiron[2];
  offsets[15] = mag_softiron[5];

  kv_set(ASC_KV_Key1, (uint8_t*)&offsets, sizeof(offsets), 0);
  
  return true;
}

bool loadCalibration(void) {

  uint8_t flag;
  
  kv_get(ASC_KV_Key0,&flag,sizeof(flag),0); 

  if (flag=!42) {
    Serial.print("FLAG NOT SET, CALIBRATION NOT FOUND");
    return false;
  }

  float offsets[16];
	
  kv_get(ASC_KV_Key1,(byte*)&offsets,sizeof(offsets),0);
  
  accel_zerog[0] = offsets[0];
  accel_zerog[1] = offsets[1];
  accel_zerog[2] = offsets[2];

  gyro_zerorate[0] = offsets[3];
  gyro_zerorate[1] = offsets[4];
  gyro_zerorate[2] = offsets[5];

  mag_hardiron[0] = offsets[6];
  mag_hardiron[1] = offsets[7];
  mag_hardiron[2] = offsets[8];

  mag_field = offsets[9];

  mag_softiron[0] = offsets[10];
  mag_softiron[1] = offsets[13];
  mag_softiron[2] = offsets[14];
  mag_softiron[3] = offsets[13];
  mag_softiron[4] = offsets[11];
  mag_softiron[5] = offsets[15];
  mag_softiron[6] = offsets[14];
  mag_softiron[7] = offsets[15];
  mag_softiron[8] = offsets[12];

  return true;
}

bool printCalibration(void) {
    Serial.println(F("------------"));

  Serial.print("Accelerometer: ");
  Serial.print(accel_zerog[0]);
  Serial.print(", ");
  Serial.print(accel_zerog[1]);
  Serial.print(", ");
  Serial.print(accel_zerog[2]);
  Serial.println();
	
  Serial.print("Gyroscope: ");
  Serial.print(gyro_zerorate[0]);
  Serial.print(", ");
  Serial.print(gyro_zerorate[1]);
  Serial.print(", ");
  Serial.print(gyro_zerorate[2]);
  Serial.println();
	
  Serial.print("Magnetometer Hard Iron: ");
  Serial.print(mag_hardiron[0]);
  Serial.print(", ");
  Serial.print(mag_hardiron[1]);
  Serial.print(", ");
  Serial.print(mag_hardiron[2]);
  Serial.println();
	
  Serial.print("Magnetic Field: ");
  Serial.print(mag_field);
  Serial.println();
  
  Serial.print("Magnetometer Soft Iron: ");
  Serial.print(mag_softiron[0]);
  Serial.print(", ");
  Serial.print(mag_softiron[1]);
  Serial.print(", ");
  Serial.print(mag_softiron[2]);
  Serial.println();
  Serial.print(mag_softiron[3]);
  Serial.print(", ");
  Serial.print(mag_softiron[4]);
  Serial.print(", ");
  Serial.print(mag_softiron[5]);
  Serial.println();
  Serial.print(mag_softiron[6]);
  Serial.print(", ");
  Serial.print(mag_softiron[7]);
  Serial.print(", ");
  Serial.print(mag_softiron[8]);
  Serial.println();
	
  Serial.println(F("\n------------"));
  return true;
}

bool printSavedCalibration(void) {
  Serial.println(F("------------"));

  uint8_t flag;
  
  kv_get(ASC_KV_Key0,&flag,sizeof(flag),0); 

  if (flag=!42) {
    Serial.print("FLAG NOT SET, CALIBRATION NOT FOUND");
    return false;
  }

  float offsets[16];
	
  kv_get(ASC_KV_Key1,(byte*)&offsets,sizeof(offsets),0);
  
  accel_zerog[0] = offsets[0];
  accel_zerog[1] = offsets[1];
  accel_zerog[2] = offsets[2];

  Serial.print("Accelerometer: ");
  Serial.print(accel_zerog[0]);
  Serial.print(", ");
  Serial.print(accel_zerog[1]);
  Serial.print(", ");
  Serial.print(accel_zerog[2]);
  Serial.println();
	
  gyro_zerorate[0] = offsets[3];
  gyro_zerorate[1] = offsets[4];
  gyro_zerorate[2] = offsets[5];

  Serial.print("Gyroscope: ");
  Serial.print(gyro_zerorate[0]);
  Serial.print(", ");
  Serial.print(gyro_zerorate[1]);
  Serial.print(", ");
  Serial.print(gyro_zerorate[2]);
  Serial.println();
	
  mag_hardiron[0] = offsets[6];
  mag_hardiron[1] = offsets[7];
  mag_hardiron[2] = offsets[8];

  Serial.println("Magnetometer Hard Iron: ");
  Serial.print(mag_hardiron[0]);
  Serial.print(", ");
  Serial.print(mag_hardiron[1]);
  Serial.print(", ");
  Serial.print(mag_hardiron[2]);
  Serial.println();
	
  mag_field = offsets[9];

  Serial.print("Magnetic Field: ");
  Serial.print(mag_field);
  Serial.println();
  
  mag_softiron[0] = offsets[10];
  mag_softiron[1] = offsets[13];
  mag_softiron[2] = offsets[14];
  mag_softiron[3] = offsets[13];
  mag_softiron[4] = offsets[11];
  mag_softiron[5] = offsets[15];
  mag_softiron[6] = offsets[14];
  mag_softiron[7] = offsets[15];
  mag_softiron[8] = offsets[12];

  Serial.print("Magnetometer Soft Iron: ");
  Serial.print(mag_softiron[0]);
  Serial.print(", ");
  Serial.print(mag_softiron[1]);
  Serial.print(", ");
  Serial.print(mag_softiron[2]);
  Serial.println();
  Serial.print(mag_softiron[3]);
  Serial.print(", ");
  Serial.print(mag_softiron[4]);
  Serial.print(", ");
  Serial.print(mag_softiron[5]);
  Serial.println();
  Serial.print(mag_softiron[6]);
  Serial.print(", ");
  Serial.print(mag_softiron[7]);
  Serial.print(", ");
  Serial.print(mag_softiron[8]);
  Serial.println();
	
/*  for (uint16_t a = ee_addr; a < ee_addr + KVStore_CAL_SIZE; a++) {
    uint8_t c = KVStore.read(a);
    Serial.print("0x");
    if (c < 0x10)
      Serial.print('0');
    Serial.print(c, HEX);
    Serial.print(", ");
    if ((a - ee_addr) % 16 == 15) {
      Serial.println();
    }
  }
  */

  Serial.println(F("\n------------"));
  return true;
}