#include <cstdint>
#include <cstring>
#include <stm32f10x_conf.h>
#include <stm32f10x.h>
#include <stm32f10x_rcc.h>
#include <stm32f10x_gpio.h>
#include <stm32f10x_i2c.h>
#include <inttypes.h>
#include <playground.h>
#include "debug.h"
#include "tinyprintf.h"

extern "C" void halt();

extern "C"
__attribute__((naked))
void HardFault_Handler_C(uint32_t *hardfault_args) {
    dbg_printf("r0  = 0x%08lx (%lu)\n", hardfault_args[0], hardfault_args[0]);
    dbg_printf("r1  = 0x%08lx (%lu)\n", hardfault_args[1], hardfault_args[1]);
    dbg_printf("r2  = 0x%08lx (%lu)\n", hardfault_args[2], hardfault_args[2]);
    dbg_printf("r3  = 0x%08lx (%lu)\n", hardfault_args[3], hardfault_args[3]);
    dbg_printf("r12 = 0x%08lx (%lu)\n", hardfault_args[4], hardfault_args[4]);
    dbg_printf("lr  = 0x%08lx (%lu)\n", hardfault_args[5], hardfault_args[5]);
    dbg_printf("pc  = 0x%08lx (%lu)\n", hardfault_args[6], hardfault_args[6]);
    dbg_printf("psr = 0x%08lx (%lu)\n", hardfault_args[7], hardfault_args[7]);
    dbg_printf("\n");

    halt();
}

size_t strlen(const char *s) {
    size_t size = 0;
    while (*s++ != '\0') size++;
    return size;
}

bool run = true;

volatile USART_TypeDef *usart1 = (volatile USART_TypeDef *) USART1_BASE;
volatile I2C_TypeDef *i2c1 = (volatile I2C_TypeDef *) I2C1_BASE;
volatile I2C_TypeDef *i2c2 = (volatile I2C_TypeDef *) I2C2_BASE;

extern "C"
void assert_failed(uint8_t *file, uint32_t line) {
    dbg_printf("assert_failed: %s: %" PRIu32 "\n", file, line);
}

int start_wait_count;

void i2c_hard_reset(I2C_TypeDef *I2Cx, I2C_InitTypeDef &i2c_init) {
    dbg_printf("start_wait_count=%d\n", start_wait_count);
    I2C_DeInit(I2Cx);
    I2C_Cmd(I2Cx, DISABLE);
    I2C_Init(I2Cx, &i2c_init);

    GPIO_InitTypeDef init;
    GPIO_StructInit(&init);
    init.GPIO_Pin = GPIO_Pin_10;
    init.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_Init(GPIOB, &init);

    GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_RESET);
    for (int i = 0; i < 8; i++) {
        GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);
        GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_RESET);
    }

    init.GPIO_Pin = GPIO_Pin_10;
    init.GPIO_Mode = GPIO_Mode_AF_OD;
    GPIO_Init(GPIOB, &init);

    I2C_Cmd(I2Cx, ENABLE);

    I2C_SoftwareResetCmd(I2Cx, ENABLE);
    for (int i = 0; i < 100; i++) {
        __NOP();
    }
    I2C_SoftwareResetCmd(I2Cx, DISABLE);
}

static inline bool i2c_wait_for_flag(I2C_TypeDef *I2Cx, uint32_t flag, FlagStatus status, unsigned int timeout) {
    do {
        auto flagStatus = I2C_GetFlagStatus(I2Cx, flag);
        if (flagStatus == status) {
            return true;
        }
    } while (timeout-- > 0);

    return false;
}

static inline bool i2c_wait_for_event(I2C_TypeDef *I2Cx, uint32_t expected, unsigned int timeout) {
    uint32_t actual;
    do {
        actual = I2C_GetLastEvent(I2Cx);
        if (actual == expected) {
            return true;
        }
    } while (timeout-- > 0);

    dbg_printf("i2c_wait_for_event, expected=%08" PRIx32 ", actual=%08" PRIx32 "\n", expected, actual);

    return false;
}

const unsigned int timeout = 1000 * 1000;

