#include <stdint.h>
#include <stdio.h>
#include "main.h"
#include "usbd_cdc_if.h"

#define I2C_DEBUG 0

#define I2C_MTU 512

#define HAL_CHECK(x) do { \
    int rv = (x);         \
    if (rv != HAL_OK) {   \
        printf("HAL ERR %d at %s:%d\r\n", rv, __FILE__, __LINE__);  \
    }\
} while (0)

/**
 * Packet format:
 *
 * Transmit packets
 *   LEN_MSB LEN_LSB ADDR Bytes...
 *
 * LEN does not include the length field itself!
 *
 * Command packets are sent as ADDR=255
 *   0xFF CMD ARGs...
 *   - Set address: 0x01 ADDR
 *   - Set speed:   0x02 Speed TODO!!
 *     - Speed 1 ... 100kHz
 *     - Speed 4 ... 400kHz
 */

#define COMMAND_ADDRESS 0xFF
#define CMD_SET_ADDR  1  // the next param is the address, without shifting (i.e. 1-127)
#define CMD_SET_SPEED 2  // speed is 1 or 4 (100khz, 400khz)

enum FrameState {
    FRAME_LEN_MSB,
    FRAME_LEN_LSB,
    FRAME_PAYLOAD,
};

static uint16_t framelen = 0;
static uint8_t i2c_recbuf[I2C_MTU + 3] = {};
static size_t recbuf_wp = 0;
static size_t framestate = FRAME_LEN_MSB;
static uint32_t last_rx_tick;
#define CMD_TIMEOUT_TICKS 500


static void handle_pkt(uint8_t *packet, size_t len);
static void handle_control_pkt(uint8_t *packet, size_t len);

void i2c_command(uint8_t *packet, size_t len)
{
    uint32_t now = HAL_GetTick();
    if (framestate != FRAME_LEN_MSB && (now - last_rx_tick) > CMD_TIMEOUT_TICKS) {
        framestate = FRAME_LEN_MSB;
    }
    last_rx_tick = now;

    for (int i=0; i<len;i++) {
        uint8_t b = packet[i];
        switch (framestate) {
            case FRAME_LEN_MSB:
                framelen = ((uint16_t)b) << 8;
                framestate = FRAME_LEN_LSB;
                break;
            case FRAME_LEN_LSB:
                framelen |= (uint16_t)b;
                if (framelen == 0) {
                    framestate = FRAME_LEN_MSB;
                } else {
                    framestate = FRAME_PAYLOAD;
                    recbuf_wp = 0;
                }
                break;
            case FRAME_PAYLOAD:
                i2c_recbuf[recbuf_wp++] = b;
                if (recbuf_wp >= framelen) {
                    handle_pkt(i2c_recbuf, framelen);
                    framestate = FRAME_LEN_MSB;
                }
                break;
        }
    }
}

static void handle_pkt(uint8_t *packet, size_t len)
{
#if I2C_DEBUG
    printf("Handle packet (%d bytes): ", len);
    for(int i=0; i<len;i++) {
        printf("%02x ", packet[i]);
    }
    printf("\r\n");
#endif

    if (packet[0] == COMMAND_ADDRESS) {
        printf("ctl: ");
        // This is an in-band control packet
        handle_control_pkt(packet, len);
    } else {
        uint8_t dest = packet[0];

#if I2C_DEBUG
        printf("Tx(%d)->%d\r\n", len - 1, dest);
#endif

        // Blocking transfer
        HAL_CHECK(HAL_I2C_DisableListen_IT(&hi2c1));
        HAL_CHECK(HAL_I2C_Master_Transmit(&hi2c1, (dest & 0x7F) << 1, &packet[1], len - 1, 100));
        HAL_CHECK(HAL_I2C_EnableListen_IT(&hi2c1));
    }
}

static void handle_control_pkt(uint8_t *packet, size_t len)
{
    uint8_t cmd = packet[1];
    uint8_t arg;

    switch (cmd) {
        case CMD_SET_ADDR:
            if (len < 3) return;
            arg = packet[2];
            printf("Addr=0x%02x\r\n", arg);
            MODIFY_REG(hi2c1.Instance->OAR1,
                       (I2C_OAR1_ADDMODE | I2C_OAR1_ADD8_9 | I2C_OAR1_ADD1_7 | I2C_OAR1_ADD0),
                       (I2C_ADDRESSINGMODE_7BIT | ((arg & 0x7F) << 1) ));
            HAL_CHECK(HAL_I2C_EnableListen_IT(&hi2c1));
            break;

        default:
            printf("bad cmd!\r\n");
    }
}

/**
  * @brief  Slave Address Match callback.
  * @param  hi2c Pointer to a I2C_HandleTypeDef structure that contains
  *                the configuration information for the specified I2C.
  * @param  TransferDirection Master request Transfer Direction (Write/Read), value of @ref I2C_XferDirection_definition
  * @param  AddrMatchCode Address Match Code
  * @retval None
  */
void HAL_I2C_AddrCallback(I2C_HandleTypeDef *hi2c, uint8_t TransferDirection, uint16_t AddrMatchCode)
{
    HAL_CHECK(HAL_I2C_Slave_Seq_Receive_IT(hi2c, i2c_recbuf, I2C_MTU, I2C_FIRST_AND_LAST_FRAME));
}

/**
  * @brief  Slave Rx Transfer completed callback.
  * @param  hi2c Pointer to a I2C_HandleTypeDef structure that contains
  *                the configuration information for the specified I2C.
  * @retval None
  */
void HAL_I2C_SlaveRxCpltCallback(I2C_HandleTypeDef *hi2c)
{
    int numBytes = I2C_MTU - hi2c->XferCount;
#if I2C_DEBUG
    printf("rxd %d\r\n", numBytes);
#endif

    UserTxBufferFS[0] = (numBytes >> 8) & 0xFF;
    UserTxBufferFS[1] = numBytes & 0xFF;
    memcpy(&UserTxBufferFS[2], i2c_recbuf, numBytes);

    CDC_Transmit_FS(UserTxBufferFS, numBytes + 2);
}

/**
  * @brief  Listen Complete callback.
  * @param  hi2c Pointer to a I2C_HandleTypeDef structure that contains
  *                the configuration information for the specified I2C.
  * @retval None
  */
void HAL_I2C_ListenCpltCallback(I2C_HandleTypeDef *hi2c)
{
    // Start again
    HAL_CHECK(HAL_I2C_EnableListen_IT(hi2c));
}