/*
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "AP_Compass_MMC5xx3.h"

#include <AP_HAL/AP_HAL.h>
#include <stdio.h>

extern const AP_HAL::HAL &hal;

#define REG_PRODUCT_ID      0x2F
#define REG_XOUT_L          0x00
#define REG_STATUS          0x08
#define REG_CONTROL0        0x09
#define REG_CONTROL1        0x0A
#define REG_CONTROL2        0x0B

// bits in REG_CONTROL0
#define REG_CONTROL0_RESET  0x10 // Set coil for measuring offset
#define REG_CONTROL0_SET    0x08 // Reset coil for measuring offset
#define REG_CONTROL0_TMM    0x01 // Take Measurement for Magnetic field
#define REG_CONTROL0_TMT    0x02 // Take Measurement for Temperature

// bits in REG_CONTROL1
#define REG_CONTROL1_SW_RST 0x80 // Software reset
#define REG_CONTROL1_BW0    0x01
#define REG_CONTROL1_BW1    0x02

#define MMC5983_ID 0x30

AP_Compass_Backend *AP_Compass_MMC5XX3::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
                                              bool force_external,
                                              enum Rotation rotation)
{
    if (!dev) {
        return nullptr;
    }
    AP_Compass_MMC5XX3 *sensor = new AP_Compass_MMC5XX3(std::move(dev), force_external, rotation);
    if (!sensor || !sensor->init()) {
        delete sensor;
        return nullptr;
    }

    return sensor;
}

AP_Compass_MMC5XX3::AP_Compass_MMC5XX3(AP_HAL::OwnPtr<AP_HAL::Device> _dev,
                                       bool _force_external,
                                       enum Rotation _rotation)
    : dev(std::move(_dev))
    , force_external(_force_external)
    , rotation(_rotation)
    , have_initial_offset(false)
{
}

bool AP_Compass_MMC5XX3::init()
{
    // take i2c bus sempahore
    WITH_SEMAPHORE(dev->get_semaphore());

    dev->set_retries(10);

    // setup to allow reads on SPI
    if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
        dev->set_read_flag(0x80);
    }

    uint8_t whoami;
    if (!dev->read_registers(REG_PRODUCT_ID, &whoami, 1) ||
        whoami != MMC5983_ID) {
        // not a MMC5983
        return false;
    }

    // reset sensor
    dev->write_register(REG_CONTROL1, REG_CONTROL1_SW_RST);

    // 10ms minimum startup time
    hal.scheduler->delay(15);

    if (!dev->write_register(REG_CONTROL1, REG_CONTROL1_BW0 | REG_CONTROL1_BW1)) {
        return false;
    } //  // This BW config sets the sensor measurement time to 0.5ms and filter bandwidth to 800Hz


    /* register the compass instance in the frontend */
    dev->set_device_type(DEVTYPE_MMC5983);
    if (!register_compass(dev->get_bus_id(), compass_instance)) {
        return false;
    }

    set_dev_id(compass_instance, dev->get_bus_id());

    printf("Found a MMC5983 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance);

    set_rotation(compass_instance, rotation);

    if (force_external) {
        set_external(compass_instance, true);
    }

    dev->set_retries(1);

    // call timer() at 1kHz
    dev->register_periodic_callback(1000,
                                    FUNCTOR_BIND_MEMBER(&AP_Compass_MMC5XX3::timer, void));

    return true;
}

