#include "mag_calib.h"
#include "parameters.h"
#include <fixarray.h>

mag_calib_t mag_calib;

void mag_calib_init()
{
    mag_calib.bias_x = fix16_from_int(params.mag_bias_x);
    mag_calib.bias_y = fix16_from_int(params.mag_bias_y);
    mag_calib.bias_z = fix16_from_int(params.mag_bias_z);
    mag_calib.scale_x = fix16_from_int(params.mag_scale_x);
    mag_calib.scale_y = fix16_from_int(params.mag_scale_y);
    mag_calib.scale_z = fix16_from_int(params.mag_scale_z);
}

static fix16_t clamp(fix16_t delta)
{
    if (delta > F16(0.01)) delta = F16(0.01);
    if (delta < -F16(0.01)) delta = -F16(0.01);
    return delta;
}

void mag_calib_update(const v3d *mag)
{
    fix16_t dist = v3d_norm(mag);
    
    if (fix16_abs(dist - fix16_one) < F16(0.01))
        return; // Close enough, don't adjust
    
    // Jacobian, scaled by 256
    fix16_t jacob[6] = {
        fix16_div(fix16_sq(mag->x * 16), mag_calib.scale_x),
        fix16_div(fix16_sq(mag->y * 16), mag_calib.scale_y),
        fix16_div(fix16_sq(mag->z * 16), mag_calib.scale_z),
        fix16_div(mag->x * 256, mag_calib.scale_x),
        fix16_div(mag->y * 256, mag_calib.scale_y),
        fix16_div(mag->z * 256, mag_calib.scale_z)
    };
    
    // Pseudoinverse
    // div = (norm(jacob) / 16)^2 * dist / (dist - 1) / step size
    fix16_t div = fix16_sq(fa16_norm(jacob, 1, 6) / 16);
    div = fix16_mul(div, dist);
    div = fix16_div(div, dist - F16(1));
    div = fix16_div(div, F16(0.001));
    
    // Deltas
    mag_calib.scale_x += clamp(fix16_div(jacob[0], div));
    mag_calib.scale_y += clamp(fix16_div(jacob[1], div));
    mag_calib.scale_z += clamp(fix16_div(jacob[2], div));
    mag_calib.bias_x += clamp(fix16_div(jacob[3], div));
    mag_calib.bias_y += clamp(fix16_div(jacob[4], div));
    mag_calib.bias_z += clamp(fix16_div(jacob[5], div));
}