/*
 * This file is part of Cleanflight and Betaflight.
 *
 * Cleanflight and Betaflight are free software. You can redistribute
 * this software and/or modify this software 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.
 *
 * Cleanflight and Betaflight are distributed in the hope that they
 * 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 software.
 *
 * If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdbool.h>
#include <stdint.h>

#include <math.h>

#include "platform.h"

#ifdef USE_MAG_QMC5883

#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"

#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_i2c_busdev.h"
#include "drivers/sensor.h"
#include "drivers/time.h"

#include "compass.h"
#include "compass_qmc5883l.h"

#define QMC5883L_MAG_I2C_ADDRESS 0x0D

// Registers
#define QMC5883L_REG_CONF1 0x09
#define QMC5883L_REG_CONF2 0x0A

// data output rates for 5883L
#define QMC5883L_ODR_10HZ  (0x00 << 2)
#define QMC5883L_ODR_50HZ  (0x01 << 2)
#define QMC5883L_ODR_100HZ (0x02 << 2)
#define QMC5883L_ODR_200HZ (0x03 << 2)

// Sensor operation modes
#define QMC5883L_MODE_STANDBY    0x00
#define QMC5883L_MODE_CONTINUOUS 0x01

#define QMC5883L_RNG_2G (0x00 << 4)
#define QMC5883L_RNG_8G (0x01 << 4)

#define QMC5883L_OSR_512 (0x00 << 6)
#define QMC5883L_OSR_256 (0x01 << 6)
#define QMC5883L_OSR_128 (0x10 << 6)
#define QMC5883L_OSR_64  (0x11 << 6)

#define QMC5883L_RST 0x80

#define QMC5883L_REG_DATA_OUTPUT_X 0x00
#define QMC5883L_REG_DATA_UNLOCK 0x05
#define QMC5883L_REG_STATUS 0x06
#define QMC5883L_REG_STATUS_DRDY 0x01
#define QMC5883L_REG_STATUS_OVL  0x02
#define QMC5883L_REG_STATUS_DOR  0x04

#define QMC5883L_REG_ID 0x0D
#define QMC5883_ID_VAL 0xFF

static bool qmc5883lInit(magDev_t *magDev)
{
    bool ack = true;
    extDevice_t *dev = &magDev->dev;

    busDeviceRegister(dev);

    ack = ack && busWriteRegister(dev, 0x0B, 0x01);
    ack = ack && busWriteRegister(dev, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G);

    if (!ack) {
        return false;
    }

    magDev->magOdrHz = 200; // QMC5883L_ODR_200HZ
    return true;
}

static bool qmc5883lRead(magDev_t *magDev, int16_t *magData)
{
    static uint8_t buf[6];
    static uint8_t status = 0; // request status on first read
    static enum {
        STATE_WAIT_DRDY,
        STATE_READ,
    } state = STATE_WAIT_DRDY;

    extDevice_t *dev = &magDev->dev;

    switch (state) {
        default:
        case STATE_WAIT_DRDY:
            if (status & QMC5883L_REG_STATUS_DRDY) {
                // New data is available
                if (busReadRegisterBufferStart(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, sizeof(buf))) {
                    state = STATE_READ;
                }
            } else if (status & QMC5883L_REG_STATUS_DOR) {
                // Data overrun (and data not ready). Data registers may be locked, read unlock regiter (ZH)
                if (busReadRegisterBufferStart(dev, QMC5883L_REG_DATA_UNLOCK, buf + sizeof(buf) - 1, 1)) {
                    status = 0;   // force status read next
                }
            } else {
                // Read status register to check for data ready - status will be untouched if read fails
                busReadRegisterBufferStart(dev, QMC5883L_REG_STATUS, &status, sizeof(status));
            }
            return false;

        case STATE_READ:
            magData[X] = (int16_t)(buf[1] << 8 | buf[0]);
            magData[Y] = (int16_t)(buf[3] << 8 | buf[2]);
            magData[Z] = (int16_t)(buf[5] << 8 | buf[4]);

            state = STATE_WAIT_DRDY;

            // Indicate that new data is required
            status = 0;

            return true;
    }

    return false;
}

bool qmc5883lDetect(magDev_t *magDev)
{

    extDevice_t *dev = &magDev->dev;

    if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
        dev->busType_u.i2c.address = QMC5883L_MAG_I2C_ADDRESS;
    }

    // Must write reset first  - don't care about the result
    busWriteRegister(dev, QMC5883L_REG_CONF2, QMC5883L_RST);
    delay(20);

    uint8_t sig = 0;
    bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_ID, &sig, 1);
    if (ack && sig == QMC5883_ID_VAL) {
        // Should be in standby mode after soft reset and sensor is really present
        // Reading ChipID of 0xFF alone is not sufficient to be sure the QMC is present
        ack = busReadRegisterBuffer(dev, QMC5883L_REG_CONF1, &sig, 1);
        if (ack && sig != QMC5883L_MODE_STANDBY) {
            return false;
        }
        magDev->init = qmc5883lInit;
        magDev->read = qmc5883lRead;
        return true;
    }

    return false;
}
#endif
