#include "set_emu_tms.h"
#include "set_crc.h"
#include <string.h>
static void analog(uint8_t *reply, unsigned index, int32_t value)
{
    uint16_t word = (uint16_t)value;
    reply[30 + index*2] = (uint8_t)word;
    reply[31 + index*2] = (uint8_t)(word >> 8);
}


void set_tms_init(set_tms_emu *s) { memset(s,0,sizeof(*s)); }
void set_tms_tick(set_tms_emu *s)
{
    int32_t wanted = s->running ? s->target : 0;
    /* 10 Hz/s, signed hundredths of Hz. Independent of UMP recorder. */
    if (++s->divider < 10) return;
    s->divider = 0;
    if (s->frequency < wanted) s->frequency += wanted-s->frequency < 10 ? wanted-s->frequency : 10;
    if (s->frequency > wanted) s->frequency -= s->frequency-wanted < 10 ? s->frequency-wanted : 10;
}


static void apply(set_tms_emu *s)
{
    unsigned i;uint16_t word=s->commands[0];
    for(i=0;i<6;++i)s->controls[i]=(uint8_t)s->commands[4+i];
    s->running=(uint8_t)((s->controls[0]&0x41)==0x41);
    if((s->controls[0]&0x40) && (s->controls[2]&4))
        s->target=word<0x8000?(int32_t)word:(int32_t)word-65536;
}
static void snapshot(set_tms_emu *s,uint8_t *reply)
{
    memset(reply, 0, 101);
    reply[0] = 0; reply[1] = 65;
    /* BALZAM_7: byte10.Go; ready/Go LED; remote selector. */
    reply[11] = s->running;
    reply[14] = (uint8_t)(0x0b | (s->running << 5));
    reply[15] = (uint8_t)(6 | ((s->controls[0] & 0x40) ? 0x20 : 0));
    reply[16] = (uint8_t)(3 | ((s->controls[4] & 0xc0) >> 3));
    analog(reply, 0, s->frequency); analog(reply, 1, 2000);
    analog(reply, 4, s->frequency); analog(reply, 5, s->frequency);
    analog(reply, 6, 540); analog(reply, 7, 540);
    analog(reply, 8, s->frequency / 10); analog(reply, 9, s->frequency / 10);
    analog(reply, 10, s->frequency); analog(reply, 11, s->frequency < 0 ? -1 : 1);
    analog(reply, 12, 540); analog(reply, 13, 540);
    analog(reply, 14, s->frequency); analog(reply, 15, s->frequency);
    analog(reply, 25, 25); analog(reply, 26, 30);
    analog(reply, 28, 400); analog(reply, 29, 400);
    analog(reply, 30, (s->controls[4] & 0x40) ? 120 : 40);
    analog(reply, 31, (s->controls[4] & 0x40) ? 120 : 40);
    analog(reply, 32, (s->controls[4] & 0x80) ? 120 : 40);
    analog(reply, 33, (s->controls[4] & 0x80) ? 120 : 40);
}
unsigned set_tms_read(void *ctx,uint16_t off,uint16_t count,uint16_t *out)
{
    set_tms_emu *s=(set_tms_emu *)ctx;uint8_t r[101];unsigned i,a;
    if(!s || !out || !count || (uint32_t)off+count>SET_TMS_WORDS)return 2;
    snapshot(s,r);
    for(i=0;i<count;++i) {
        uint16_t v=0;a=off+i;
        if(a==0)v=0x544d;
        else if(a==1)v=1;
        else if(a==2)v=s->running;
        else if(a==3)v=(uint16_t)s->frequency;
        else if(a==4)v=(uint16_t)s->target;
        else if(a>=8 && a<18)v=s->commands[a-8];
        else if(a>=32 && a<46)v=(uint16_t)(r[2+(a-32)*2]|((unsigned)r[3+(a-32)*2]<<8));
        else if(a>=46)v=(uint16_t)(r[30+(a-46)*2]|((unsigned)r[31+(a-46)*2]<<8));
        out[i]=v;
    }
    return 0;
}
unsigned set_tms_write(void *ctx,uint16_t off,uint16_t v)
{
    set_tms_emu *s=(set_tms_emu *)ctx;
    if(!s)return 3;
    if(off>=8 && off<18) {
        if(off>=12 && v>255)return 3;
        s->commands[off-8]=v;return 0;
    }
    if(off==18) { if(v!=1)return 3;apply(s);return 0; }
    return 2;
}
size_t set_tms_rtu(set_tms_emu *s,unsigned device,const uint8_t *r,size_t n,
    uint8_t *out,size_t cap)
{
    unsigned i;uint16_t crc;
    if(!s || !r || !out || n!=18 || cap<101 || !device || device>255 ||
       r[0]!=device || r[1]!=65 || set_crc16_modbus(r,n))return 0;
    for(i=0;i<4;++i)s->commands[i]=(uint16_t)(r[2+2*i]|((unsigned)r[3+2*i]<<8));
    for(i=0;i<6;++i)s->commands[4+i]=r[10+i];
    apply(s);snapshot(s,out);out[0]=(uint8_t)device;
    crc=set_crc16_modbus(out,98);out[98]=(uint8_t)crc;out[99]=(uint8_t)(crc>>8);
    return 101;
}