void AP_Compass_MMC5XX3::timer()
{
    // recalculate the offset with set/reset operation every measure_count_limit measurements
    // sensor is read at about 500Hz, so about every 10 seconds
    const uint16_t measure_count_limit = 5000;
    const uint16_t zero_offset = 32768; // 16 bit mode
    const uint16_t sensitivity = 4096; // counts per Gauss, 16 bit mode
    constexpr float counts_to_milliGauss = 1.0e3f / sensitivity;

    /*
      we use the SET/RESET method to remove bridge offset every
      measure_count_limit measurements. This involves a fairly complex
      state machine, but means we are much less sensitive to
      temperature changes
     */
    switch (state) {

    // perform a set operation
    case MMCState::STATE_SET: {
        if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_SET)) {
            break;
        }
        // minimum time to wait after set/reset before take measurement request is 1ms
        state = MMCState::STATE_SET_MEASURE;
        break;
    }

    // request a measurement for field and offset calculation after set operation
    case MMCState::STATE_SET_MEASURE: {
        if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) {
            break;
        }
        state = MMCState::STATE_SET_WAIT;
        break;
    }

    // wait for measurement to be ready after set operation, then read the
    // measurement data and request a reset operation
    case MMCState::STATE_SET_WAIT: {
        uint8_t status;
        if (!dev->read_registers(REG_STATUS, &status, 1)) {
            state = MMCState::STATE_SET;
            break;
        }

        // check if measurement is ready
        if (!(status & 1)) {
            break;
        }

        // read measurement
        if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data0[0], 6)) {
            state = MMCState::STATE_SET;
            break;
        }

        // request set operation
        if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_RESET)) {
            break;
        }
        // minimum time to wait after set/reset before take measurement request is 1ms
        state = MMCState::STATE_RESET_MEASURE;
        break;
    }

    // request a measurement for field and offset calculation after reset operation
    case MMCState::STATE_RESET_MEASURE: {
        // take measurement request
        if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) {
            state = MMCState::STATE_SET;
            break;
        }

        state = MMCState::STATE_RESET_WAIT;
        break;
    }

    // wait for measurement to be ready after reset operation,
    // then read the measurement data, calculate the field and offset,
    // and begin requesting field measurements
    case MMCState::STATE_RESET_WAIT: {
        uint8_t status;
        if (!dev->read_registers(REG_STATUS, &status, 1)) {
            state = MMCState::STATE_SET;
            break;
        }
        // check if measurement is ready
        if (!(status & 1)) {
            break;
        }

        uint8_t data1[6];
        if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
            state = MMCState::STATE_SET;
            break;
        }

        /*
          calculate field and offset
         */
        Vector3f f1 {float((data0[0] << 8) + data0[1]) - zero_offset,
                    float((data0[2] << 8) + data0[3]) - zero_offset,
                    float((data0[4] << 8) + data0[5]) - zero_offset};
        Vector3f f2 {float((data1[0] << 8) + data1[1]) - zero_offset,
                    float((data1[2] << 8) + data1[3]) - zero_offset,
                    float((data1[4] << 8) + data1[5]) - zero_offset};

        Vector3f field {(f2 - f1) * counts_to_milliGauss * 0.5f};
        Vector3f new_offset {(f1 + f2) * counts_to_milliGauss * 0.5f};

        if (!have_initial_offset) {
            offset = new_offset;
            have_initial_offset = true;
        } else {
            // low pass changes to the offset
            offset = offset * 0.5f + new_offset * 0.5f;
        }

        accumulate_sample(field, compass_instance);

        if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) {
            printf("failed to initiate measurement\n");
            state = MMCState::STATE_SET;
        } else {
            state = MMCState::STATE_MEASURE;
        }

        break;
    }

    // take repeated field measurements, set/reset is performed again after
    // measure_count_limit measurements
    case MMCState::STATE_MEASURE: {
        uint8_t status;
        if (!dev->read_registers(REG_STATUS, &status, 1)) {
            state = MMCState::STATE_SET;
            break;
        }

        // check if measurement is ready
        if (!(status & 1)) {
            break;
        }

        uint8_t data1[6];
        if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
            printf("cant read data\n");
            state = MMCState::STATE_SET;
            break;
        }

        Vector3f field {float((data1[0] << 8) + data1[1]) - zero_offset,
                       float((data1[2] << 8) + data1[3]) - zero_offset,
                       float((data1[4] << 8) + data1[5]) - zero_offset};
        field *= counts_to_milliGauss;
        field -= offset;
        accumulate_sample(field, compass_instance);

        // we stay in STATE_MEASURE for measure_count_limit cycles
        if (measure_count++ >= measure_count_limit) {
            measure_count = 0;
            state = MMCState::STATE_SET;
        } else {
            if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) { // Take Measurement
                state = MMCState::STATE_SET;
            }
        }
        break;
    }
    }
}

void AP_Compass_MMC5XX3::read()
{
    drain_accumulated_samples(compass_instance);
}