bool i2c_write(I2C_TypeDef *port, uint8_t address, uint8_t *buf, uint8_t count, bool generate_stop) {
    if (!i2c_wait_for_flag(port, I2C_FLAG_BUSY, RESET, timeout)) {
        dbg_printf("Failed while waiting for I2C_FLAG_BUSY\n");
        return false;
    }

    I2C_GenerateSTART(port, ENABLE);

    if (!i2c_wait_for_event(port, I2C_EVENT_MASTER_MODE_SELECT, timeout)) {
        dbg_printf("Failed while waiting for I2C_EVENT_MASTER_MODE_SELECT\n");
        return false;
    }

    I2C_Send7bitAddress(port, address, I2C_Direction_Transmitter);

    if (!i2c_wait_for_event(port, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED, timeout)) {
        dbg_printf("Failed while waiting for I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED\n");
        return false;
    }

    do {
        I2C_SendData(port, *buf++);

        if (!i2c_wait_for_event(port, I2C_EVENT_MASTER_BYTE_TRANSMITTED, timeout)) {
            dbg_printf("Failed while waiting for I2C_EVENT_MASTER_BYTE_TRANSMITTED\n");
            return false;
        }
    } while (--count);

    if (generate_stop) {
        I2C_GenerateSTOP(port, ENABLE);

        while (I2C_GetFlagStatus(port, I2C_FLAG_STOPF)) {
            __NOP();
        }
    }

    return true;
}

bool i2c_read(I2C_TypeDef *port, uint8_t address, uint8_t *buf, uint8_t count, bool generate_start) {
    start_wait_count = 0;
    int dbg_counter = 0;
    (void) dbg_counter;

    if (generate_start) {
        if (!i2c_wait_for_flag(port, I2C_FLAG_BUSY, RESET, timeout)) {
            dbg_printf("Failed while waiting for I2C_FLAG_BUSY\n");
            return false;
        }

        I2C_AcknowledgeConfig(port, ENABLE);
//    dbg_printf("%2d SR2 SR1 = %08" PRIx32 "\n", dbg_counter, I2C_GetLastEvent(port));
        I2C_NACKPositionConfig(port, I2C_NACKPosition_Current);
//    dbg_printf("%2d SR2 SR1 = %08" PRIx32 "\n", dbg_counter, I2C_GetLastEvent(port));

        I2C_GenerateSTART(port, ENABLE);

//    dbg_printf("%2d SR2 SR1 = %08" PRIx32 "\n", dbg_counter, I2C_GetLastEvent(port));

        /* I2C_EVENT_MASTER_MODE_SELECT = BUSY, MSL and SB flag */
        if (!i2c_wait_for_event(port, I2C_EVENT_MASTER_MODE_SELECT, timeout)) {
            dbg_printf("Failed while waiting for I2C_EVENT_MASTER_MODE_SELECT\n");
            return false;
        }

        I2C_Send7bitAddress(port, address, I2C_Direction_Receiver);

        // I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED == BUSY, MSL and ADDR
        if (!i2c_wait_for_event(port, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED, timeout)) {
            dbg_printf("Failed while waiting for I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED\n");
            return false;
        }
    }

    if (count == 1) {
        I2C_AcknowledgeConfig(port, DISABLE);

        __disable_irq();
        (void) port->SR2;
        I2C_GenerateSTOP(port, ENABLE);
        __enable_irq();

        while (!I2C_CheckEvent(port, I2C_EVENT_MASTER_BYTE_RECEIVED)) {
            __NOP();
        }

        *buf = I2C_ReceiveData(port);
    } else if (count == 2) {
        I2C_NACKPositionConfig(port, I2C_NACKPosition_Next);

        __disable_irq();
        (void) port->SR2;
        I2C_AcknowledgeConfig(port, DISABLE);
        __enable_irq();

        while (!I2C_CheckEvent(port, I2C_EVENT_MASTER_BYTE_RECEIVED)) {
            __NOP();
        }

        __disable_irq();
        I2C_GenerateSTOP(port, ENABLE);
        *buf++ = I2C_ReceiveData(port);
        __enable_irq();
        *buf = I2C_ReceiveData(port);
    } else {
        do {
            if (!i2c_wait_for_event(port, I2C_EVENT_MASTER_BYTE_RECEIVED, timeout)) {
                dbg_printf("Failed while waiting for I2C_EVENT_MASTER_BYTE_RECEIVED, count=%d\n", count);
                return false;
            }

            *buf++ = I2C_ReceiveData(port);
        } while (--count != 2);

        I2C_AcknowledgeConfig(port, DISABLE);

        __disable_irq();
        *buf++ = I2C_ReceiveData(port);
        I2C_GenerateSTOP(port, ENABLE);
        __enable_irq();

        *buf++ = I2C_ReceiveData(port);

        if (!i2c_wait_for_event(port, I2C_EVENT_MASTER_BYTE_RECEIVED, timeout)) {
            dbg_printf("Failed while waiting for I2C_EVENT_MASTER_BYTE_RECEIVED\n");
            return false;
        }

        *buf = I2C_ReceiveData(port);
    }

    while (I2C_GetFlagStatus(port, I2C_FLAG_STOPF)) {
        __NOP();
    }

    return true;
}

