//
// Created by cfif on 16.09.22.
//
#include "Accel_QMA6100P.h"
#include <math.h>
#include "SystemDelayInterface.h"
//#include "AsciiStringAssmeblingUtils.h"

// Return If Not Ok
#define QMA6100P_RINO(FUNC){bool fres = FUNC; if(!fres){return false;}}


static bool vAccelWriteReg(tAccelQma6100P *env, uint8_t reg, uint8_t value, uint32_t timeout) {
    uint16_t back;

    uint16_t adr_reg = reg;
    uint16_t value_reg = value;

    QMA6100P_RINO(SpiPortChipSelect(env->port, timeout))

    QMA6100P_RINO(SpiPortTransmit(env->port, &adr_reg, timeout))
    QMA6100P_RINO(SpiPortTransmit(env->port, &value_reg, timeout))

    QMA6100P_RINO(SpiPortChipRelease(env->port, timeout))

    return true;
}


static bool vAccelReadReg(tAccelQma6100P *env, uint8_t reg, uint8_t *data, uint32_t timeout) {

    uint16_t back;
    uint16_t adr_reg = reg | 0x80;

    QMA6100P_RINO(SpiPortChipSelect(env->port, timeout))

    QMA6100P_RINO(SpiPortTransmit(env->port, &adr_reg, timeout))
    QMA6100P_RINO(SpiPortReceive(env->port, &back, timeout))

    QMA6100P_RINO(SpiPortChipRelease(env->port, timeout))

    *data = back;

    return true;
}

void AccelQma6100P_Init(tAccelQma6100P *env, tSpiPortIO *accelIO) {
    env->port = accelIO;
}

typedef enum {
    QMA_6100P_MODE_STANDBY = 0,
    QMA_6100P_MODE_WAKE = 1,
} eAccelQma6100P_Mode;

//void AccelQma6100P_SetMode(tAccelQma6100P *env, eAccelQma6100P_Mode mode) {
//    uint8_t reg;
//    uint16_t err;
//
//    reg = vAccelReadReg(env, 0x11, &err, 1);
//
//    if (mode == QMA_6100P_MODE_WAKE)
//        reg = (reg & 0x7f) | 0x80;
//    else if (mode == QMA_6100P_MODE_STANDBY)
//        reg = (reg & 0x7f);
//    else {
//        return;
//    }
//
//    vAccelWriteReg(env, 0x11, reg, QMA6100P_TIMEOUT);
//}

