#include "calibration.h"

#include <cmath>

Calibrator::Calibrator() : accel(), omega() {
  // general configuration
  accel.bias_sf[0] = accel.bias_sf[1] = accel.bias_sf[2] = 0;
  accel.bias_base[0] = (6707494.4345 + 6785486.0589 + 6706581.1063 + 6743161.0299) / 4;
  accel.bias_base[1] = (6725233.6115 + 6750500.5362 + 6762172.8771 + 6761972.9751) / 4;
  accel.bias_base[2] = (6766908.2567 + 6772220.6185 + 6872485.4751 + 6758977.2691) / 4;
  accel.sf[0] = (8557518.4364 - 4877722.7209) / 2 / 9.80665;
  accel.sf[1] = (8603621.1179 - 4904645.7508) / 2 / 9.80665;
  accel.sf[2] = (8568510.0249 - 4984761.4785) / 2 / 9.80665;
  for(int i(0); i < 3; i++){
    for(int j(0); j < 3; j++){
      accel.alignment[i][j] = (i == j) ? 1 : 0;
    }
  }
  omega.bias_sf[0] = omega.bias_sf[1] = omega.bias_sf[2] = 0;
  omega.bias_base[0] = 8392638.88;
  omega.bias_base[1] = 8387794.396;
  omega.bias_base[2] = 8410700.93;
  omega.sf[0] = omega.sf[1] = omega.sf[2] = (180.0 / std::acos(-1.0)) * 33000 / 5;
  for(int i(0); i < 3; i++){
    for(int j(0); j < 3; j++){
      omega.alignment[i][j] = (i == j) ? -1 : 0;
    }
  }
}

Calibrator::~Calibrator(){
  
}

void Calibrator::convert(
    const Kernel::msg_adc_t &msg_adc, 
    TimeUpdateInfo &tu_info){
  tu_info.itow = (float_sylph_t)1E-3 * msg_adc.time_ms;
  // 加速度(m/s^2)
  calibrate(
      &msg_adc.ch[0], msg_adc.ch[7],
      accel, tu_info.accel);
  // 角速度(rad/sec)
  calibrate(
      &msg_adc.ch[3], msg_adc.ch[7],
      omega, tu_info.omega);
}

Calibrator calibrator;