int main() {
    SystemInit();

    init_printf(nullptr, dbg_putc);

    dbg_printf("i2c1\n");

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO
                           | RCC_APB2Periph_USART1
                           | RCC_APB2Periph_GPIOA
                           | RCC_APB2Periph_GPIOB
                           | RCC_APB2Periph_GPIOC,
                           ENABLE);

    /* ***************************************** */

    // Debug on port B

    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, ENABLE);
    RCC_APB2PeriphResetCmd(RCC_APB2Periph_GPIOB, DISABLE);

    // Make Port B's pin #5 the debug output pin
    GPIO_InitTypeDef init;
    GPIO_StructInit(&init);
    init.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7;
    init.GPIO_Mode = GPIO_Mode_Out_PP;
    init.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_Init(GPIOB, &init);

    /* ***************************************** */

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2,
                           ENABLE);

    /*
     * PB10 I2C2_SCL
     * PB11 I2C2_SDA
     */

    RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C2, ENABLE);
    RCC_APB1PeriphResetCmd(RCC_APB1Periph_I2C2, DISABLE);

    GPIO_StructInit(&init);
    init.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
    init.GPIO_Mode = GPIO_Mode_AF_OD;
    GPIO_Init(GPIOB, &init);

    /*
    i2c2->CR2 |= 36; // APB1 freq is 36 MHz
    // 36 MHz = 27.78 ns, 400 kHz = 2500ns
    // 2500 ns / 27.78 ns = 89.99
    i2c2->CCR |= I2C_CCR_FS | 90;
    */

    I2C_TypeDef *const i2c_port = I2C2;
    I2C_DeInit(i2c_port);
    I2C_InitTypeDef i2c_init;
    I2C_StructInit(&i2c_init);
    i2c_init.I2C_ClockSpeed = 100 * 1000;
    i2c_init.I2C_Ack = I2C_Ack_Enable;
    i2c_init.I2C_Mode = I2C_Mode_I2C;
    I2C_Init(i2c_port, &i2c_init);
    I2C_Cmd(i2c_port, ENABLE);

    const int DS3231_address = 0b11010000; // 0x68

    run = true;
    bool toggle = true;
    bool success = true;
    while (run) {
        GPIO_WriteBit(GPIOB, GPIO_Pin_5, toggle ? Bit_SET : Bit_RESET);
        GPIO_WriteBit(GPIOB, GPIO_Pin_6, toggle ? Bit_SET : Bit_RESET);
        GPIO_WriteBit(GPIOB, GPIO_Pin_7, toggle ? Bit_SET : Bit_RESET);
        toggle = !toggle;

        if (!success) {
            dbg_printf("resetting...\n");
            i2c_hard_reset(i2c_port, i2c_init);
        }

        uint8_t buf[12];
        for (size_t i = 0; i < SizeOfArray(buf); i++) {
            buf[i] = 0;
        }

        success = i2c_write(i2c_port, DS3231_address, buf, 1, false);

        if (!success) {
            dbg_printf("i2c_write failed\n");
            continue;
        }

        success = i2c_read(i2c_port, DS3231_address, buf, SizeOfArray<uint8_t>(buf), false);

        if (success) {
            dbg_printf(
                "%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d\n"
//                "%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x" /*" %02x %02x %02x %02x"*/ "\n"
                    "\n",
//                buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], buf[8],
//                buf[9], buf[10], buf[11], buf[12], buf[13], buf[14], buf[15],
                buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], buf[8],
                buf[9], buf[10], buf[11]/*, buf[12], buf[13], buf[14], buf[15]*/);
        }
    }

    return 0;
}