#include "instrument.h"
#include <math.h>
#include <stdlib.h>

/* A hard limit */
#define REVERB_MAX 0

/* This is not exposed to other files, as this should be controlled by
   individual instruments. */
typedef struct {
  float freq;
  float amp;
  unsigned char pred;           /* if this is 0, then no frequency
                                   modulation is present. */
} FM;

/* By similar reasoning, ADSR is not exposed either. */
typedef struct {
  float attack_p;
  float decay_p;
  float sustain_p;
  float release_p;
  float sustain_level;
  unsigned char pred;           /* If this is 0 then the instrument
                                   does not adapt the ADSR model. */
} ADSR;

/* NOTE: The default method for an instrument to produce sounds is in
   accordance with its FM, ADSR, and Fourier coefficients.  But
   individual instruments can override that default method by
   providing their own functions.  This way we would have more
   flexibility. */

struct Instrument_s {
  float *sin_coefs;             /* coefficients of sin in the Fourier
                                   expansion. */
  float *cos_coefs;             /* coefficients of cos in the Fourier
                                   expansion. */
  sound_t sound;                /* the function to produce sounds. */
  merger_t merger;              /* the function to merge waves. */
  float reverb;                 /* The percentage of reverberation. */
  FM fm;
  ADSR adsr;
};

WaveFrag
make_sound(Instrument * in, Volume v, Hertz h, Seconds s)
{
  return in->sound(in, v, h, s);
}

U_ATTR
WaveFrag
merge_waves(Instrument *in, WaveFrag *frags, LENT len, int step)
{
  return in->merger(in, frags, len, step);
}

/* Now comes various instruments */

/* The first is piano */

UH_ATTR
static float
smooth_step(float t)
{
  if (t <= 0.0f) {
    return 0.0f;
  } else if (t >= 1.0f) {
    return 1.0f;
  } else {
    return 6*pow(t, 5)-15*pow(t, 4)+10*pow(t, 3);
  }
}

UH_ATTR
static float
old_piano_sound_internal(Instrument *piano, float theta)
{
  return sin(theta)+1.869*sin(2.0f*theta)+0.042f*sin(3.0f*theta)
    +0.022f*sin(4.0f*theta)+cos(theta);
}

UH_ATTR
static float
piano_sound_internal(Instrument *piano, float theta)
{
  return (*(piano->sin_coefs) * sin(theta)+
          *(piano->sin_coefs+1) * sin(2.0f*theta)+
          *(piano->sin_coefs+2) * sin(3.0f*theta))*
    exp(-0.004f*theta); 
}

UH_ATTR
WaveFrag
piano_sound(void * in, Volume v, Hertz h, Seconds s)
{
  Instrument *piano = (Instrument *)in;
  
  float step = (float) (h * 2 * M_PI) / SAMPLE_RATE;
  LENT sample_num = (LENT) floor(SAMPLE_RATE * s);
  
  Wave w = MYALLOC(Pulse, sizeof(*w) * sample_num);
  
  float theta = 0.0f, temp = 0.0f;
  
  for (LENT i = 0; i < sample_num; i++, theta += step) {
    temp = piano_sound_internal(piano, theta);
    temp += temp*temp*temp;
    *(w+i) = (float) v * piano_sound_internal(piano, temp);
  }

  return (WaveFrag) { w, sample_num };
}

/* Just concatenates waves together. */
UH_ATTR
WaveFrag
simple_merger(void *in, WaveFrag *frags, LENT len, int step)
{
  WaveFrag wf;

  LENT total_len = 0;

  for (LENT i = 0; i < len; i += step)
    total_len += (frags+i)->n;

  wf.n = total_len;
  wf.w = MYALLOC(Pulse, total_len);

  for (LENT i = 0, counter = 0, local = 0;
       i < len && counter < total_len;
       i += step) {
    local = (frags+i)->n;
    
    for (LENT j = 0; j < local;)
      *(wf.w+counter++) = *((frags+i)->w+j++);
    
    free((frags+i)->w);
  }

  free(frags);

  return wf;  
}

