#include "TrackerMpu.h"

#include <../2025_include_user/Print.h>

#define TRACKER_ADDR 0x12
#define TRACKER_DATA_ADDR 0x30

uint8_t TrackerMpu::_buffer = 0x00;
TrackerMpu::Data TrackerMpu::_data;

void WriteByte(const uint8_t addr, const uint8_t dat) {
    uint8_t temp[2];
    temp[0] = addr;
    temp[1] = dat;

    DL_I2C_fillControllerTXFIFO(I2C_1_INST, temp, 2);
    while (!(DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))

        DL_I2C_startControllerTransfer(I2C_1_INST, 0x12, DL_I2C_CONTROLLER_DIRECTION_TX, 2);
    while (DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS)
        while (!(DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE))
            DL_I2C_flushControllerTXFIFO(I2C_1_INST);
}

uint8_t ReadByte(const uint8_t addr) {
    uint8_t data;

    DL_I2C_fillControllerTXFIFO(I2C_1_INST, &addr, 1);

    DL_I2C_disableInterrupt(I2C_1_INST, DL_I2C_INTERRUPT_CONTROLLER_TXFIFO_TRIGGER);

    while (!(DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
    DL_I2C_startControllerTransfer(I2C_1_INST, 0x12, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    while (DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
    while (!(DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
    //    DL_I2C_flushControllerTXFIFO(I2C_1_INST);

    DL_I2C_startControllerTransfer(I2C_1_INST, 0x12, DL_I2C_CONTROLLER_DIRECTION_RX, 1);
    while (DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
    while (!(DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_IDLE));
    data = DL_I2C_receiveControllerData(I2C_1_INST);

    return data;
}

void TrackerMpu::init() {}

void TrackerMpu::inject() {
    WriteByte(0x01, 1);
    WriteByte(0x01, 0);
    TrackerMpu::_buffer = ReadByte(0x30);
    // print("Raw: %u (0x%02X)\n", _buffer, _buffer);
    for (uint8_t i = 0; i < 8; i++) {
        TrackerMpu::_data.data[i] = (TrackerMpu::_buffer >> (7 - i)) & 0x01;
    }
}

TrackerMpu::Data TrackerMpu::getData() {
    TrackerMpu::inject();
    return TrackerMpu::_data;
}