#include "imu_processing.h"
#include "biquad_filter.h"
#include "math.h"

static float gyro_bias_x = 0.0f;
static float gyro_bias_y = 0.0f;
static float gyro_bias_z = 0.0f;

static float accel_bias_x = 0.0f;
static float accel_bias_y = 0.0f;
static float accel_bias_z = 0.0f;

static biquad_t accel_x_lpf;
static biquad_t accel_y_lpf;
static biquad_t accel_z_lpf;

static biquad_t gyro_x_lpf;
static biquad_t gyro_y_lpf;
static biquad_t gyro_z_lpf;

void imu_processing_init()
{
  biquad_init_lpf(&accel_x_lpf, 30.0f, 500.0f);
  biquad_init_lpf(&accel_y_lpf, 30.0f, 500.0f);
  biquad_init_lpf(&accel_z_lpf, 30.0f, 500.0f);
  
  biquad_init_lpf(&gyro_x_lpf, 120.0f, 500.0f);
  biquad_init_lpf(&gyro_y_lpf, 120.0f, 500.0f);
  biquad_init_lpf(&gyro_z_lpf, 120.0f, 500.0f);
}

void imu_read_scaled(imu_scaled_t* out)
{
  static imu_raw_t raw;
  
  icm_read_raw(&raw);
  
  out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
  out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
  out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
  
  out->ax = biquad_apply(&accel_x_lpf, out->ax);
  out->ay = biquad_apply(&accel_y_lpf, out->ay);
  out->az = biquad_apply(&accel_z_lpf, out->az);
  
  out->gx = raw.gx / GYRO_SENS_SCALE_FACTOR - gyro_bias_x;
  out->gy = raw.gy / GYRO_SENS_SCALE_FACTOR - gyro_bias_y;
  out->gz = raw.gz / GYRO_SENS_SCALE_FACTOR - gyro_bias_z;
  
  out->gx = biquad_apply(&gyro_x_lpf, out->gx);
  out->gy = biquad_apply(&gyro_y_lpf, out->gy);
  out->gz = biquad_apply(&gyro_z_lpf, out->gz);
}

void imu_calibrate()
{
  const int samples = 1000;
  
  float sum_ax = 0;
  float sum_ay = 0;
  float sum_az = 0;
  
  float sum_gx = 0;
  float sum_gy = 0;
  float sum_gz = 0;
  
  imu_raw_t imu;
  
  for (uint16_t i = 0; i < samples; ++i)
  {
    icm_read_raw(&imu);
    
    sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
    sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
    sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
    
    sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
    sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
    sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
    
    for (volatile uint16_t d = 0; d < 5000; ++d);
  }
  
  accel_bias_x = sum_ax / samples;
  accel_bias_y = sum_ay / samples;
  accel_bias_z = (sum_az / samples) + 1.0f;
  
  gyro_bias_x = sum_gx / samples;
  gyro_bias_y = sum_gy / samples;
  gyro_bias_z = sum_gz / samples;
}