#include <AP_Math/AP_Math.h>
#include <AP_AHRS/AP_AHRS.h>

#include <AP_Compass/AP_Compass.h>
#include <AP_Declination/AP_Declination.h>
#include <DataFlash/DataFlash.h>

#include "Compass_learn.h"
#include <GCS_MAVLink/GCS.h>

#include <stdio.h>

extern const AP_HAL::HAL &hal;

// constructor
CompassLearn::CompassLearn(Compass &_compass) :
    compass(_compass)
{
    gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised");
}

/*
  update when new compass sample available
 */
void CompassLearn::update(void)
{
    const AP_AHRS &ahrs = AP::ahrs();
    if (converged || compass.get_learn_type() != Compass::LEARN_INFLIGHT ||
        !hal.util->get_soft_armed() || ahrs.get_time_flying_ms() < 3000) {
        // only learn when flying and with enough time to be clear of
        // the ground
        return;
    }

    if (!have_earth_field) {
        Location loc;
        if (!ahrs.get_position(loc)) {
            // need to wait till we have a global position
            return;
        }

        // remember primary mag
        primary_mag = compass.get_primary();

        // setup the expected earth field at this location
        float declination_deg=0, inclination_deg=0, intensity_gauss=0;
        AP_Declination::get_mag_field_ef(loc.lat*1.0e-7, loc.lng*1.0e-7, intensity_gauss, declination_deg, inclination_deg);

        // create earth field
        mag_ef = Vector3f(intensity_gauss*1000, 0.0, 0.0);
        Matrix3f R;

        R.from_euler(0.0f, -ToRad(inclination_deg), ToRad(declination_deg));
        mag_ef = R * mag_ef;

        have_earth_field = true;

        // form eliptical correction matrix and invert it. This is
        // needed to remove the effects of the eliptical correction
        // when calculating new offsets
        const Vector3f &diagonals = compass.get_diagonals(primary_mag);
        const Vector3f &offdiagonals = compass.get_offdiagonals(primary_mag);
        mat = Matrix3f(
            diagonals.x, offdiagonals.x, offdiagonals.y,
            offdiagonals.x,    diagonals.y, offdiagonals.z,
            offdiagonals.y, offdiagonals.z,    diagonals.z
            );
        if (!mat.invert()) {
            // if we can't invert, use the identity matrix
            mat.identity();
        }

        // set initial error to field intensity
        for (uint16_t i=0; i<num_sectors; i++) {
            errors[i] = intensity_gauss*1000;
        }
        
        gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: have earth field");
        hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&CompassLearn::io_timer, void));
    }

    AP_Notify::flags.compass_cal_running = true;

    if (sample_available) {
        // last sample still being processed by IO thread
        return;
    }

    Vector3f field = compass.get_field(primary_mag);
    Vector3f field_change = field - last_field;
    if (field_change.length() < min_field_change) {
        return;
    }

    {
        WITH_SEMAPHORE(sem);
        // give a sample to the backend to process
        new_sample.field = field;
        new_sample.offsets = compass.get_offsets(primary_mag);
        new_sample.attitude = Vector3f(ahrs.roll, ahrs.pitch, ahrs.yaw);
        sample_available = true;
        last_field = field;
        num_samples++;
    }

    if (sample_available) {
        DataFlash_Class::instance()->Log_Write("COFS", "TimeUS,OfsX,OfsY,OfsZ,Var,Yaw,WVar,N", "QffffffI",
                                               AP_HAL::micros64(),
                                               (double)best_offsets.x,
                                               (double)best_offsets.y,
                                               (double)best_offsets.z,
                                               (double)best_error,
                                               (double)best_yaw_deg,
                                               (double)worst_error,
                                               num_samples);
    }

    if (!converged) {
        WITH_SEMAPHORE(sem);

        // set offsets to current best guess
        compass.set_offsets(primary_mag, best_offsets);

        // set non-primary offsets to match primary
        Vector3f field_primary = compass.get_field(primary_mag);
        for (uint8_t i=0; i<compass.get_count(); i++) {
            if (i == primary_mag || !compass._state[i].use_for_yaw) {
                continue;
            }
            Vector3f field2 = compass.get_field(i);
            Vector3f new_offsets = compass.get_offsets(i) + (field_primary - field2);
            compass.set_offsets(i, new_offsets);
        }

        // stop updating the offsets once converged
        if (num_samples > 30 && best_error < 50 && worst_error > 65) {
            // set the offsets and enable compass for EKF use. Let the
            // EKF learn the remaining compass offset error
            for (uint8_t i=0; i<compass.get_count(); i++) {
                if (compass._state[i].use_for_yaw) {
                    compass.save_offsets(i);
                    compass.set_use_for_yaw(i, true);
                }
            }
            compass.set_learn_type(Compass::LEARN_NONE, true);
            // setup so use can trigger it again
            converged = false;
            sample_available = false;
            num_samples = 0;
            have_earth_field = false;
            memset(predicted_offsets, 0, sizeof(predicted_offsets));
            worst_error = 0;
            best_error = 0;
            best_yaw_deg = 0;
            best_offsets.zero();
            gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: finished");
            AP_Notify::flags.compass_cal_running = false;
            AP_Notify::events.compass_cal_saved = true;
        }
    }
}

/*
  we run the math intensive calculations in the IO thread
 */
void CompassLearn::io_timer(void)
{
    if (!sample_available) {
        return;
    }

    struct sample s;

    {
        WITH_SEMAPHORE(sem);
        s = new_sample;
        sample_available = false;
    }

    process_sample(s);
}

/*
  process a new compass sample
 */
void CompassLearn::process_sample(const struct sample &s)
{
    uint16_t besti = 0;
    float bestv = 0, worstv=0;

    /*
      we run through the 72 possible yaw error values, and for each
      one we calculate a value for the compass offsets if that yaw
      error is correct. 
     */
    for (uint16_t i=0; i<num_sectors; i++) {
        float yaw_err_deg = i*(360/num_sectors);

        // form rotation matrix for the euler attitude
        Matrix3f dcm;
        dcm.from_euler(s.attitude.x, s.attitude.y, wrap_2PI(s.attitude.z + radians(yaw_err_deg)));

        // calculate the field we would expect to get if this yaw error is correct
        Vector3f expected_field = dcm.transposed() * mag_ef;

        // calculate a value for the compass offsets for this yaw error
        Vector3f v1 = mat * s.field;
        Vector3f v2 = mat * expected_field;
        Vector3f offsets = (v2 - v1) + s.offsets;
        float delta = (offsets - predicted_offsets[i]).length();

        if (num_samples == 1) {
            predicted_offsets[i] = offsets;
        } else {
            // lowpass the predicted offsets and the error
            const float learn_rate = 0.92;
            predicted_offsets[i] = predicted_offsets[i] * learn_rate + offsets * (1-learn_rate);
            errors[i] = errors[i] * learn_rate + delta * (1-learn_rate);
        }

        // keep track of the current best prediction
        if (i == 0 || errors[i] < bestv) {
            besti = i;
            bestv = errors[i];
        }
        // also keep the worst error. This is used as part of the convergence test
        if (i == 0 || errors[i] > worstv) {
            worstv = errors[i];
        }
    }

    WITH_SEMAPHORE(sem);

    // pass the current estimate to the front-end
    best_offsets = predicted_offsets[besti];
    best_error = bestv;
    worst_error = worstv;
    best_yaw_deg = wrap_360(degrees(s.attitude.z) + besti * (360/num_sectors));
}