#include "pcan_can.h"
#include "pcan_led.h"
#include "pcan_packet.h"
#include "pcan_timestamp.h"
#include "pcan_usb.h"
#include "punker.h"
#include <assert.h>
#include <stdint.h>
#include <string.h>

#define SJA1000_BASICCAN 0
#define SJA1000_PELICAN  1
static struct
{
    uint8_t bus_active;
    uint8_t ext_vcc_state;
    uint8_t led_mode;

    uint16_t last_timestamp_sync;
    uint16_t last_flush;
    struct
    {
        /* error handling related */
        uint8_t  err;
        uint8_t  ecc;
        uint8_t  rx_err;
        uint8_t  tx_err;
        /* config */
        uint8_t  btr0;
        uint8_t  btr1;
        uint8_t  silient;
        uint8_t  loopback;
        uint8_t  err_mask;
        uint32_t quartz_freq;
    } can;
    uint8_t  sja1000_shadow[6];
    uint8_t  sja1000_ic_mode;
    uint8_t  device_id;
    uint32_t device_serial;
}
pcan_device =
{
  .device_id = 0xFF,
  .device_serial = 0xFFFFFFFF,

  .can =
  {
    .quartz_freq = PCAN_USB_CRYSTAL_HZ,
    .loopback = 0,
  },
};

#define BLOCK_SIZE           (64)
#define HEADER_SIZE          (2)
#define PCAN_MAX_RECORD_SIZE (BLOCK_SIZE * 14)
typedef struct
{
    /* raw data array */
    uint8_t  buffer[PCAN_MAX_RECORD_SIZE];
    /* ts counter, need to provide correct timestamp correction ( 2 or 1 byte ) */
    uint16_t ts_counter;
    uint16_t pos;
} PCAN_RECORD_BUFFER_EX;

static uint8_t          temp_data_buffer[PCAN_MAX_RECORD_SIZE] = { 0 };
static struct t_m2h_fsm data_fsm                               = {
                                  .state   = 0,
                                  .ep_addr = PCAN_USB_EP_MSGIN,
                                  .pdbuf   = temp_data_buffer,
                                  .dbsize  = sizeof(temp_data_buffer),
};

static PCAN_RECORD_BUFFER_EX pcan_records;

void pcan_sja1000_write(uint8_t reg_addr, uint8_t value)
{
    if (pcan_device.sja1000_ic_mode == SJA1000_PELICAN)
    {
        reg_addr &= 0x7F;
        switch (reg_addr)
        {
            case 6:
                pcan_device.sja1000_shadow[2] = value;
                break;
            case 7:
                pcan_device.sja1000_shadow[3] = value;
                break;
            case 14:
                pcan_device.sja1000_shadow[4] = value;
                break;
            case 15:
                pcan_device.sja1000_shadow[5] = (value == 0xFF) ? 0x7F : value;
                break;
            case 16:
                pcan_device.sja1000_shadow[0] = value;
                break;
            case 20:
                pcan_device.sja1000_shadow[1] = value;
                break;
            case 31:
                if ((value & 0x80) == 0)
                {
                    pcan_device.sja1000_ic_mode = SJA1000_BASICCAN;
                }
                break;
        }
    }
    else if (pcan_device.sja1000_ic_mode == SJA1000_BASICCAN)
    {
        reg_addr &= 0x1F;
        switch (reg_addr)
        {
            case 4:
                pcan_device.sja1000_shadow[0] = value;
                break;
            case 5:
                pcan_device.sja1000_shadow[1] = value;
                break;
            case 6:
                pcan_device.sja1000_shadow[2] = value;
                break;
            case 7:
                pcan_device.sja1000_shadow[3] = value;
                break;
            case 31:
                if ((value & 0x80) != 0)
                {
                    pcan_device.sja1000_ic_mode = SJA1000_PELICAN;
                }
                break;
        }
    }
}

