/*
   This program 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 program 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 <utility>

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

#include "AP_Compass_MAG3110.h"

extern const AP_HAL::HAL &hal;


/*
EN:
    at first glance, the magnetometer MAG3110 consists only of flaws:
    * noisy, with a bad characteristic, with very large difference on axes
    * it can't be calibrated in any way, you just have to believe what he has been measured
    * There are no adjustments and settings, it just sends data with some unknown sensitivity. Or does not sends :)
    * One and a half setup registers, in which only the frequency of operation and the number of averagings are specified
    
    This is a device, wooden to the waist. But of all these shortcomings, its sole and basic virtue arises:
    * He will never comes buggy or clevers.
    
    And since we do not need much from a magnetometer and it is required to calibrate the Ardupilot itself, the device
    appears in a completely new light - as a reliable info "north is there." What we really need.

RUS:
    на первый взгляд, магнитометр MAG3110 состоит из одних лишь недостатков:
    * шумный, с кривой характеристикой, 
    * никак не калибруется, приходится просто верить тому что он намерял
    * нет никаких регулировок и настроек, он просто выдает данные с некой неизвестной чувствительностью. Или не выдает :)
    * полтора настроечных регистра, в которых задается только частота работы и количество усреднений
    
    Такой вот девайс, по пояс деревянный. Но из всех этих недостатков проистекает его единственное и основное достоинство:
    * он никогда не глючит и не умничает.
    
    А так как нам от магнитометра особо много и не требуется, а калибровать Ардупилот и сам умеет, то девайс
    предстает в совсем новом свете - как надежный указатель "север там". Что нам собственно и надо.

*/

/*
  the vector length filter can help with noise on the bus, but may
  interfere with higher level processing. It should really be moved
  into the AP_Compass_backend code, with a parameter to enable it.
 */
#ifndef MAG3110_ENABLE_LEN_FILTER
#define MAG3110_ENABLE_LEN_FILTER 0
#endif


// Registers
#define MAG3110_MAG_REG_STATUS       0x00
#define MAG3110_MAG_REG_HXL          0x01
#define MAG3110_MAG_REG_HXH          0x02
#define MAG3110_MAG_REG_HYL          0x03
#define MAG3110_MAG_REG_HYH          0x04
#define MAG3110_MAG_REG_HZL          0x05
#define MAG3110_MAG_REG_HZH          0x06
#define MAG3110_MAG_REG_WHO_AM_I     0x07
#define MAG3110_MAG_REG_SYSMODE      0x08
#define MAG3110_MAG_REG_CTRL_REG1    0x10
#define MAG3110_MAG_REG_CTRL_REG2    0x11

#define BIT_STATUS_REG_DATA_READY    (1 << 3)




AP_Compass_MAG3110::AP_Compass_MAG3110(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev)
    : AP_Compass_Backend(compass)
    , _dev(std::move(dev))
{
}

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

    return sensor;
}


bool AP_Compass_MAG3110::init(enum Rotation rotation)
{

    bool success = _hardware_init();

    if (!success) {
        return false;
    }

    _initialised = true;

    // perform an initial read
    read();
    
    /* register the compass instance in the frontend */
    _compass_instance = register_compass();

    set_rotation(_compass_instance, rotation);

    _dev->set_device_type(DEVTYPE_MAG3110);
    set_dev_id(_compass_instance, _dev->get_bus_id());

    set_external(_compass_instance, true);

    // read at 75Hz
    _dev->register_periodic_callback(13333, FUNCTOR_BIND_MEMBER(&AP_Compass_MAG3110::_update, void)); 

    return true;
}

bool AP_Compass_MAG3110::_hardware_init()
{

    AP_HAL::Semaphore *bus_sem = _dev->get_semaphore();
    if (!bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
        AP_HAL::panic("MAG3110: Unable to get semaphore");
    }

    // initially run the bus at low speed
    _dev->set_speed(AP_HAL::Device::SPEED_LOW);

    bool ret=false;
    
    _dev->set_retries(5);
    
    uint8_t sig = 0;
    bool ack = _dev->read_registers(MAG3110_MAG_REG_WHO_AM_I, &sig, 1);    
    if (!ack || sig != 0xC4) goto exit;

    ack = _dev->write_register(MAG3110_MAG_REG_CTRL_REG1, 0x01); //  active mode 80 Hz ODR with OSR = 1
    if (!ack) goto exit;

    hal.scheduler->delay(20);
    
    ack = _dev->write_register(MAG3110_MAG_REG_CTRL_REG2, 0xA0); // AUTO_MRST_EN + RAW
    if (!ack) goto exit;

    ret = true;

    _dev->set_retries(3);
    
    printf("MAG3110 found on bus 0x%x\n", (uint16_t)_dev->get_bus_id());

exit:
    _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
    bus_sem->give();
    return ret;
}


// Read Sensor data
bool AP_Compass_MAG3110::_read_sample()
{
    {
        uint8_t status;
        bool ack = _dev->read_registers(MAG3110_MAG_REG_STATUS, &status, 1);
    
        if (!ack || (status & BIT_STATUS_REG_DATA_READY) == 0) {
            return false;
        }
    }

    uint8_t buf[6];
    bool ack = _dev->read_registers(MAG3110_MAG_REG_HXL, buf, 6);
    if (!ack) {
        return false;
    }

    _mag_x = (int16_t)(buf[0] << 8 | buf[1]);
    _mag_y = (int16_t)(buf[2] << 8 | buf[3]);
    _mag_z = (int16_t)(buf[4] << 8 | buf[5]);

    return true;
}


#define MAG_SCALE (1.0f/10000 / 0.0001f * 1000)  // 1 Tesla full scale of +-10000, 1 Gauss = 0,0001 Tesla, library needs milliGauss

void AP_Compass_MAG3110::_update()
{
    if (!_read_sample()) {
        return;
    }

    Vector3f raw_field = Vector3f((float)_mag_x, (float)_mag_y, (float)_mag_z) * MAG_SCALE;

    // rotate raw_field from sensor frame to body frame
    rotate_field(raw_field, _compass_instance);
    
    // publish raw_field (uncorrected point sample) for calibration use
    publish_raw_field(raw_field, _compass_instance);
    
    // correct raw_field for known errors
    correct_field(raw_field, _compass_instance);

    if (!field_ok(raw_field)) {
        return;
    }
    
    if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
        _mag_x_accum += raw_field.x;
        _mag_y_accum += raw_field.y;
        _mag_z_accum += raw_field.z;
        _accum_count++;
        if (_accum_count == 10) {
            _mag_x_accum /= 2;
            _mag_y_accum /= 2;
            _mag_z_accum /= 2;
            _accum_count /= 2;
        }
        _sem->give();
    }
}


// Read Sensor data
void AP_Compass_MAG3110::read()
{
    if (!_initialised) {
        return;
    }

    if (!_sem->take_nonblocking()) {
        return;
    }
    
    if (_accum_count == 0) {
        /* We're not ready to publish*/
        _sem->give();
        return;
    }

    Vector3f field = Vector3f(_mag_x_accum, _mag_y_accum, _mag_z_accum);
    field /= _accum_count;

    _accum_count = 0;
    _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;

    _sem->give();    

    publish_filtered_field(field, _compass_instance);
}