/* test and demo of noise shaping out to wave file

   Note that this version was written in a dielect of 'C'
   that works for the ROOL complier for the RISC OS
   operating system. You'd need to modify some of the
   details to get it to compile on other platforms.
   e.g. here the 'char' is being used to hold bytes
   and you'd need to deal with that differently.
   But it should show a 'C' programmer how the demo
   program models a Noise Shaping system. */

#include <stdio.h>
#include <math.h>
#include "kernel.h"
#include <swis.h>
#include <math.h>
#include <stdlib.h>

_kernel_swi_regs rin,rout;


char datablock[1152000];

int blocksize,icut,icut2,icut24,icut224,ibfactor;


char ChunkID[5],Format[5],Subchunk1ID[5],Subchunk2ID[5];

int ChunkSize,Subchunk1Size,AudioFormat;
int NumChannels,SampleRate,ByteRate,BlockAlign;
int BitsPerSample,Subchunk2Size,BytesPerSample;
int SubChunk1Size, blocks, payload;

FILE* outfile;

void write_output_header(int);

void write4int(int);
void write2int(int);
void write4(char*);

int icut, icut2, icut24, icut224;
double pi2, amp, f, dt, t, phi, dphi, vnow;
int i, ii, j, erin, erout, sc;

int lt, rt, ib, xnow;

int x[2048], plain_dithered[2048], noise_shaped[2048];

int quantise16(int);


double dither24source(void);

int dither8lsbits(void);

void make_waves(void);




int main(void)
{
  
  /* set up waveform values, etc */
  
  icut=32768; icut2=2*icut;  icut24=icut*256; icut224=2*icut24; 
  pi2=3.1415927*2.0; erin=0;
  
  f=500.0;              /* test frequency in Hz */
  dt = 1.0/192000.0;    /* test sample interval */
  
  amp = (double)icut224;
  amp = amp/10000.0;    /* test amplitude about 80dB down on 24bit max */
  
  dphi = dt*pi2*f;      /* phase rate of signal */
  
  t=0.0; i=0; phi=0.0;
  
  /* output a test sequence so the user can see that
     values are being produced by the required processes
     and see their sizes, etc. */
  
  do
  {
    
    vnow=amp*sin(phi)+dither24source();  /* This line generates a dithered sinewave value */
    
    x[i]=(int)vnow;                      /* converted to an int */
    
    /* The next line does a plain dither down to a 16 bit value */ 
    
    plain_dithered[i]= 256 * quantise16( x[i]+dither8lsbits() );  

    
    /* The next four lines do a simple Noise Shaped 24 -> 16 bit conversion */
    
    erout=erin;
    sc=x[i]-erout;
    noise_shaped[i]= 256 * quantise16( sc + dither8lsbits() );
    erin=noise_shaped[i]-sc;
    
    
    printf("%8.4lf,  %6d, %6d, %6d\n",t*1000000.0,x[i],plain_dithered[i],noise_shaped[i]);
    
    /* update time and sinewave phase */
    
    phi+=dphi;
    i++;
    t+=dt;
    
    
  } while (i<2000);
  
  
  /* Now do a similar process to save the results into a 192k 16bit wave file */
  
  /* set up 16 bit 192k output file format */
  
  SampleRate=192000;  BytesPerSample=2;  BitsPerSample=16;  BlockAlign=4;
  SubChunk1Size=16;   AudioFormat=1;     NumChannels=2;     ibfactor=4;
  
  blocksize=SampleRate*BytesPerSample*2;
  dt=1.0/(double)SampleRate;
  ByteRate=blocksize;
  
  blocks=10;  /* Number of seconds for content of wave file */
  
  payload=blocks*ByteRate;
  
  printf("Payload in Bytes = %d\n",payload);
  
  
  sprintf(ChunkID,"RIFF\0");
  sprintf(Format,"WAVE\0");
  sprintf(Subchunk1ID,"fmt \0");
  sprintf(Subchunk2ID,"data\0");
  
  /* Note the output filename below. Change this as required */
  
  outfile=fopen("ram:shapetest/wav","wd");
  
  write_output_header(payload);
  
  t=0.0; phi=0.0; erin=0; erout=0; /* reset waveform clock  then make waves... */
  
  j=0; 
  do
  {
    ib=0;
    
    do
    {
      make_waves();
      
    } while (ib<blocksize);
    
    fwrite(datablock,sizeof(char),blocksize,outfile);
    
    j++;
    printf("sec %d done\n",j);
  } while (j<blocks);
 
 
  fclose(outfile);
                                    
}