uint8_t pcan_sja1000_read(uint8_t reg_addr)
{
    if (pcan_device.sja1000_ic_mode == SJA1000_PELICAN)
    {
        reg_addr &= 0x7F;
        switch (reg_addr)
        {
            case 0:
                return 0x01;
            case 6:
                return pcan_device.sja1000_shadow[2];
            case 7:
                return pcan_device.sja1000_shadow[3];
            case 14:
                return pcan_device.sja1000_shadow[4];
            case 15:
                return pcan_device.sja1000_shadow[5];
            case 16:
                return pcan_device.sja1000_shadow[0];
            case 20:
                return pcan_device.sja1000_shadow[1];
            case 31:
                return 0x80; /* PeliCAN mode */
            default:
                return 0;
        }
    }
    else if (pcan_device.sja1000_ic_mode == SJA1000_BASICCAN)
    {
        reg_addr &= 0x1F;
        switch (reg_addr)
        {
            case 0:
                return 0x01;
            case 4:
                return pcan_device.sja1000_shadow[0];
            case 5:
                return pcan_device.sja1000_shadow[1];
            case 6:
                return pcan_device.sja1000_shadow[2];
            case 7:
                return pcan_device.sja1000_shadow[3];
            case 31:
                return 0x00; /* BasicCAN mode */
            default:
                return 0;
        }
    }

    return 0;
}

void pcan_record_write_header(uint8_t *p)
{
    p[0] = 0x02; /* record header */
    p[1] = 0x00; /* record count */
}

void pcan_record_buffer_reset(PCAN_RECORD_BUFFER_EX *prec)
{
    pcan_record_write_header(prec->buffer);
    prec->ts_counter = 0;
    prec->pos        = HEADER_SIZE;
}

/* request free slot for specific amount of data */
uint8_t *pcan_record_buffer_request(PCAN_RECORD_BUFFER_EX *prec, uint16_t size)
{
    if (size > (BLOCK_SIZE - HEADER_SIZE))
        return 0;
    uint16_t unused = BLOCK_SIZE - (prec->pos & (BLOCK_SIZE - 1));
    /* allocate new slot ? */
    if (unused < size)
    {
        /* align data to next slot bound */
        uint8_t next = (prec->pos + (BLOCK_SIZE - 1)) & (~((BLOCK_SIZE - 1)));
        if ((next + HEADER_SIZE) >= PCAN_MAX_RECORD_SIZE)
            return 0;
        memset(&prec->buffer[next], 0x00, BLOCK_SIZE);
        pcan_record_write_header(&prec->buffer[next]);
        next += 2;
        prec->pos        = next;
        /* reset ts_counter for next buffer */
        prec->ts_counter = 0;
    }

    if ((prec->pos + size) > PCAN_MAX_RECORD_SIZE)
        return (void *)0;

    return &prec->buffer[prec->pos];
}

void pcan_record_buffer_commit(PCAN_RECORD_BUFFER_EX *prec, uint16_t size)
{
    uint16_t pos = prec->pos & (~(BLOCK_SIZE - 1));
    /* increse record count */
    ++prec->buffer[pos + 1];
    prec->pos += size;
}

uint16_t pcan_record_buffer_flush(PCAN_RECORD_BUFFER_EX *prec)
{
    uint16_t ts_ticks = pcan_timestamp_ticks();
    int      res, flush_size;

    if (prec->pos <= HEADER_SIZE)
    {
        /* try to send ZLP, original 800 */
        if (pcan_device.last_flush && (((uint16_t)(ts_ticks - pcan_device.last_flush)) >= PCAN_TICKS_FROM_US(800)))
        {
            res = pcan_flush_data(&data_fsm, 0, 0);
            if (res)
            {
                pcan_device.last_flush = 0;
            }
        }
        return 0;
    }

    flush_size = (prec->pos + (BLOCK_SIZE - 1)) & (~((BLOCK_SIZE - 1)));
    res        = pcan_flush_data(&data_fsm, prec->buffer, flush_size);
    if (res)
    {
        pcan_record_buffer_reset(prec);
        pcan_device.last_flush = ts_ticks;
        return 1;
    }

    return 0;
}

