#include "altera_stream.h"
#include "pcan_id.h"
#include <string.h>

typedef struct {
    pcan_parser_t parser;
    uint64_t indices[LAS_CAPACITY], next_index;
    uint16_t samples[LAS_CAPACITY];
    uint8_t breaks[LAS_CAPACITY];
    uint32_t head, count, period, session, received, missing, duplicates, invalid, ignored;
    uint8_t device, metadata, started;
} las_context;
static uint16_t r16(const uint8_t *p) { return (uint16_t)(p[0]|((uint16_t)p[1]<<8)); }
static uint32_t r32(const uint8_t *p) { return (uint32_t)r16(p)|((uint32_t)r16(p+2)<<16); }
static void w16(uint8_t *p,uint16_t v) { p[0]=(uint8_t)v;p[1]=(uint8_t)(v>>8); }
static void w32(uint8_t *p,uint32_t v) { w16(p,(uint16_t)v);w16(p+2,(uint16_t)(v>>16)); }
uint32_t las_device_type(void) { return LAS_DEVICE_TYPE; }
uint32_t las_device_id(void) { return LAS_DEVICE_ID; }
const char *las_device_name(void) { return "Altera Logic"; }
size_t las_context_size(void) { return sizeof(las_context); }
int las_init(void *ctx,uint32_t device) {
    las_context *s=(las_context *)ctx;
    if(!ctx || device>15) return 0;
    memset(s,0,sizeof(*s));s->device=(uint8_t)device;pcan_parser_init(&s->parser);return 1;
}
static void append(las_context *s,uint64_t index,uint16_t value,uint8_t gap) {
    s->indices[s->head]=index;s->samples[s->head]=value;s->breaks[s->head]=gap;
    s->head=(s->head+1U)%LAS_CAPACITY;
    if(s->count<LAS_CAPACITY) ++s->count;
}
void las_can(void *ctx,uint32_t raw,const uint8_t *p,size_t n,uint32_t extended,uint32_t remote) {
    las_context *s=(las_context *)ctx;pcan_id_t id;uint32_t index,delta,period,session;
    if(!extended || remote || raw>PCAN_ID_MASK) { ++s->ignored;return; }
    pcan_id_unpack(raw,&id);
    if(id.device_type!=LAS_DEVICE_TYPE || id.device_id!=s->device ||
       id.route!=PCAN_ROUTE_FROM_DEVICE || id.msg_type!=PCAN_MSG_GAS ||
       (id.msg_body!=LAS_GAS_META && id.msg_body!=LAS_GAS_DATA)) { ++s->ignored;return; }
    if(n!=8 || !p) { ++s->invalid;return; }
    if(id.msg_body==LAS_GAS_META) {
        period=r32(p+4);session=r16(p+2);
        if(r16(p)!=LAS_TAG || period==0) { ++s->invalid;return; }
        if(s->metadata && session==s->session) {
            if(period!=s->period) ++s->invalid;
            return;
        }
        if(s->metadata && (uint16_t)(session-s->session)>=0x8000U) {
            ++s->duplicates;return;
        }
        s->metadata=1;s->session=session;s->period=period;
        s->count=0;s->head=0;s->started=0;s->next_index=0;
        return;
    }
    if(!s->metadata) { ++s->ignored;return; }
    if(r16(p)!=s->session) { ++s->ignored;return; }
    index=r32(p+4);
    delta=0;
    if(s->started) {
        delta=index-(uint32_t)s->next_index;
        if(delta>=0x80000000U) { ++s->duplicates;return; }
        s->missing+=delta;
    } else s->next_index=index;
    s->next_index+=delta;
    append(s,s->next_index,r16(p+2),(uint8_t)(!s->started || delta!=0));
    s->next_index+=1;s->started=1;++s->received;
}
static void frame_callback(const pcan_frame_t *frame,void *user) {
    if(frame->flags & (PCAN_FLAG_DIR|PCAN_FLAG_ERR)) {
        ++((las_context *)user)->ignored;return;
    }
    las_can(user,frame->id,frame->data,frame->dlc,
            frame->flags&PCAN_FLAG_IDE,frame->flags&PCAN_FLAG_RTR);
}
void las_uart(void *ctx,const uint8_t *data,size_t size) {
    las_context *s=(las_context *)ctx;
    if(data) pcan_parser_feed(&s->parser,data,size,frame_callback,s);
}
uint32_t las_get(const void *ctx,uint32_t field) {
    const las_context *s=(const las_context *)ctx;
    switch(field) {
        case LAS_COUNT:return s->count;case LAS_PERIOD_NS:return s->period;
        case LAS_SESSION:return s->session;case LAS_RECEIVED:return s->received;
        case LAS_MISSING:return s->missing;case LAS_DUPLICATES:return s->duplicates;
        case LAS_INVALID:return s->invalid;case LAS_IGNORED:return s->ignored;
        case LAS_CRC_ERRORS:return s->parser.stats.crc_errors;
        case LAS_HAS_META:return s->metadata;default:return 0;
    }
}
size_t las_snapshot(const void *ctx,uint64_t *indices,uint16_t *samples,uint8_t *breaks,size_t cap) {
    const las_context *s=(const las_context *)ctx;uint32_t i,pos;
    if(!indices || !samples || !breaks || cap<s->count) return 0;
    pos=(s->head+LAS_CAPACITY-s->count)%LAS_CAPACITY;
    for(i=0;i<s->count;++i) {
        indices[i]=s->indices[pos];samples[i]=s->samples[pos];breaks[i]=s->breaks[pos];
        pos=(pos+1U)%LAS_CAPACITY;
    }
    return s->count;
}
size_t las_trace(const void *ctx,uint32_t channel,uint64_t *indices,uint8_t *levels,
                 uint8_t *moves,size_t cap) {
    const las_context *s=(const las_context *)ctx;uint32_t i,pos;
    uint64_t previous_x=0;uint8_t previous_y=0;size_t n=0;
    if(channel>=16 || !indices || !levels || !moves || cap<2U*s->count) return 0;
    pos=(s->head+LAS_CAPACITY-s->count)%LAS_CAPACITY;
    for(i=0;i<s->count;++i) {
        uint64_t x=s->indices[pos];uint8_t y=(uint8_t)((s->samples[pos]>>channel)&1U);
        if(!i) { indices[n]=x;levels[n]=y;moves[n++]=1; }
        else if(s->breaks[pos]) {
            indices[n]=previous_x;levels[n]=previous_y;moves[n++]=0;
            indices[n]=x;levels[n]=y;moves[n++]=1;
        } else if(y!=previous_y) {
            indices[n]=x;levels[n]=previous_y;moves[n++]=0;
            indices[n]=x;levels[n]=y;moves[n++]=0;
        }
        previous_x=x;previous_y=y;pos=(pos+1U)%LAS_CAPACITY;
    }
    if(s->count>1) { indices[n]=previous_x;levels[n]=previous_y;moves[n++]=0; }
    return n;
}
static size_t packet(uint32_t device,uint16_t body,const uint8_t *data,uint32_t uart,
                     uint32_t *raw,uint8_t *out,size_t cap) {
    pcan_id_t id;pcan_frame_t frame;
    if(device>15 || !raw || !out || cap<(uart?PCAN_FRAME_MAX:8U)) return 0;
    memset(&id,0,sizeof(id));id.device_type=LAS_DEVICE_TYPE;id.device_id=(uint8_t)device;
    id.priority=PCAN_PRIORITY_STANDARD;id.route=PCAN_ROUTE_FROM_DEVICE;
    id.msg_type=PCAN_MSG_GAS;id.msg_body=body;
    memset(&frame,0,sizeof(frame));frame.id=pcan_id_pack(&id);
    frame.flags=PCAN_FLAG_IDE;frame.dlc=8;memcpy(frame.data,data,8);*raw=frame.id;
    if(uart) return pcan_frame_encode(&frame,out,cap);
    memcpy(out,data,8);return 8;
}
size_t las_metadata(uint32_t device,uint32_t session,uint32_t period,uint32_t uart,
                    uint32_t *id,uint8_t *out,size_t cap) {
    uint8_t data[8];if(session>65535 || !period) return 0;
    w16(data,LAS_TAG);w16(data+2,(uint16_t)session);w32(data+4,period);
    return packet(device,LAS_GAS_META,data,uart,id,out,cap);
}
size_t las_data(uint32_t device,uint32_t session,uint32_t index,uint32_t sample,uint32_t uart,
               uint32_t *id,uint8_t *out,size_t cap) {
    uint8_t data[8];if(session>65535 || sample>65535) return 0;
    w16(data,(uint16_t)session);w16(data+2,(uint16_t)sample);w32(data+4,index);
    return packet(device,LAS_GAS_DATA,data,uart,id,out,cap);
}
void las_demo_step(void *ctx,uint32_t count) {
    las_context *s=(las_context *)ctx;uint32_t i,bit,idx,raw;uint8_t packet_data[PCAN_FRAME_MAX];size_t n;
    if(!s->metadata) {
        n=las_metadata(s->device,1,1000000,1,&raw,packet_data,sizeof(packet_data));
        las_uart(s,packet_data,n);
    }
    if(count>4096) count=4096;
    for(i=0;i<count;++i) {
        uint16_t a=0;idx=(uint32_t)s->next_index;
        for(bit=0;bit<16;++bit) {
            if((idx/(5U+bit*7U))&1U) a|=(uint16_t)(1U<<bit);
        }
        n=las_data(s->device,s->session,idx,a,1,&raw,packet_data,sizeof(packet_data));
        las_uart(s,packet_data,n);
    }
}