bool AccelQma6100P_ConfigureDefaults(tAccelQma6100P *env) {

//    // Сброс
//    vAccelWriteReg(env,  QMA6100P_REG_RESET, QMA6100P_RESET, QMA6100P_TIMEOUT);
//    SystemDelayMs(10);
//    vAccelWriteReg(env,  QMA6100P_REG_RESET, 0, QMA6100P_TIMEOUT);
//    SystemDelayMs(10);
//
//    // Перевод микросхемы в активное состояние
//    vAccelWriteReg(env, 
//                   QMA6100P_REG_PM,
//                   QMA6100P_ENABLE_ACTIVE_MODE | QMA6100P_CIC_MULT8, QMA6100P_TIMEOUT
//    );
//
//    // Сброс значения регистров по умолчанию
//    SystemDelayMs(10);
//    vAccelWriteReg(env,  QMA6100P_REG_INTERNAL, QMA6100P_DEFAULT, QMA6100P_TIMEOUT);
//    SystemDelayMs(10);
//
//    // Перевод микросхемы в активное состояние
//    vAccelWriteReg(env,  QMA6100P_REG_PM, QMA6100P_ENABLE_ACTIVE_MODE, QMA6100P_TIMEOUT);
//
//    // Установка полной шкалы измерения
//    vAccelWriteReg(env,  QMA6100P_REG_FSR, QMA6100P_FULL_SCALE_RAGE, QMA6100P_TIMEOUT);
//
//    // Включение прерывания по готовности данных
//    vAccelWriteReg(env,  QMA6100P_REG_INT_EN1, QMA6100P_DATA_READY_ENABLE_INT1, QMA6100P_TIMEOUT);
//    vAccelWriteReg(env,  QMA6100P_REG_INT_MAP1, QMA6100P_ENABLE_INT1, QMA6100P_TIMEOUT);
//
//
//    vAccelWriteReg(env,  QMA6100P_REG_INT_CFG, QMA6100P_DEFAULT_INT_CFG_VALUE, QMA6100P_TIMEOUT);
//    vAccelWriteReg(env,  QMA6100P_REG_MOT_CONF3, QMA6100P_DEFAULT_MOT_CONF3_VALUE, QMA6100P_TIMEOUT);



    vAccelWriteReg(env, QMA6100P_REG_RESET, QMA6100P_RESET, QMA6100P_TIMEOUT);
    SystemDelayMs(50);
    vAccelWriteReg(env, QMA6100P_REG_RESET, 0, QMA6100P_TIMEOUT);
    SystemDelayMs(55);
//
//

    vAccelWriteReg(env, 0x4A, 0x20, QMA6100P_TIMEOUT);//2w spi
    vAccelWriteReg(env, 0x56, 0x01, QMA6100P_TIMEOUT);//2w spi
    vAccelWriteReg(env, 0x5F, 0x80, QMA6100P_TIMEOUT);//2w spi
    SystemDelayMs(1);
    vAccelWriteReg(env, 0x5F, 0x0, QMA6100P_TIMEOUT);//2w spi

//    vAccelWriteReg(env, 0x11, 0x4, QMA6100P_TIMEOUT);//2w spi
//
//
//    uint8_t id;
//    vAccelReadReg(env, 0x00, &id, QMA6100P_TIMEOUT);
//
//    bool is_qma = (id & 0xF0) == 90;


    vAccelWriteReg(env, 0x20, 0x05, QMA6100P_TIMEOUT);//2w spi
    vAccelWriteReg(env, 0x11, 0x84, QMA6100P_TIMEOUT);//mode active, mclk 51khz

    vAccelWriteReg(env, 0x0F, 0x01, QMA6100P_TIMEOUT);//hpf,2g (244ug/LSB)
    AccelQma6100P_SetRange(env, ACCEL_QMA6100P_RANGE_16G);

    vAccelWriteReg(env, 0x21, 0x61, QMA6100P_TIMEOUT);//int, disable i2c

    vAccelWriteReg(env, 0x10, (1 << 6) | (1 << 5), QMA6100P_TIMEOUT);//bandwidth settings

    vAccelWriteReg(env, 0x2C, 0x01, QMA6100P_TIMEOUT);
    vAccelWriteReg(env, 0x2E, 0x03, QMA6100P_TIMEOUT);//motion interrupt
    vAccelWriteReg(env, 0x2F, 0x40, QMA6100P_TIMEOUT);

    vAccelWriteReg(env, 0x18, 0x07, QMA6100P_TIMEOUT);//enable interrupt
    vAccelWriteReg(env, 0x1A, 0x01, QMA6100P_TIMEOUT);//interrupt map

//    vAccelWriteReg(env, 0x09, 0x01, QMA6100P_TIMEOUT);


//    uint16_t err = 0x00;
//    uint8_t res = vAccelReadReg(env, 0x00, &err, QMA6100P_TIMEOUT);
//
//
//    vAccelWriteReg(env, 0x20, 0x05, QMA6100P_TIMEOUT);
//
//
////    vAccelWriteReg(env, 0x09, 0x01, QMA6100P_TIMEOUT);



    //todo return on error, сделать вывод с ошибкой (false) в случае сбоя при записи конфигурации
    return true;
}


bool AccelQma6100P_SetRange(tAccelQma6100P *env, eAccelQma6100pRanges value) {
    uint8_t reg;
    QMA6100P_RINO(vAccelReadReg(env, QMA6100P_REG_FSR, &reg, QMA6100P_TIMEOUT));

    uint8_t regValue;

#define V2R(T) case ACCEL_QMA6100P_RANGE_##T: regValue = ACCEL_QMA6100P_RANGE_REG_##T; break;

    switch (value) {
        V2R(2G)
        V2R(4G)
        V2R(8G)
        V2R(16G)
        V2R(32G)
        default:
            return false;
    }

#undef V2R

    reg = (reg & 0b0000) | regValue;

    QMA6100P_RINO(vAccelWriteReg(env, QMA6100P_REG_FSR, reg, QMA6100P_TIMEOUT));

    env->range = value;

    return true;
}

bool AccelQma6100P_ReadOne(tAccelQma6100P *env, uint8_t firstReg, float *value) {

    uint8_t regLow;
    uint8_t regHigh;

    QMA6100P_RINO(vAccelReadReg(env, firstReg, &regLow, QMA6100P_TIMEOUT));
    QMA6100P_RINO(vAccelReadReg(env, firstReg + 1, &regHigh, QMA6100P_TIMEOUT));

    uint16_t rawRegs = regHigh;
    rawRegs <<= 8;
    rawRegs |= regLow;

//    uint16_t rawRegs;
//    AccelReadAutoIncAdrReg(env->port, firstReg, (uint8_t *)&rawRegs, 2, QMA6100P_TIMEOUT);

    int16_t rowValue = ((int16_t) rawRegs) >> 2;

    int32_t valueMicroG = rowValue * env->range;

    *value = roundf(valueMicroG) / 1000000.f;

    return true;
}

bool AccelQma6100P_Read(tAccelQma6100P *env, vector3 *value) {

    QMA6100P_RINO(AccelQma6100P_ReadOne(env, QMA6100P_REG_ACC_X_L, &value->x));
    QMA6100P_RINO(AccelQma6100P_ReadOne(env, QMA6100P_REG_ACC_Y_L, &value->y));
    QMA6100P_RINO(AccelQma6100P_ReadOne(env, QMA6100P_REG_ACC_Z_L, &value->z));

    return true;
}

#undef QMA6100P_RINO