#if 0
static void pcan_bus_event( PCAN_RECORD_BUFFER_EX *prec, uint8_t flags )
{
  uint8_t *ptr, sl, pos = 0;

  ptr = pcan_record_buffer_request( prec, 6 );
  if( !ptr )
    return;

  /* sl */
  sl = PCAN_USB_STATUSLEN_INTERNAL | 0x03;
  pack_u8( ptr, sl );
  ptr += 1;
  pos += 1;

  /* function */
  pack_u8( ptr, PCAN_USB_REC_BUSEVT );
  ptr += 1;
  pos += 1;

  /* number 0x80, 0x00 */
  pack_u8( ptr, flags );
  ptr += 1;
  pos += 1;

  /* ecc */
  pack_u8( ptr, pcan_device.can.ecc );
  ptr += 1;
  pos += 1;

  /* rx error counter */
  pack_u8( ptr, pcan_device.can.rx_err );
  ptr += 1;
  pos += 1;

  /* tx error counter */
  pack_u8( ptr, pcan_device.can.tx_err );
  ptr += 1;
  pos += 1;

  /* add new record */
  pcan_record_buffer_commit( prec, pos );
}
#endif

#define BUFFER_MINIMAL_GAP_SIZE (16u)
static void pcan_rx_can_frame(PCAN_RECORD_BUFFER_EX *prec, const can_message_t *msg)
{
    uint8_t *ptr, pos = 0;

    ptr = pcan_record_buffer_request(prec, BUFFER_MINIMAL_GAP_SIZE);
    /* wow ! too fast too furious !!! */
    if (!ptr)
    {
        if (!pcan_record_buffer_flush(prec))
        {
            pcan_device.can.err |= PCAN_USB_ERROR_RXQOVR;
            return;
        }
        ptr = pcan_record_buffer_request(prec, BUFFER_MINIMAL_GAP_SIZE);
        /* o_0 */
        if (!ptr)
            return;
    }
    /* recheck dlc for abnormal frames... */
    uint8_t sl = msg->dlc > 8 ? 8 : msg->dlc;

    if (msg->flags & CAN_FLAG_EXTID)
    {
        sl |= PCAN_USB_STATUSLEN_EXT_ID;
    }
    if (msg->flags & CAN_FLAG_RTR)
    {
        sl |= PCAN_USB_STATUSLEN_RTR;
    }

    pack_u8(ptr, sl);

    ptr += 1;
    pos += 1;

    if (sl & PCAN_USB_STATUSLEN_EXT_ID)
    {
        uint32_t id = (msg->id & 0x1FFFFFFF) << 3;
        if (msg->flags & CAN_FLAG_ECHO)
        {
            id |= 0x01;
        }

        pack_u32(ptr, id);

        ptr += 4;
        pos += 4;
    }
    else
    {
        uint16_t id = (msg->id & 0x7FF) << 5;
        if (msg->flags & CAN_FLAG_ECHO)
        {
            id |= 0x01;
        }
        pack_u16(ptr, id);

        ptr += 2;
        pos += 2;
    }

    /* timestamp to ticks, 1 tick is 42.666us */
    uint16_t timestamp16 = msg->timestamp;

    if (prec->ts_counter == 0)
    {
        pack_u16(ptr, timestamp16);
        ptr += 2;
        pos += 2;
    }
    else
    {
        pack_u8(ptr, timestamp16 & 0xFF);
        ptr += 1;
        pos += 1;
    }
    ++prec->ts_counter;

    if (!(sl & PCAN_USB_STATUSLEN_RTR))
    {
        memcpy(ptr, msg->data, msg->dlc);

        ptr += msg->dlc;
        pos += msg->dlc;
    }

    if (msg->flags & CAN_FLAG_ECHO)
    {
        pack_u8(ptr, msg->dummy);
        ptr += 1;
        pos += 1;
    }

    /* add new record */
    pcan_record_buffer_commit(prec, pos);
}

static void pcan_rx_message(can_message_t *msg)
{
    if (!pcan_device.bus_active)
        return;
    if ((msg->flags & CAN_FLAG_ECHO) == 0)
    {
        pcan_led_set_mode(LED_CH0_RX, LED_MODE_BLINK_FAST, 237);
    }

    pcan_rx_can_frame(&pcan_records, msg);
}

static void pcan_can_error(uint8_t err, uint8_t rx_err, uint8_t tx_err)
{
    pcan_device.can.rx_err = rx_err;
    pcan_device.can.tx_err = tx_err;

    if (err & CAN_ERROR_FLAG_BUSOFF)
    {
        pcan_device.can.err |= PCAN_USB_ERROR_BUS_OFF;
    }
}

