
#include "PicoInt.h"

int PsndRate=0,PsndLen=0;
short *PsndOut=NULL;

static int KeyOn[8]={0,0,0,0,0,0,0,0};

// A operator is a single sine wave generator
struct Operator
{
  unsigned short angle; // 0-0xffff
  short volume;
  unsigned char key;
};

struct Channel
{
  struct Operator op[4]; // 4 operators for the channel
  unsigned short freq; // Written to 0xa4 and 0xa0
};

static struct Channel Chan[8];


static int WriteReg(int side,int a,int d)
{
  struct Channel *pc=NULL;

  if (a==0x28)
  {
    pc=Chan+(d&7);
    // Key On/Key Off
    pc->op[0].key=(unsigned char)(d>>4);
    return 0;
  }

  // Find channel:
  pc=Chan+(a&3); if (side) pc+=4;

  if ((a&0xf0)==0xa0)
  {
    if (a&4) pc->freq =(unsigned short)(d<<8);
    else     pc->freq|=d;
  }

  return 0;
}

int SoundReset()
{
  int i=0;
  memset(&Chan,0,sizeof(Chan));

//test: Change Sine wave into a square wave
  for (i=0x00; i<0x080; i++) Sine[i]= 0x4000;
  for (i=0x80; i<0x100; i++) Sine[i]=-0x4000;

  return 0;
}

int SoundFm(int a,int d)
{
  int side=0;

  a&=3; side=a>>1; // Which side: channels 0-2 or 3-5

  if (a&1) WriteReg(side,Pico.s.fmsel[side],d); // Write register
  else     Pico.s.fmsel[side]=(unsigned char)d; // Select register

  return 0;
}

static int OneChannel(struct Channel *pc)
{
  int d=0;
  struct Operator *op=NULL;
  int freq=0;

  op=pc->op;
  
  // Adjust envelope:
  if (op->key) { op->volume+=0x100; if (op->volume>=0x4000) op->volume=0x4000; }
  else         { op->volume-=0x100; if (op->volume<      0) op->volume=     0; }
    
  // Work out the frequency of the channel
  freq=pc->freq&0x7ff;
  freq<<=(pc->freq>>11)&7;
  freq*=2455; freq>>=16; //test

  d=Sine[(op->angle>>8)&0xff];

  d*=op->volume; d>>=17;

  // Increment angle:
  op->angle=(unsigned short)(op->angle+freq);

  return d;
}

static int OneSample(short *ps)
{
  int total=0,i=0;

  for (i=0;i<8;i++) total+=OneChannel(Chan+i);

       if (total<-0x8000) total=-0x8000;
  else if (total> 0x7fff) total= 0x7fff;
  
  ps[0]=ps[1]=(short)total;

  return 0;
}

int SoundRender()
{
  short *ps=NULL; int i=0;

  if (PsndOut==NULL) return 1; // Not rendering sound

  for (i=0,ps=PsndOut; i<PsndLen; i++,ps+=2)
  {
    OneSample(ps);
  }

  return 0;
}
