#include "imu.h"

/*static I2C_Request* current_req = 0;
static uint8_t i2c_buf[16];
static uint8_t i2c_index = 0;*/

static I2C_Request* i2c_head = 0;
static I2C_Request* i2c_current = 0;

static uint8_t imu_buffer[16];

void imu_pow_init()
{
	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
	GPIOC->MODER &= ~(3 << (13 * 2));
	GPIOC->MODER |= 1 << (13 * 2);
	GPIOC->OTYPER &= ~(1 << 13);
	GPIOC->PUPDR &= ~(3 << (13 * 2));
	GPIOC->BSRR = 1 << (13 + 16);
}

void i2c_gpio_init()
{
	// enable GPIOB clock
	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
	
	// Alt function mode PB8, PB9
	GPIOB->MODER &=  ~((3 << 8 * 2) | (3 << 9 * 2));
	GPIOB->MODER |= (2 << (8 * 2)) | (2 << (9 * 2));
	
	// select AF4 for I2C1 at PB8 and PB9
	GPIOB->AFR[1] &= ~((0xF << ((8 - 8) * 4)) | (0xF << ((9 - 8) * 4)));
	GPIOB->AFR[1] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4)));
	
  // high speed
  GPIOB->OSPEEDR |= (2 << (8 * 2)) | (2 << (9 * 2));
  
	// enable open-drain
	GPIOB->OTYPER |= (1 << 8) | (1 << 9);
	
	// set pull-up
	GPIOB->PUPDR &= ~((3 << (8 * 2)) | (3 << (9 * 2)));
	GPIOB->PUPDR |= (1 << 8 * 2) | (1 << 9 * 2);
}

void i2c1_init()
{
	
	RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN; // enable I2C1
	
	I2C1->TIMINGR = 0x10802D9BUL;         // 400 kHz @ 16 MHz
	
  I2C1->CR1 |= I2C_CR1_PE;
}

void imu_init()
{
	// select bank 0
	i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
	
	// wake up, auto clock
	i2c_write(ICM_ADDR, REG_PWR_MGMT_1, 1);
	
	// select bank 2
	i2c_write(ICM_ADDR, REG_BANK_SEL, 2 << 4);
	
	// gyro ~2000 dps, FS_SEL = 3
	i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, GYRO_FS_SEL_2000 | GYRO_DLPFCFG_73 | GYRO_FCHOICE_ON);
	
	// accel 8g, FS_SEL = 2
	i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, ACCEL_FS_SEL_8 | ACCEL_DLPFCFG_69 | ACCEL_FCHOICE_ON);
	
	// back to bank 0
	i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
}

void imu_tim6_init(const uint16_t freq)
{
	RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
	
  TIM6->CR1 = 0;
  TIM6->ARR = 1000 - 1;
	TIM6->PSC = (SystemCoreClock / 1000 / freq) - 1;
		
	TIM6->DIER |= TIM_DIER_UIE;   // interrupt enable
	TIM6->CR1 |= TIM_CR1_CEN;     // counter enable
	
	NVIC_EnableIRQ(TIM6_DAC_IRQn);
}

void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
{
	i2c_wait_idle(I2C1);
	
	// write register address
	I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
	
	while (!(I2C1->ISR & I2C_ISR_TXIS));
	I2C1->TXDR = reg;
	
	while (!(I2C1->ISR & I2C_ISR_TC));
	
	I2C1->CR2 = (addr << 1) | I2C_CR2_RD_WRN | (len << I2C_CR2_NBYTES_Pos) | I2C_CR2_AUTOEND | I2C_CR2_START;
	
	for (uint8_t i = 0; i < len; ++i)
	{
		while (!(I2C1->ISR & I2C_ISR_RXNE));
		buf[i] = I2C1->RXDR;
	}
	
	while (!(I2C1->ISR & I2C_ISR_STOPF));
	I2C1->ICR |= I2C_ICR_STOPCF;
}

static void i2c_enqueue(I2C_Request* req)
{
    __disable_irq();

    req->Next = i2c_head;
    i2c_head = req;

    __enable_irq();

    // если I2C свободен — запускаем
    if (!i2c_busy)
    {
        NVIC_SetPendingIRQ(I2C1_EV_IRQn);
    }
}

