#include <stdio.h>
#include <dos.h>
#include "sb.h"

static void WriteFM(int chip, int addr, unsigned char data)
{
    register int ChipAddr = SbIOaddr + ((chip) ? RIGHT_FM_ADDRESS :
						LEFT_FM_ADDRESS);

    outportb(ChipAddr,addr);
    inportb(ChipAddr);

    outportb(ChipAddr+1,data);
    inportb(ChipAddr);
    inportb(ChipAddr);
    inportb(ChipAddr);
    inportb(ChipAddr);
}

void Sb_FM_Reset(void)
{
    WriteFM(0,1,0);
    WriteFM(1,1,0);
}

void Sb_FM_Key_Off(int voice)
{
    unsigned char reg_num;
    int chip = voice / 11;
    
    /* turn voice off */
    reg_num = 0xB0 + voice % 11;

    WriteFM(chip,reg_num,0);
}

void Sb_FM_Key_On(int voice, int freq, int octave)
{
    register unsigned char reg_num;
    unsigned char tmp;
    int chip = voice / 11;

    reg_num = 0xa0 + voice % 11;

    WriteFM(chip,reg_num,freq & 0xff);

    reg_num = 0xb0 + voice % 11;
    tmp = (freq >> 8) | (octave << 2) | 0x20;

    WriteFM(chip,reg_num,tmp);
}

void Sb_FM_Voice_Volume(int voice, int vol)
{
    unsigned char reg_num;
    int chip = voice / 11;

    reg_num = 0x40 + voice % 11;

    WriteFM(chip,reg_num,vol);
}

void Sb_FM_Set_Voice(int voice_num, FM_Instrument *ins)
{
    register unsigned char op_cell_num;
    int cell_offset;
    int chip = voice_num / 11;

    voice_num %= 11;

    /* check on voice_num range */
    cell_offset = voice_num % 3 + ((voice_num / 3) << 3);

    /* set sound characteristic */
    op_cell_num = 0x20 + (char)cell_offset;

    WriteFM(chip,op_cell_num,ins->SoundCharacteristic[0]);
    op_cell_num += 3;
    WriteFM(chip,op_cell_num,ins->SoundCharacteristic[1]);

    /* set level/output */
    op_cell_num = 0x40 + (char)cell_offset;
    WriteFM(chip,op_cell_num,ins->Level[0]);
    op_cell_num += 3;
    WriteFM(chip,op_cell_num,ins->Level[1]);

    /* set Attack/Decay */
    op_cell_num = 0x60 + (char)cell_offset;
    WriteFM(chip,op_cell_num,ins->AttackDecay[0]);
    op_cell_num += 3;
    WriteFM(chip,op_cell_num,ins->AttackDecay[1]);

    /* set Sustain/Release */
    op_cell_num = 0x80 + (char)cell_offset;
    WriteFM(chip,op_cell_num,ins->SustainRelease[0]);
    op_cell_num += 3;
    WriteFM(chip,op_cell_num, ins->SustainRelease[1]);

    /* set Wave Select */
    op_cell_num = 0xE0 + (char)cell_offset;
    WriteFM(chip,op_cell_num,ins->WaveSelect[0]);
    op_cell_num += 3;
    WriteFM(chip,op_cell_num,ins->WaveSelect[1]);

    /* set Feedback/Selectivity */
    op_cell_num = (unsigned char)0xC0 + (unsigned char)voice_num;
    WriteFM(chip,op_cell_num,ins->Feedback);
}

#ifdef TEST
void main()
{
    static FM_Instrument instrument = {
        0x11, 0x01, 0x8a, 0x40,
        0xf1, 0xf1, 0x11, 0xb3,
        0x00, 0x00, 0x06, 0x00,
        0x00, 0x00, 0x00, 0x00 
    };
    int notes[12] = {0x16B,0x181,0x198,0x1B0,0x1CA,0x1E5,
		     0x202,0x220,0x241,0x263,0x287,0x2AE};

    if(GetSBParams(&SbIOaddr,&SbIRQ,&SbDMAchan))
    {
        puts("BLASTER environment variable not set.");
        exit(1);
    }

    Sb_FM_Reset();

    Sb_FM_Set_Voice(0,&instrument);
    Sb_FM_Set_Voice(1,&instrument);
    Sb_FM_Set_Voice(11,&instrument);
    Sb_FM_Set_Voice(12,&instrument);

    Sb_FM_Key_On(0,notes[11],2);
    Sb_FM_Key_On(1,notes[3],3);
    Sb_FM_Key_On(11,notes[6],3);
    Sb_FM_Key_On(12,notes[11],3);

    getch();

    Sb_FM_Key_Off(0);
    Sb_FM_Key_Off(1);
    Sb_FM_Key_Off(11);
    Sb_FM_Key_Off(12);

    Sb_FM_Reset();
}
#endif