#define WAIT_FOR_TXSLOTS 1
static uint8_t pcan_decode_data_frame(uint8_t *ptr, uint16_t size, uint8_t flags)
{
    can_message_t msg      = { 0 };
    uint8_t       rec_len  = flags & PCAN_USB_STATUSLEN_DLC;
    /* return back frame to PC */
    uint8_t       srr_flag = 0;

    if (!pcan_device.bus_active)
        return 0;

    if (flags & PCAN_USB_STATUSLEN_EXT_ID)
    {
        uint32_t id;
        if (size < sizeof(uint32_t))
            return 0;
        id       = unpack_u32(ptr);
        srr_flag = id & 0x01;
        id >>= 3;

        msg.flags = CAN_FLAG_EXTID;
        msg.id    = id;

        ptr += sizeof(uint32_t);
        size -= sizeof(uint32_t);
    }
    else
    {
        uint16_t id;
        if (size < sizeof(uint16_t))
            return 0;
        id       = unpack_u16(ptr);
        srr_flag = id & 0x01;
        id >>= 5;

        msg.id = id;

        ptr += sizeof(uint16_t);
        size -= sizeof(uint16_t);
    }

    /* check for MAX DLC */
    msg.dlc = rec_len < 8 ? rec_len : 8;

    if (size < rec_len)
        return 0;

    if (!(flags & PCAN_USB_STATUSLEN_RTR))
    {
        memcpy(msg.data, ptr, msg.dlc);

        ptr += rec_len;
        size -= rec_len;
    }
    else
    {
        msg.flags |= CAN_FLAG_RTR;
    }

    /* self receive flag ? */
    if (srr_flag)
    {
        if (size < sizeof(uint8_t))
            return 0;

        msg.dummy = unpack_u8(ptr);

        ptr += sizeof(uint8_t);
        size -= sizeof(uint8_t);
    }

    msg.timestamp = pcan_timestamp_ticks();

    pcan_led_set_mode(LED_CH0_TX, LED_MODE_BLINK_FAST, 237);

#if WAIT_FOR_TXSLOTS
    const uint16_t ts_poll = pcan_timestamp_ticks();
    /* need more time to send data... ? */
    while (pcan_can_send_message(&msg) < 0)
    {
        /* USB will get NACK and we will not miss other data */
        pcan_can_poll();
        uint16_t ts_diff = pcan_timestamp_ticks() - ts_poll;
        /* we can't tramsit couse bus off or timeout ? */
        if ((pcan_device.can.err & PCAN_USB_ERROR_BUS_OFF) || (ts_diff >= PCAN_TICKS_FROM_US(1000000u)))
        {
            /* tx buffer overflow, drop all data */
            pcan_device.can.err |= PCAN_USB_ERROR_TXFULL;
            return size;
        }
    }
#else
    if (pcan_can_send_message(&msg) < 0)
    {
        /* tx buffer overflow, drop all data */
        pcan_device.can.err |= PCAN_USB_ERROR_TXFULL;
        return size;
    }
#endif
    /* return back to PC */
    if (srr_flag)
    {
        msg.flags |= CAN_FLAG_ECHO;
        pcan_rx_message(&msg);
    }
    return size;
}

static void pcan_set_bitrate(uint8_t *ptr)
{
    /* decode BTR1 */
    volatile uint8_t tseg1 = (ptr[0] & 0xF) + 1;
    volatile uint8_t tseg2 = ((ptr[0] >> 4) & 0x7) + 1;

    /* decode BTR0 */
    volatile uint16_t brp  = (ptr[1] & 0x3f) + 1;
    volatile uint8_t  sjw  = ((ptr[1] >> 6) & 0x3) + 1;

    uint32_t bitrate       = (((PCAN_USB_CRYSTAL_HZ / 2) / brp) / (1 /*tq*/ + tseg1 + tseg2));
    (void)bitrate;

    pcan_can_set_bitrate(brp, tseg1, tseg2, sjw);
}