Instrument *make_piano()
{
  Instrument *ins = malloc(sizeof *ins * 1);

  /* The piano's sound is simulated directly by some coefficients. */
  ins->fm.pred = 0;
  ins->adsr.pred = 0;
  ins->sin_coefs = malloc(sizeof(*(ins->sin_coefs)) * 3);
  ins->cos_coefs = malloc(sizeof(*(ins->cos_coefs)) * 1);
  ins->reverb = 0.0f;

  *(ins->sin_coefs) = 0.65f;
  *(ins->sin_coefs+1) = 0.45f;
  *(ins->sin_coefs+2) = 0.1f;
  *(ins->cos_coefs) = 1;
  ins->sound = piano_sound;
  ins->merger = simple_merger;

  return ins;
}

void
destroy_piano(Instrument *in)
{
  free(in->sin_coefs);
  free(in->cos_coefs);
  free(in);
}

/* Then comes violin */

UH_ATTR
Pulse
violin_sound_internal(Instrument *in, float theta)
{
  /* return -1.0f*fmod(theta, 2.0f*M_PI)/M_PI + 1.0f; */
  /* return (*(in->sin_coefs)*sin(theta)+
   *         *(in->sin_coefs+1)*sin(2.0f*theta)+
   *         *(in->sin_coefs+2)*sin(3.0f*theta)+
   *         *(in->sin_coefs+3)*sin(4.0f*theta)+
   *         *(in->sin_coefs+4)*sin(5.0f*theta)); */
  Pulse result = 0.0f;
  for (int i = 0; i < 26; i++)
    result += *(in->sin_coefs+i) * sin((float) (i+1)*theta);
  /* result = sin(theta); */
  
  return result;
}

UH_ATTR
WaveFrag
violin_sound(void * in, Volume v, Hertz h, Seconds s)
{
  Instrument *violin = (Instrument *)in;
  
  /* We produce a little more samples so that we can create the effect
     of reverberation later on. */
  s *= (1.0f+violin->reverb);
  
  float step = (float) (h * 2 * M_PI) / SAMPLE_RATE;
  LENT sample_num = (LENT) floor(SAMPLE_RATE * s);
  float attack_step = 1.0f / (float) (sample_num * violin->adsr.attack_p);
  float decay_step = 1.0f / (float) (sample_num * violin->adsr.decay_p);
  float sustain_step = 1.0f / (float) (sample_num * violin->adsr.sustain_p);
  float release_step = 1.0f / (float) (sample_num * violin->adsr.release_p);

  Wave w = MYALLOC(Pulse, sizeof(*w) * sample_num);
  
  float theta = 0.0f, temp = 0.0f, adsrcounter = 0.0f;

  unsigned char phase = 0, sustain_flag = 0;
  
  for (LENT i = 0; i < sample_num; i++, theta += step) {
    temp = violin_sound_internal(violin, theta);
    /* temp += temp*temp*temp; */
    /* *(w+i) = (float) v * temp * ((sustain_flag) ? SUSTAIN_LEVEL : adsrcounter); */
    *(w+i) = (float) v * temp;
    
    switch (phase) {
    case 0:                     /* attack phase */
      adsrcounter += attack_step;
      if (adsrcounter >= 1.0f) {
        adsrcounter = 1.0f;
        phase++;
      }
      break;
    case 1:                     /* decay phase */
      adsrcounter -= decay_step;
      if (adsrcounter <= SUSTAIN_LEVEL) {
        adsrcounter = 0.0f;
        sustain_flag = 1;
        phase++;
      }
      break;
    case 2:                     /* sustain phase */
      adsrcounter += sustain_step;
      if (adsrcounter >= 1.0f) {
        adsrcounter = SUSTAIN_LEVEL;
        sustain_flag = 0;
        phase++;
      }
      break;
    default:                    /* release phase */
      adsrcounter -= release_step;
      if (adsrcounter <= 0.0f) adsrcounter = 0.0f;
      break;
    }
  }

  return (WaveFrag) { w, sample_num };
}