static void i2c_start_next()
{
    if (!i2c_head)
    {
        i2c_busy = 0;
        return;
    }

    i2c_busy = 1;

    i2c_current = i2c_head;
    i2c_head = i2c_head->Next;
    
    I2C1->CR1 |= I2C_CR1_TXIE |
                 I2C_CR1_RXIE |
                 I2C_CR1_TCIE |
                 I2C_CR1_STOPIE;

    NVIC_EnableIRQ(I2C1_EV_IRQn);

    // старт записи регистра
    I2C1->CR2 = (i2c_current->Address << 1) |
                (1 << I2C_CR2_NBYTES_Pos) |
                I2C_CR2_START;
}

void imu_get_async(void (*cb)(uint8_t* data, uint8_t size))
{
    static I2C_Request req;

    req.Callback = cb;
    req.Buffer = imu_buffer;
    req.Size = sizeof(imu_buffer);

    req.Address = ICM_ADDR;
    req.Write = 1;
    req.Read = 12;

    imu_buffer[0] = 0x2D; // регистр

    i2c_enqueue(&req);
}

void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
{
	i2c_wait_idle(I2C1);
	
	// write register address
	I2C1->CR2 = (addr << 1) | (2 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
	
	while (!(I2C1->ISR & I2C_ISR_TXIS));
	I2C1->TXDR = reg;
	
	while (!(I2C1->ISR & I2C_ISR_TXIS));
	I2C1->TXDR = data;
	
	while (!(I2C1->ISR & I2C_ISR_TC));
	I2C1->CR2 |= I2C_CR2_STOP;
}

void I2C1_EV_IRQHandler()
{
  static int test_irq = 0;
  test_irq++;
  
  
    uint32_t isr = I2C1->ISR;

    if (!i2c_current)
    {
        i2c_start_next();
        return;
    }
    
    static uint8_t index = 0;

    // TXIS
    if (isr & I2C_ISR_TXIS)
    {
        I2C1->TXDR = i2c_current->Buffer[0];
    }

    // TC → старт чтения
    else if (isr & I2C_ISR_TC)
    {
        I2C1->CR2 = (i2c_current->Address << 1) |
                    I2C_CR2_RD_WRN |
                    (i2c_current->Read << I2C_CR2_NBYTES_Pos) |
                    I2C_CR2_AUTOEND |
                    I2C_CR2_START;
    }

    // RXNE
    else if (isr & I2C_ISR_RXNE)
    {
        

        i2c_current->Buffer[index++] = I2C1->RXDR;

        if (index >= i2c_current->Read)
            index = 0;
    }

    // STOP
    else if (isr & I2C_ISR_STOPF)
    {
        I2C1->ICR |= I2C_ICR_STOPCF;

        if (i2c_current->Callback)
            i2c_current->Callback(i2c_current->Buffer, i2c_current->Read);

        i2c_current = 0;

        i2c_start_next();
    }
}

void imu_read_raw(imu_raw_t* data)
{
	uint8_t buf[12];
	
	i2c_read(ICM_ADDR, 0x2D, buf, 12);
	
	data->ax = (buf[0] << 8) | buf[1];
	data->ay = (buf[2] << 8) | buf[3];
	data->az = (buf[4] << 8) | buf[5];
	
	data->gx = (buf[6] << 8)  | buf[7];
	data->gy = (buf[8] << 8)  | buf[9];
	data->gz = (buf[10] << 8) | buf[11];
}

static void i2c_wait_idle(I2C_TypeDef* i2c)
{
  int timeout = 100000;
  while ((i2c->ISR & I2C_ISR_BUSY) && --timeout);

  if (timeout == 0)
  {
    // сброс I2C
    i2c->CR1 &= ~I2C_CR1_PE;
    i2c->CR1 |= I2C_CR1_PE;
  }
  
	// while (i2c->ISR & I2C_ISR_BUSY);
	
	i2c->ICR = I2C_ICR_STOPCF |
           I2C_ICR_NACKCF |
           I2C_ICR_BERRCF |
           I2C_ICR_ARLOCF;
}