/* create */
void pcan_timesync_event(PCAN_RECORD_BUFFER_EX *prec)
{
    uint8_t  sl = PCAN_USB_STATUSLEN_INTERNAL | 2 /* record data size */;
    uint8_t *ptr, pos = 0;

    ptr = pcan_record_buffer_request(prec, 5);
    if (!ptr)
        return;

    uint16_t timestamp16 = pcan_timestamp_ticks();

    pack_u8(ptr, sl);
    ptr += 1;
    pos += 1;

    /* function */
    pack_u8(ptr, PCAN_USB_REC_TS);
    ptr += 1;
    pos += 1;

    /* number */
    pack_u8(ptr, 1);
    ptr += 1;
    pos += 1;

    /* timestamp */
    pack_u16(ptr, timestamp16);
    ptr += 2;
    pos += 2;

    /* add new record */
    pcan_record_buffer_commit(prec, pos);

    /* error record */
    pos = 0;
    ptr = pcan_record_buffer_request(prec, 3);
    if (!ptr)
        return;

    pcan_device.can.err &= pcan_device.can.err_mask;

    pack_u8(ptr, PCAN_USB_STATUSLEN_INTERNAL);
    ptr += 1;
    pos += 1;

    pack_u8(ptr, PCAN_USB_REC_ERROR); /* f */
    ptr += 1;
    pos += 1;

    pack_u8(ptr, pcan_device.can.err); /* n */
    ptr += 1;
    pos += 1;

    /* add new record */
    pcan_record_buffer_commit(prec, pos);

    /* clean errors */
    pcan_device.can.err = 0x00;
}

void pcan_protocol_process_command(uint8_t *ptr, uint16_t size)
{
    PCAN_USB_PARAM *cmd = (void *)ptr;

    if (size < PCAN_USB_MSG_HEADER_LEN)
        return;

    switch (cmd->number)
    {
        case PCAN_USB_EX0:
            switch (cmd->function)
            {
                /* set mass storage mode */
                case PCAN_USB_SETCAN2FLASH:
                    break;
            }
            break;
        case PCAN_USB_EX3:
            switch (cmd->function)
            {
                /*  CAN silient control*/
                case PCAN_USB_SET_SILENT_MODE:
                    pcan_device.can.silient = cmd->param[0];
                    pcan_can_set_silent(pcan_device.can.silient);
                    break;
            }
            break;
        case PCAN_USB_SET:
            switch (cmd->function)
            {
                /* BTR0 BTR1 */
                case PCAN_USB_CMD_BITRATE:
                    pcan_device.can.btr0 = cmd->param[1];
                    pcan_device.can.btr1 = cmd->param[0];
                    pcan_set_bitrate(cmd->param);
                    break;
                /* set CAN on/off*/
                case PCAN_USB_CMD_BUS:
                    pcan_device.bus_active = cmd->param[0];
                    pcan_led_set_mode(LED_STAT, pcan_device.bus_active ? LED_MODE_BLINK_SLOW : LED_MODE_OFF, 0);
                    pcan_can_set_bus_active(pcan_device.bus_active);

                    if (pcan_device.bus_active)
                    {
                        /* provide first timesync event */
                        pcan_device.last_timestamp_sync = pcan_timestamp_ticks();
                        pcan_timesync_event(&pcan_records);
                    }
                    /* led state */
                    pcan_led_set_mode(LED_CH0_TX, pcan_device.bus_active ? LED_MODE_OFF : LED_MODE_ON, 0);
                    pcan_led_set_mode(LED_CH0_RX, pcan_device.bus_active ? LED_MODE_OFF : LED_MODE_ON, 0);
                    break;
                /* set device id 0-255 */
                case PCAN_USB_CMD_DEVID:
                    pcan_device.device_id = cmd->param[0];
                    break;
                /* unknown windows driver call */
                case PCAN_USB_CMD_CFG:
                    break;
                /* sja1000 reg write */
                case PCAN_USB_CMD_REGISTER:
                    pcan_sja1000_write(cmd->param[0], cmd->param[1]);
                    break;
                /* ext VCC on/off */
                case PCAN_USB_CMD_EXT_VCC:
                    pcan_device.ext_vcc_state = cmd->param[0];
                    break;
                /* error frame mask */
                case PCAN_USB_CMD_ERR_FR:
                    pcan_device.can.err_mask = cmd->param[0];
                    break;
                /* set led mode */
                case PCAN_USB_CMD_LED:
                    pcan_device.led_mode = cmd->param[0];
                    break;
            }
            break;
        case PCAN_USB_GET:
            switch (cmd->function)
            {
                /* BTR0 BTR1 */
                case PCAN_USB_CMD_BITRATE:
                    cmd->param[1] = pcan_device.can.btr0;
                    cmd->param[0] = pcan_device.can.btr1;
                    pcan_usb_send_command_buffer(cmd, sizeof(PCAN_USB_PARAM));
                    break;
                /* quarts freq in Mhz */
                case PCAN_USB_CMD_CLOCK:
                    {
                        cmd->param[0] = pcan_device.can.quartz_freq / 1000000u;
                        pcan_usb_send_command_buffer(cmd, sizeof(PCAN_USB_PARAM));
                    }
                    break;
                /* get device id */
                case PCAN_USB_CMD_DEVID:
                    {
                        /* default: 255 */
                        cmd->param[0] = pcan_device.device_id;
                        pcan_usb_send_command_buffer(cmd, sizeof(PCAN_USB_PARAM));
                    }
                    break;
                /* serial */
                case PCAN_USB_CMD_SN:
                    {
                        /* default: 4 bytes FFFFFFFF */
                        cmd->param[0] = (pcan_device.device_serial >> 0x18) & 0xFF;
                        cmd->param[1] = (pcan_device.device_serial >> 0x10) & 0xFF;
                        cmd->param[2] = (pcan_device.device_serial >> 0x08) & 0xFF;
                        cmd->param[3] = (pcan_device.device_serial >> 0x00) & 0xFF;
                        pcan_usb_send_command_buffer(cmd, sizeof(PCAN_USB_PARAM));
                    }
                    break;
                /* sja1000 reg read */
                case PCAN_USB_CMD_REGISTER:
                    cmd->param[1] = pcan_sja1000_read(cmd->param[0]);
                    pcan_usb_send_command_buffer(cmd, sizeof(PCAN_USB_PARAM));
                    break;
                /* get led */
                case PCAN_USB_CMD_LED:
                    cmd->param[0] = pcan_device.led_mode;
                    pcan_usb_send_command_buffer(cmd, sizeof(PCAN_USB_PARAM));
                    break;
            }
            break;
        default:
            assert(0);
            break;
    }
}