void make_waves(void)
{
    int l1,l2,r1,r2;
    erout=erin;
    
    vnow=amp*sin(phi) + dither24source();
    
    xnow=(int)vnow;
    
    
    lt = quantise16( xnow+dither8lsbits() ); /* plain dithered */
    
    
    sc=xnow-erout;
    rt=quantise16( sc + dither8lsbits() );  /* noise shaped */
    erin= (256 * rt) - sc;
    
    phi+=dphi;
    
    
    if( phi > pi2)
    {
      phi -= pi2;
    }
    
    
    /* now write into datablock */
    
    if(lt<0) lt+=2*icut24;
    if(rt<0) rt+=2*icut24;
    
    l1=(char)((lt>>8)&255);
    l2=(char)(lt&255);
    r1=(char)((rt>>8)&255);
    r2=(char)(rt&255);
    
    datablock[ib]=l2;
    datablock[ib+1]=l1;
    datablock[ib+2]=r2;
    datablock[ib+3]=r1;
    
    ib+=4;
  
  
  
}


int quantise16(int iq)
{
  int answer;
  
  if (iq>=0)
  {
    iq+=128;
  }
  else
  {
    iq-=128;
  }
  
  answer=iq/256;
  
  /* Note the output is scaled down to 16 bit range */
  
  return answer;
  
}


double dither24source(void)
{
  /* This procedure returns a 'random dither'
  level as a real value in the range from -1.0
  to -1.0 with a triangular probability density */
  
  double answer;
  int dit;
  dit=rand()%128 + rand()%128;
  answer=(double)(dit-128);
  return answer/128.0;
}



int dither8lsbits(void)
{
  /* This procedure returns a 'random dither'
  levet as an integer in the range +256 to -256 */
  
  int answer;
  
  answer=rand()%256 + rand()%256 - 256;
  return answer;
}


void write_output_header(int plo)
{
  
  printf("Payload to write = %d\n",plo);
  write4(ChunkID);
  write4int(plo+36);
  write4(Format);
  write4(Subchunk1ID);
  write4int(Subchunk1Size);
  write2int(AudioFormat);
  write2int(NumChannels);
  write4int(SampleRate);
  write4int(ByteRate);
  write2int(BlockAlign);
  write2int(BitsPerSample);
  write4(Subchunk2ID);
  write4int(plo);
  
}

void write4int(int inv)
{
  char cop1,cop2,cop3,cop4;
  
  cop1=(char)(inv&255);
  cop2=(char)((inv>>8)&255);
  cop3=(char)((inv>>16)&255);
  cop4=(char)((inv>>24)&255);
  
  fprintf(outfile,"%c",cop1);
  fprintf(outfile,"%c",cop2);
  fprintf(outfile,"%c",cop3);
  fprintf(outfile,"%c",cop4);
}


void write2int(int inv)
{
  char cop1,cop2;
  
  cop1=(char)(inv&255);
  cop2=(char)((inv>>8)&255);
  
  
  fprintf(outfile,"%c",cop1);
  fprintf(outfile,"%c",cop2);
  
}

void write4(char* outstring)
{
  int isp;
  
  isp=0;
  do
  {
    fprintf(outfile,"%c",outstring[isp]);
    isp++;
  } while (isp<4);
  
}