UH_ATTR
WaveFrag
violin_merger(void *in, WaveFrag *frags, LENT len, int step)
{
  Instrument *violin = (Instrument*) in;
 
  WaveFrag wf, local_last_wave = (WaveFrag) { NULL, 0 };
  WaveFrag local_current_wave = (WaveFrag) { NULL, 0 };
  WaveFrag local_mix_wave, local_mix_input[2];

  LENT total_len = 0;
  
  float reverb_ratio = 1.0f / (1.0f + violin->reverb);

  for (LENT i = 0; i < len; i += step)
    total_len += (LENT) ((frags+i)->n * reverb_ratio);

  wf.n = total_len;
  wf.w = MYALLOC(Pulse, total_len);

  for (LENT i = 0, counter = 0, local = 0, local_last = 0;
       i < len && counter < total_len;
       i += step) {
    /* The last fragment that we want to mix. */
    local_last = (!i) ? 0 : (frags+i-1)->n - local;
    local = (LENT) ((frags+i)->n * reverb_ratio);
    /* Make sure everything stays within bounds. */
    local_last = min(local_last, local);
    local_last = min(local_last, REVERB_MAX);

    if (local_last && i) {
      /* If we reach here then i > 0. */
      local_last_wave.n = local_last;
      local_last_wave.w = realloc(local_last_wave.w,
                                  sizeof(Pulse) * local_last);

      local_current_wave.n = local_last;
      local_current_wave.w = realloc(local_current_wave.w,
                                     sizeof(Pulse) * local_last);
      
      for (LENT j = 0; j < local_last; j++) {
        *(local_last_wave.w+j) =
          *((frags+i-1)->w+(frags+i-1)->n-local_last+j);

        *(local_current_wave.w+j) = *((frags+i)->w+j);
      }

      local_mix_input[0] = local_last_wave;
      local_mix_input[1] = local_current_wave;

      local_mix_wave = mix_waves(local_mix_input, 2);

      for (LENT j = 0; j < local_last;)
        *(wf.w+counter++) = *(local_mix_wave.w+j++);

      free(local_mix_wave.w);
      
      for (LENT j = local_last; j < local;)
        *(wf.w+counter++) = *((frags+i)->w+j++);
    } else {
      for (LENT j = 0; j < local;)
        *(wf.w+counter++) = *((frags+i)->w+j++);
    }
    
    free((frags+i)->w);
  }

  free(local_last_wave.w);
  free(local_current_wave.w);

  free(frags);

  return wf;  
}

UH_ATTR
Instrument *
make_violin()
{
  Instrument * violin= malloc(sizeof *violin);
  
  violin->fm.pred = 0;
  violin->adsr.pred = 1;
  
  violin->reverb = 0.01f;
  violin->adsr.attack_p = 0.55;
  violin->adsr.release_p = 0.25;
  violin->adsr.decay_p = 0.05;
  violin->adsr.sustain_p = max(1.0f - (violin->adsr.attack_p+
                                       violin->adsr.decay_p+
                                       violin->adsr.release_p),
                               0.0f);
  violin->adsr.sustain_level = 0.9;
  
  double temp [] = {
    1.0, 0.399064778, 0.229404484, 0.151836061,
    0.196754229, 0.093742264, 0.060871957,
    0.138605419, 0.010535002, 0.071021868,
    0.029954614, 0.051299684, 0.055948288,
    0.066208224, 0.010067391, 0.00753679,
    0.008196947, 0.012955577, 0.007316738,
    0.006216476, 0.005116215, 0.006243983,
    0.002860679, 0.002558108, 0.0, 0.001650392
  };
  
  violin->sin_coefs = malloc(sizeof(float)*26);
  for (int i = 0; i < 26; i++)
    *(violin->sin_coefs+i) = *(temp+i);
  /* *(violin->sin_coefs) = 1.0f;
   * *(violin->sin_coefs+1) = 0.41f;
   * *(violin->sin_coefs+2) = 0.35f;
   * *(violin->sin_coefs+3) = 0.39f;
   * *(violin->sin_coefs+4) = 0.45f;
   * *(violin->sin_coefs+5) = 0.35f;
   * *(violin->sin_coefs+6) = 0.5f;
   * *(violin->sin_coefs+7) = 0.4f;
   * *(violin->sin_coefs+8) = 0.35f;
   * *(violin->sin_coefs+9) = 0.1f; */

  violin->sound = violin_sound;
  violin->merger = simple_merger;

  return violin;
}

UH_ATTR
void
destroy_violin(Instrument *in)
{
  free(in->sin_coefs);
  free(in);
}