void pcan_protocol_process_data(uint8_t *ptr, uint16_t size)
{
    uint8_t msg_prefix, msg_rec_count;

    /* invalid frame header len ? */
    if (size < PCAN_USB_MSG_HEADER_LEN)
        return;

    msg_prefix    = *ptr++;
    msg_rec_count = *ptr++;

    (void)msg_prefix; /* avoid warning */

    size -= PCAN_USB_MSG_HEADER_LEN;

    for (uint8_t i = 0; i < msg_rec_count; i++)
    {
        uint16_t eated = 0, sl;

        if (size < 1)
            return;

        sl = unpack_u8(ptr);
        ptr += sizeof(uint8_t);
        size -= sizeof(uint8_t);
        ;

        /* status, error frame */
        if (sl & PCAN_USB_STATUSLEN_INTERNAL)
        {
            /* ??? abnormal, device support only CAN data frame */
            eated = 0;
        }
        /* regular CAN data frame */
        else
        {
            eated = pcan_decode_data_frame(ptr, size, sl);
        }

        /* ? error indicator */
        if (!eated)
            return;

        eated = size - eated;

        if (size < eated)
            return;

        ptr += eated;
        size -= eated;
    }
}

void pcan_protocol_init(void)
{
    pcan_device.sja1000_ic_mode = SJA1000_PELICAN;
    pcan_record_buffer_reset(&pcan_records);
    pcan_can_init();
    pcan_can_install_rx_callback(pcan_rx_message);
    pcan_can_install_error_callback(pcan_can_error);
}

void pcan_protocol_poll(void)
{
    uint16_t ts_ms = pcan_timestamp_ticks();

    if (pcan_device.bus_active)
    {
        /* each ~1000 ms */
        if ((((uint16_t)(ts_ms - pcan_device.last_timestamp_sync)) >= PCAN_TICKS_FROM_US(1000000u)))
        {
            pcan_device.last_timestamp_sync = ts_ms;
            pcan_timesync_event(&pcan_records);
        }
    }
    pcan_record_buffer_flush(&pcan_records);

    pcan_can_poll();
}