#include "ibm.h"
#include "sound_ym7128.h"

static int attenuation[32];
static int tap_position[32];

void ym7128_init(ym7128_t *ym7128) {
        int c;
        double out = 65536.0;

        for (c = 0; c < 32; c++)
                tap_position[c] = c * (2400 / 31);

        for (c = 31; c >= 1; c--) {
                attenuation[c] = (int)out;
                out /= 1.25963; /*2 dB steps*/
        }
        attenuation[0] = 0;
}

#define GET_ATTENUATION(val) (val & 0x20) ? -attenuation[val & 0x1f] : attenuation[val & 0x1f]

void ym7128_write(ym7128_t *ym7128, uint8_t val) {
        int new_dat = val & 1;
        int new_sci = val & 2;
        int new_a0 = val & 4;
        //        pclog("ym7128_write %i %i %i\n", new_dat, new_sci, new_a0);
        if (!ym7128->sci && new_sci)
                ym7128->dat = (ym7128->dat << 1) | new_dat;

        if (ym7128->a0 != new_a0) {
                //                pclog("ym7128 write %i %02x\n", ym7128->a0, ym7128->dat);
                if (!ym7128->a0)
                        ym7128->reg_sel = ym7128->dat & 0x1f;
                else {
                        //                        pclog("YM7128 write %02x %02x\n", ym7128->reg_sel, ym7128->dat);
                        switch (ym7128->reg_sel) {
                        case 0x00:
                        case 0x01:
                        case 0x02:
                        case 0x03:
                        case 0x04:
                        case 0x05:
                        case 0x06:
                        case 0x07:
                                ym7128->gl[ym7128->reg_sel & 7] = GET_ATTENUATION(ym7128->dat);
                                //                                pclog(" GL[%i] = %04x\n", ym7128->reg_sel & 7,
                                //                                GET_ATTENUATION(ym7128->dat));
                                break;
                        case 0x08:
                        case 0x09:
                        case 0x0a:
                        case 0x0b:
                        case 0x0c:
                        case 0x0d:
                        case 0x0e:
                        case 0x0f:
                                ym7128->gr[ym7128->reg_sel & 7] = GET_ATTENUATION(ym7128->dat);
                                //                                pclog(" GR[%i] = %04x\n", ym7128->reg_sel & 7,
                                //                                GET_ATTENUATION(ym7128->dat));
                                break;

                        case 0x10:
                                ym7128->vm = GET_ATTENUATION(ym7128->dat);
                                //                                pclog(" VM = %04x\n", GET_ATTENUATION(ym7128->dat));
                                break;
                        case 0x11:
                                ym7128->vc = GET_ATTENUATION(ym7128->dat);
                                //                                pclog(" VC = %04x\n", GET_ATTENUATION(ym7128->dat));
                                break;
                        case 0x12:
                                ym7128->vl = GET_ATTENUATION(ym7128->dat);
                                //                                pclog(" VL = %04x\n", GET_ATTENUATION(ym7128->dat));
                                break;
                        case 0x13:
                                ym7128->vr = GET_ATTENUATION(ym7128->dat);
                                //                                pclog(" VR = %04x\n", GET_ATTENUATION(ym7128->dat));
                                break;

                        case 0x14:
                                ym7128->c0 = (ym7128->dat & 0x3f) << 6;
                                if (ym7128->dat & 0x20)
                                        ym7128->c0 |= 0xfffff000;
                                break;
                        case 0x15:
                                ym7128->c1 = (ym7128->dat & 0x3f) << 6;
                                if (ym7128->dat & 0x20)
                                        ym7128->c1 |= 0xfffff000;
                                break;

                        case 0x16:
                        case 0x17:
                        case 0x18:
                        case 0x19:
                        case 0x1a:
                        case 0x1b:
                        case 0x1c:
                        case 0x1d:
                        case 0x1e:
                                ym7128->t[ym7128->reg_sel - 0x16] = tap_position[ym7128->dat & 0x1f];
                                //                                pclog(" T[%i] = %i\n", ym7128->reg_sel - 0x16,
                                //                                tap_position[ym7128->dat & 0x1f]);
                                break;
                        }
                        ym7128->regs[ym7128->reg_sel] = ym7128->dat;
                }
                ym7128->dat = 0;
        }

        ym7128->sci = new_sci;
        ym7128->a0 = new_a0;
}

#define GET_DELAY_SAMPLE(ym7128, offset)                                                                                         \
        (((ym7128->delay_pos - offset) < 0) ? ym7128->delay_buffer[(ym7128->delay_pos - offset) + 2400]                          \
                                            : ym7128->delay_buffer[ym7128->delay_pos - offset])

void ym7128_apply(ym7128_t *ym7128, int16_t *buffer, int len) {
        int c, d;

        for (c = 0; c < len * 2; c += 4) {
                /*YM7128 samples a mono stream at ~24 kHz, so downsample*/
                int32_t samp =
                        ((int32_t)buffer[c] + (int32_t)buffer[c + 1] + (int32_t)buffer[c + 2] + (int32_t)buffer[c + 3]) / 4;
                int32_t filter_temp, filter_out;
                int32_t samp_l = 0, samp_r = 0;

                filter_temp = GET_DELAY_SAMPLE(ym7128, ym7128->t[0]);
                filter_out = ((filter_temp * ym7128->c0) >> 11) + ((ym7128->filter_dat * ym7128->c1) >> 11);
                filter_out = (filter_out * ym7128->vc) >> 16;

                samp = (samp * ym7128->vm) >> 16;
                samp += filter_out;

                ym7128->delay_buffer[ym7128->delay_pos] = samp;

                for (d = 0; d < 8; d++) {
                        samp_l += (GET_DELAY_SAMPLE(ym7128, ym7128->t[d + 1]) * ym7128->gl[d]) >> 16;
                        samp_r += (GET_DELAY_SAMPLE(ym7128, ym7128->t[d + 1]) * ym7128->gr[d]) >> 16;
                }

                samp_l = (samp_l * ym7128->vl * 2) >> 16;
                samp_r = (samp_r * ym7128->vr * 2) >> 16;

                buffer[c] += ((int32_t)samp_l + (int32_t)ym7128->prev_l) / 2;
                buffer[c + 1] += ((int32_t)samp_r + (int32_t)ym7128->prev_r) / 2;
                buffer[c + 2] += samp_l;
                buffer[c + 3] += samp_r;

                ym7128->delay_pos++;
                if (ym7128->delay_pos >= 2400)
                        ym7128->delay_pos = 0;

                ym7128->filter_dat = filter_temp;
                ym7128->prev_l = samp_l;
                ym7128->prev_r = samp_r;
        }
}
