/*
    Run Gauss Newton fitting algorithm over the sample space and come up with offsets, diagonal/scale factors
    and crosstalk/offdiagonal parameters
*/
void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
{
    if (_sample_buffer == nullptr) {
        return;
    }
    fitness = calc_mean_squared_residuals(_param.s);
    float min_fitness = fitness;
    union param_u fit_param = _param;
    uint8_t num_iterations = 0;

    while(num_iterations < max_iterations) {
        float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS] {};
        VectorP JTFI;

        for(uint16_t k = 0; k<_samples_collected; k++) {
            Vector3f sample;
            get_sample(k, sample);

            VectorN<float,ACCEL_CAL_MAX_NUM_PARAMS> jacob;

            calc_jacob(sample, fit_param.s, jacob);

            for(uint8_t i = 0; i < get_num_params(); i++) {
                // compute JTJ
                for(uint8_t j = 0; j < get_num_params(); j++) {
                    JTJ[i*get_num_params()+j] += jacob[i] * jacob[j];
                }
                // compute JTFI
                JTFI[i] += jacob[i] * calc_residual(sample, fit_param.s);
            }
        }

        if (!inverse(JTJ, JTJ, get_num_params())) {
            return;
        }

        for(uint8_t row=0; row < get_num_params(); row++) {
            for(uint8_t col=0; col < get_num_params(); col++) {
                fit_param.a[row] -= JTFI[col] * JTJ[row*get_num_params()+col];
            }
        }

        fitness = calc_mean_squared_residuals(fit_param.s);

        if (isnan(fitness) || isinf(fitness)) {
            return;
        }

        if (fitness < min_fitness) {
            min_fitness = fitness;
            _param = fit_param;
        }

        num_iterations++;
    }
}
END_TEST

START_TEST (test_get_num_params)
{
	ck_assert(check_enter_method("program", parser_data) != NULL);

	ck_assert(check_enter_method("fun", parser_data) != NULL);

	set_method_param_count(100, parser_data);

	ck_assert_int_eq(100, get_num_params("fun", parser_data));
}
void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
{
    if(_sample_buffer == NULL) {
        return;
    }
    fitness = calc_mean_squared_residuals(_params);
    float min_fitness = fitness;

    struct param_t fit_param = _params;
    float* param_array = (float*)&fit_param;
    uint8_t num_iterations = 0;

    while(num_iterations < max_iterations) {
        float last_fitness = fitness;

        float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS];
        float JTFI[ACCEL_CAL_MAX_NUM_PARAMS];

        memset(&JTJ,0,sizeof(JTJ));
        memset(&JTFI,0,sizeof(JTFI));

        for(uint16_t k = 0; k<_samples_collected; k++) {
            Vector3f sample;
            get_sample(k, sample);

            float jacob[ACCEL_CAL_MAX_NUM_PARAMS];

            calc_jacob(sample, fit_param, jacob);

            for(uint8_t i = 0; i < get_num_params(); i++) {
                // compute JTJ
                for(uint8_t j = 0; j < get_num_params(); j++) {
                    JTJ[i*get_num_params()+j] += jacob[i] * jacob[j];
                }
                // compute JTFI
                JTFI[i] += jacob[i] * calc_residual(sample, fit_param);
            }
        }

        if(!inverse(JTJ, JTJ, get_num_params())) {
            return;
        }

        for(uint8_t row=0; row < get_num_params(); row++) {
            for(uint8_t col=0; col < get_num_params(); col++) {
                param_array[row] -= JTFI[col] * JTJ[row*get_num_params()+col];
            }
        }

        fitness = calc_mean_squared_residuals(fit_param);

        if(isnan(fitness) || isinf(fitness)) {
            return;
        }

        if(fitness < min_fitness) {
            min_fitness = fitness;
            _params = fit_param;
        }

        num_iterations++;
        if (fitness - last_fitness < 1.0e-9f) {
            break;
        }
    }
}