/* Copyright (c) 2021 Skyward Experimental Rocketry
 * Author: Alberto Nidasio
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "MPU9250.h"

#include <drivers/timer/TimestampTimer.h>
#include <interfaces/endianness.h>
#include <utils/Constants.h>

namespace Boardcore
{

MPU9250::MPU9250(SPIBusInterface& bus, miosix::GpioPin cs, SPIBusConfig config,
                 unsigned short samplingRate, GyroFSR gyroFsr,
                 AccelFSR accelFsr)
    : spiSlave(bus, cs, config), samplingRate(samplingRate), gyroFsr(gyroFsr),
      accelFsr(accelFsr)
{
}

SPIBusConfig MPU9250::getDefaultSPIConfig()
{
    SPIBusConfig spiConfig{};
    spiConfig.clockDivider = SPI::ClockDivider::DIV_64;
    spiConfig.mode         = SPI::Mode::MODE_3;
    return spiConfig;
}

bool MPU9250::init()
{
    // Check if already initialized
    if (initialized)
    {
        LOG_ERR(logger, "Already initialized");

        lastError = SensorErrors::ALREADY_INIT;

        return false;
    }

    // Reset the device
    resetDevice();
    miosix::Thread::sleep(100);

    // Wake up the chip
    selectAutoClock();

    // Check WHO AM I
    if (!checkWhoAmI())
    {
        LOG_ERR(logger, "Invalid WHO AM I");

        lastError = SensorErrors::INVALID_WHOAMI;

        return false;
    }

    // Setup I2C master interface to communicate with the AK8963
    enableMpuI2CMasterInterface();
    setMpuI2CMasterInterfaceClock(I2C_MST_IF_CLK_400);

    // Init the AK8963
    if (!initAk())
    {
        return false;
    }

    // Set full scale resolution for gyroscope and accelerometer (they are
    // enabled by default)
    setGyroFsr(gyroFsr);
    setAccelFsr(accelFsr);

    // Set the sample rate
    setSampleRate(samplingRate);

    LOG_DEBUG(logger, "Magnetometer sensitivity adjustment: {}, {}, {}",
              magSensAdjCoeff[0], magSensAdjCoeff[1], magSensAdjCoeff[2]);

    initialized = true;
    return true;
}

MPU9250Data MPU9250::sampleImpl()
{
    MPU9250RawData rawData;
    MPU9250Data data;

    {
        SPITransaction transaction(spiSlave);
        transaction.readRegisters(REG_ACCEL_XOUT_H, (uint8_t*)rawData.bytes,
                                  sizeof(MPU9250RawData));
    }

    // Save timestamps
    uint64_t timestamp          = TimestampTimer::getTimestamp();
    data.accelerationTimestamp  = timestamp;
    data.temperatureTimestamp   = timestamp;
    data.angularSpeedTimestamp  = timestamp;
    data.magneticFieldTimestamp = timestamp;

    // Save data
    data.accelerationX =
        normalizeAcceleration(swapBytes16(rawData.bits.accelX));
    data.accelerationY =
        normalizeAcceleration(swapBytes16(rawData.bits.accelY));
    data.accelerationZ =
        normalizeAcceleration(swapBytes16(rawData.bits.accelZ));
    data.temperature   = normalizeTemperature(swapBytes16(rawData.bits.temp));
    data.angularSpeedX = normalizeGyroscope(swapBytes16(rawData.bits.gyroX));
    data.angularSpeedY = normalizeGyroscope(swapBytes16(rawData.bits.gyroY));
    data.angularSpeedZ = normalizeGyroscope(swapBytes16(rawData.bits.gyroZ));
    data.magneticFieldX =
        normalizeMagnetometer(rawData.bits.magX, magSensAdjCoeff[0]);
    data.magneticFieldY =
        normalizeMagnetometer(rawData.bits.magY, magSensAdjCoeff[1]);
    data.magneticFieldZ =
        normalizeMagnetometer(rawData.bits.magZ, magSensAdjCoeff[2]);

    return data;
}

void MPU9250::resetDevice()
{
    SPITransaction transaction(spiSlave);

    transaction.writeRegister(REG_PWR_MGMT_1, REG_PWR_MGMT_1_BIT_H_RESET);
}

void MPU9250::selectAutoClock()
{
    SPITransaction transaction(spiSlave);

    // Wake up by clearing sleep bit and set clock auto select
    writeSPIWithDelay(transaction, REG_PWR_MGMT_1, REG_PWR_MGMT_1_CLKSEL_AUTO);
}

void MPU9250::setGyroFsr(GyroFSR fs)
{
    SPITransaction transaction(spiSlave);

    writeSPIWithDelay(transaction, REG_GYRO_CONFIG, fs);
}

void MPU9250::setAccelFsr(AccelFSR fs)
{
    SPITransaction transaction(spiSlave);

    writeSPIWithDelay(transaction, REG_ACCEL_CONFIG, fs);
}

void MPU9250::setSampleRate(unsigned short rate)
{
    if (rate < 4)
        rate = 4;
    else if (rate > 1000)
        rate = 1000;

    uint8_t data = 1000 / rate - 1;

    SPITransaction transaction(spiSlave);

    writeSPIWithDelay(transaction, REG_SMPLRT_DIV, data);

    // Set the digital low pass filter in order to use the sample rate divider
    writeSPIWithDelay(transaction, REG_CONFIG, REG_CONFIG_DLPF_CFG_1);

    // We do not need to write Fchoise_b bits becouse they reset to 0 and they
    // are the inverse of Fchoise, see page 13 of MPU9250 register map
}

void MPU9250::enableMpuI2CMasterInterface()
{
    SPITransaction transaction(spiSlave);

    writeSPIWithDelay(transaction, REG_USER_CTRL, REG_USER_CTRL_I2C_MST_EN);
}

void MPU9250::setMpuI2CMasterInterfaceClock(I2CMasterInterfaceClock clk)
{
    SPITransaction transaction(spiSlave);

    writeSPIWithDelay(transaction, REG_I2C_MST_CTRL, clk);
}

void MPU9250::setI2CMasterSlaveRead(uint8_t addr, uint8_t reg, uint8_t nBytes,
                                    uint8_t slave)
{
    uint8_t regSlvAddr, regSlvReg, regSlvCtrl;

    switch (slave)
    {
        case 0:
            regSlvAddr = REG_I2C_SLV0_ADDR;
            regSlvReg  = REG_I2C_SLV0_REG;
            regSlvCtrl = REG_I2C_SLV0_CTRL;
            break;
        case 1:
            regSlvAddr = REG_I2C_SLV1_ADDR;
            regSlvReg  = REG_I2C_SLV1_REG;
            regSlvCtrl = REG_I2C_SLV1_CTRL;
            break;
        case 2:
            regSlvAddr = REG_I2C_SLV2_ADDR;
            regSlvReg  = REG_I2C_SLV2_REG;
            regSlvCtrl = REG_I2C_SLV2_CTRL;
            break;
        case 3:
            regSlvAddr = REG_I2C_SLV3_ADDR;
            regSlvReg  = REG_I2C_SLV3_REG;
            regSlvCtrl = REG_I2C_SLV3_CTRL;
            break;

        default:
            LOG_ERR(logger,
                    "invalid slave parameter in function "
                    "setI2CMasterSlaveRead");
            return;
    }

    SPITransaction transaction(spiSlave);

    writeSPIWithDelay(transaction, regSlvAddr, addr | 0x80);  // Set read bit
    writeSPIWithDelay(transaction, regSlvReg, reg);

    // Set bytes number and enable the data transfer on each sample
    writeSPIWithDelay(transaction, regSlvCtrl, REG_I2C_SLV_CTRL_EN | nBytes);
}

void MPU9250::setI2CMasterSlaveWrite(uint8_t addr, uint8_t reg, uint8_t data,
                                     uint8_t slave)
{
    uint8_t regSlvAddr, regSlvReg, regSlvCtrl, regSlvDo;

    switch (slave)
    {
        case 0:
            regSlvAddr = REG_I2C_SLV0_ADDR;
            regSlvReg  = REG_I2C_SLV0_REG;
            regSlvCtrl = REG_I2C_SLV0_CTRL;
            regSlvDo   = REG_I2C_SLV0_DO;
            break;
        case 1:
            regSlvAddr = REG_I2C_SLV1_ADDR;
            regSlvReg  = REG_I2C_SLV1_REG;
            regSlvCtrl = REG_I2C_SLV1_CTRL;
            regSlvDo   = REG_I2C_SLV1_DO;
            break;
        case 2:
            regSlvAddr = REG_I2C_SLV2_ADDR;
            regSlvReg  = REG_I2C_SLV2_REG;
            regSlvCtrl = REG_I2C_SLV2_CTRL;
            regSlvDo   = REG_I2C_SLV2_DO;
            break;
        case 3:
            regSlvAddr = REG_I2C_SLV3_ADDR;
            regSlvReg  = REG_I2C_SLV3_REG;
            regSlvCtrl = REG_I2C_SLV3_CTRL;
            regSlvDo   = REG_I2C_SLV3_DO;
            break;

        default:
            LOG_ERR(logger,
                    "invalid slave parameter in function "
                    "setI2CMasterSlaveRead");
            return;
    }

    SPITransaction transaction(spiSlave);

    writeSPIWithDelay(transaction, regSlvAddr, addr);
    writeSPIWithDelay(transaction, regSlvReg, reg);
    writeSPIWithDelay(transaction, regSlvDo, data);

    // Enable the data transfer on each sample
    writeSPIWithDelay(transaction, regSlvCtrl, REG_I2C_SLV_CTRL_EN | 0x1);
}

void MPU9250::disableI2CMasterSlave(uint8_t slave)
{
    uint8_t regSlvCtrl;

    switch (slave)
    {
        case 0:
            regSlvCtrl = REG_I2C_SLV0_CTRL;
            break;
        case 1:
            regSlvCtrl = REG_I2C_SLV1_CTRL;
            break;
        case 2:
            regSlvCtrl = REG_I2C_SLV2_CTRL;
            break;
        case 3:
            regSlvCtrl = REG_I2C_SLV3_CTRL;
            break;
            // In the control register of slave 4 there is also the
            // configuration for the slaves sampling rate. If slave 4 is needed
            // the salmpling rate divider should not be changed case 4:
            //     regSlvCtrl = REG_I2C_SLV4_CTRL;
            //     break;

        default:
            LOG_ERR(logger,
                    "invalid slave parameter in function "
                    "setI2CMasterSlaveRead");
            return;
    }

    SPITransaction transaction(spiSlave);

    // Enable the data transfer on each sample
    writeSPIWithDelay(transaction, regSlvCtrl, 0);
}

uint8_t MPU9250::readFromAk(uint8_t reg)
{
    // Set the device address and register
    setI2CMasterSlaveRead(AK8963_ADDR, reg);

    // Wait for the sample
    miosix::Thread::sleep(1);

    SPITransaction transaction(spiSlave);

    return transaction.readRegister(REG_EXT_SENS_DATA_00);
}

void MPU9250::writeToAk(uint8_t reg, uint8_t data)
{
    // Set the device address, register and data byte
    setI2CMasterSlaveWrite(AK8963_ADDR, reg, data);
    miosix::Thread::sleep(1);
}

bool MPU9250::initAk()
{
    // Set the sample rate to 1KHz in order to communicate faster, but safely,
    // with the AK (if we use 8KHz the communication does not work properly)
    setSampleRate(1000);

    // Power down the magnetometer
    writeToAk(AK8963_REG_CNTL1, AK8963_REG_CNTL1_POWER_DOWN_MODE);

    // Wait a bit more, the first slave communication seams to be delayed by 1
    // sample
    miosix::Thread::sleep(1);

    // Reset
    writeToAk(AK8963_REG_CNTL2, AK8963_REG_CNTL2_BIT_SRST);

    // Check AK8963 WHO AM I
    if (!checkAkWhoAmI())
    {
        LOG_ERR(logger, "Invalid AK8963 WHO AM I");

        lastError = SensorErrors::INVALID_WHOAMI;

        return false;
    }

    // Enter rom access mode
    writeToAk(AK8963_REG_CNTL1, AK8963_REG_CNTL1_POWER_DOWN_MODE);
    writeToAk(AK8963_REG_CNTL1, AK8963_REG_CNTL1_FUSE_ROM_ACCESS_MODE);

    // Read magnetometer sensitivity adjustment data (page 53 of register map)
    magSensAdjCoeff[0] =
        (((readFromAk(AK8963_REG_ASAX) - 128) + .5) / 128 + 1) * 4912.0f /
        32760.0f;
    magSensAdjCoeff[1] =
        (((readFromAk(AK8963_REG_ASAY) - 128) + .5) / 128 + 1) * 4912.0f /
        32760.0f;
    magSensAdjCoeff[2] =
        (((readFromAk(AK8963_REG_ASAZ) - 128) + .5) / 128 + 1) * 4912.0f /
        32760.0f;

    // Set continuos measurement mode
    writeToAk(AK8963_REG_CNTL1, AK8963_REG_CNTL1_POWER_DOWN_MODE);
    writeToAk(AK8963_REG_CNTL1, AK8963_REG_CNTL1_CONT_MES_MODE_2);

    // Set I2C master slave communication on each sample
    setI2CMasterSlaveRead(AK8963_ADDR, AK8963_REG_ST1, 8);

    return true;
}

bool MPU9250::checkWhoAmI()
{
    SPITransaction transaction(spiSlave);

    uint8_t whoAmIValue = transaction.readRegister(REG_WHO_AM_I);

    return whoAmIValue == REG_WHO_AM_I_VAL;
}

bool MPU9250::checkAkWhoAmI()
{
    uint8_t whoAmIValue = readFromAk(AK8963_REG_WHO_AM_I);

    return whoAmIValue == AK8963_REG_WHO_AM_I_VAL;
}

void MPU9250::writeSPIWithDelay(SPITransaction& transaction, uint8_t reg,
                                uint8_t data)
{
    transaction.writeRegister(reg, data);
    miosix::delayUs(1);
    /*transaction.readRegister(reg);
    miosix::delayUs(1);*/
}

float MPU9250::normalizeAcceleration(int16_t rawValue)
{
    return static_cast<float>(rawValue) / 32768.0f *
           ACCELERATION_FS_MAP[accelFsr >> 3] * Constants::g;
}

// Page 33 of register map document
float MPU9250::normalizeTemperature(int16_t rawValue)
{
    return static_cast<float>(rawValue) / 512.0f + 21.0f;
}

float MPU9250::normalizeGyroscope(int16_t rawValue)
{
    return static_cast<float>(rawValue) / 32768.0f *
           GYROSCOPE_FS_MAP[gyroFsr >> 3] * Constants::DEGREES_TO_RADIANS;
}

float MPU9250::normalizeMagnetometer(int16_t rawValue, float adjustmentCoeff)
{
    // Page 50 and 53 of register map document
    return static_cast<float>(rawValue) * adjustmentCoeff;
}

}  // namespace Boardcore
