#ifndef __CALIBRATION_H__
#define __CALIBRATION_H__

#include "config.h"
#include "kernel.h"
#include "navigation.h"

struct Calibrator {
  
  template <std::size_t N>
  struct calibration_info_t {
    float_sylph_t bias_sf[N];
    float_sylph_t bias_base[N];
    float_sylph_t sf[N];
    float_sylph_t alignment[N][N];
  };
  calibration_info_t<3> accel, omega;
  
  template <class NumType, std::size_t N>
  static void calibrate(
      const NumType raw[N],
      const NumType &bias_mod,
      const calibration_info_t<N> &info,
      float_sylph_t (&res)[N]) {
    
    // バイアス計算(温度補正など)
    float_sylph_t bias[N];
    for(int i(0); i < N; i++){
      bias[i] = info.bias_base[i] + (info.bias_sf[i] * bias_mod); 
    }
    
    // バイアス除去ならびにスケールファクタ積算による正規化
    float_sylph_t tmp[N];
    for(int i(0); i < N; i++){
      tmp[i] = (((float_sylph_t)raw[i] - bias[i]) / info.sf[i]);
    }
    
    // ミスアライメント補正
    for(int i(0); i < N; i++){
      res[i] = 0;
      for(int j(0); j < N; j++){
        res[i] += info.alignment[i][j] * tmp[j];
      }
    }
  }
  
  Calibrator();
  ~Calibrator();
  
  void convert(
      const Kernel::msg_adc_t &msg_adc, 
      TimeUpdateInfo &tu_info);
};

extern Calibrator calibrator;

#endif /* __CALIBRATION_H__ */