#include <avr/pgmspace.h>
#include "mult8x8.h"
//#include <AudioCodec.h>

unsigned int signal1 = 0;
unsigned int signal2 = 0;
unsigned int signal3 = 0;
unsigned int signal4 = 0;
unsigned int frequency1 = 1;
unsigned int frequency2 = 1;
unsigned int frequency3 = 1;
unsigned int frequency4 = 1;
byte out_temp1;
byte out_temp2;
byte out_temp3;
byte out_temp4;
byte midi = 0;
byte lastmessage = 0;
byte amp1 = 0;
byte amp2 = 0;
byte amp3 = 0;
byte amp4 = 0;
byte ramp1 = 3;
byte ramp2 = 3;
byte ramp3 = 3;
byte ramp4 = 3;
unsigned int count = 0;
byte mask1 = 0xff;
byte mask2= 0xff;
byte mask3 = 0xff;
byte mask4 = 0xff;
byte feedback = 0;
unsigned int rando = 0xdead;
byte sweep1 = 0;
unsigned int sweep2 = 0;
unsigned int sweep3 = 0;
unsigned int sweep4 = 0;
byte attack1 = 0x80;
byte attack2 = 0x80;
byte attack3 = 0x80;
byte attack4 = 0x80;
byte decay1 = 0x01;
byte decay2 = 0x01;
byte decay3 = 0x01;
byte decay4 = 0x01;
byte timer = 0;
unsigned int freq1 = 0x0010;
unsigned int freq2 = 0x01ff;
unsigned int freq3 = 0x01ff;
unsigned int freq4 = 0x01ff;
byte fm_mod1 = 0;
byte fm_mod2 = 0;
byte fm_mod3 = 0;
byte fm_mod4 = 0;
char filt[1024];
unsigned int filt_ptr = 0;
unsigned int filt_freq = 127;
byte control = 0;
volatile byte trig_fm = 0;
unsigned int fm_freq = 0x037f;
unsigned int fm_count = 0;
unsigned int filt_freq_temp = 0;

PROGMEM prog_uint8_t sinewave[]  = {
  #include <sine_8b_1k.inc>
};

PROGMEM prog_int16_t notetable[]  = {
  #include <notelookup2.inc>
};

PROGMEM prog_uint8_t cymbal[]  = {
  #include <cymbal2.inc>
};

PROGMEM prog_uint16_t logtable[]  = {
  #include <logtable2.inc>
};

void setup() {
  //  Set MIDI baud rate:
  UCSR0A &= ~((1<<U2X0)|(1<<MPCM0));
  UCSR0B = (1<<RXEN0);
  UCSR0C = ((1<<UCSZ01)|(1<<UCSZ00));
  UBRR0H = (((F_CPU/31250)/16) - 1) >> 8;
  UBRR0L = ((F_CPU/31250)/16) - 1;
  // set t1 for envelope generation
  TCCR1A = (1<<WGM11)|(0<<WGM10); // phase/freq correct
  TCCR1B = (1<<WGM13)|((0<<WGM12)|(5<<CS10)); // icr1a as top, ck/1024
  OCR1AH = 0;
  OCR1AL = 15; // interrupt timing
  OCR1BH = 0;
  OCR1BL = 45; // interrupt timing
  ICR1H = 0;
  ICR1L = 75; // top initialization
  TIMSK1 = (1<<ICIE1)|(1<<OCIE1B)|(1<<OCIE1A);//|(1<<TOIE1); // enable all interrupts
  // set t2 for audio generation
  TCCR2A = (2<<COM2A0)|(2<<COM2B0)|(1<<WGM20); // phase correct on oc2a and oc2b
  TCCR2B = (1<<CS20); // ck/1
  TIMSK2 = (1<<TOIE2); // enable overflow interrupt
  DDRB |= (1<<DDB3); // output for oc2a
  DDRD |= (1<<DDD3); // output for oc2b
  // set t0 for audio generation
  TCCR0A = (2<<COM0A0)|(2<<COM0B0)|(1<<WGM00); // ocr0a as top
  TCCR0B = (1<<CS00); // ck/1
  TIMSK0 = 0; // turn off all interrupts
  DDRD |= (1<<DDD6)|(1<<DDD5); // outputs for oc0a and oc0b
  // synchronize t0 and t2
  GTCCR = (1<<TSM); // halt the prescaler
  TCNT0 = 0; // clear the counters
  TCNT2 = 0;
  GTCCR = 0; // restart the counters
}

void loop() {
  if (UCSR0A & (1<<RXC0)) { // check for data in buffer
    byte data = UDR0; // get data from buffer
    if (data & 0x80) { // check if command byte
      if (data == 0x90) midi = 1; // note on
      else if (data == 0xb0) midi = 8; // control change
      else midi = 0; // none
    }
    else if (midi == 1) { // get note number
      lastmessage = data;
      midi = 2; // wait for velocity
    }
    else if (midi == 2) { // get note velocity
      byte temp = lastmessage;
      if (data) { // note-on velocity
        if (temp == 20) {
          cli();
          filt_freq_temp = filt_freq;
          ramp1 = 1;
          sei();
        }
        else if (temp == 21) {
          cli(); // block during modifications
          if (trig_fm) frequency2 = freq2 - (fm_mod2 >> 1);
          else frequency2 = freq2 + (fm_mod2 >> 1);
          ramp2 = 1;
          sei();
        }
        else if (temp == 22) {
          cli(); // block during modifications
          if (trig_fm) frequency3 = freq3 - (fm_mod3 >> 1);
          else frequency3 = freq3 + (fm_mod3 >> 1);
          ramp3 = 1;
          sei();
        }
        else if (temp == 23) {
          cli(); // block during modifications
          if (trig_fm) frequency4 = freq4 - (fm_mod4 >> 1);
          else frequency4 = freq4 + (fm_mod4 >> 1);
          ramp4 = 1;
          sei();
        }
      }
      midi = 1; // wait for next note (running status)
    }
    else if (midi == 8) { // control change
      if (data == 10) midi = 10; // distortion
      else if (data == 11) midi = 11; // attack
      else if (data == 12) midi = 12; // decay
      else if (data == 13) midi = 13; // feedback
      else if (data == 14) midi = 14; // feedback
      else if (data == 15) midi = 15; // filter decay
      else if (data == 16) midi = 16; // filter decay
      else if (data == 17) midi = 17; // filter cutoff
      else midi = 20; // none of the above
    }
    else if (midi == 10) {
      control = data >> 5;
      midi = 8;
    }
    else if (midi == 11) {
      if (control == 0) attack1 = data + 1;
      else if (control == 1) attack2 = data + 1;
      else if (control == 2) attack3 = data + 1;
      else attack4 = data + 1;
      midi = 8;
    }
    else if (midi == 12) {
      if (control == 0) fm_mod1 = data;
      else if (control == 1) decay2 = (data >> 1) + 1;
      else if (control == 2) decay3 = (data >> 1) + 1;
      else decay4 = (data >> 1) + 1;
      midi = 8;
    }
    else if (midi == 13) {
      if (control == 0) sweep1 = data;
      else if (control == 1) sweep2 = data + 0xffc0;
      else if (control == 2) sweep3 = data + 0xffc0;
      else sweep4 = data + 0xffc0;
      midi = 8;
    }
    else if (midi == 14) {
      if (control == 0) filt_freq = pgm_read_byte_near(logtable + data);
      else if (control == 1) freq2 = data + 1;
      else if (control == 2) freq3 = pgm_read_word_near(notetable + data);
      else freq4 = pgm_read_word_near(notetable + data);
      midi = 8;
    }
    else if (midi == 15) {
      if (control == 0) feedback = data << 1;
      else if (control == 1) fm_mod2 = data << 1;
      else if (control == 2) fm_mod3 = data << 1;
      else fm_mod4 = data << 1;
      midi = 8;
    }
    else if (midi == 16) {
      fm_freq = (data << 4) + 8;
      midi = 8;
    }
    else if (midi == 17) {
      if (control == 0) decay1 = data;
      else if (control == 1) mask2 = 0xff - data;
      else if (control == 2) mask3 = 0xff - data;
      else mask4 = 0xff - data;
      midi = 8;
    }
    else if (midi == 20) midi = 8; // running status reset
  }
}

ISR(TIMER2_OVF_vect) {
  OCR2A = out_temp1;
  OCR2B = out_temp2;
  OCR0A = out_temp3;
  OCR0B = out_temp4;
  if (ramp1 & 0x01) {
    char temp3 = rando;
    rando = (rando >> 1) ^ (-(rando & 0x0001) & 0xb400);
    char temp4 = filt[((filt_ptr - filt_freq_temp) & 0x03ff)];

  int temp1;
  byte temp2;
  asm volatile ( \
    "mov %2, %3 \n\t"\
    "mulsu %4, %2 \n\t"\
    "movw %0, r0 \n\t"\
    "com %2 \n\t"\
    "mulsu %1, %2 \n\t"\
    "sub %A0, r0 \n\t"\
    "sbc %B0, r1 \n\t"\
    "clr r1 \n\t"\
    : \
    "=&r" (temp1) \
    : \
    "r" (temp4), \
    "r" (temp2), \
    "r" (feedback),\
    "r" (temp3)\
    :\
    );
  temp3 = temp1 >> 8;
    
    filt_ptr = (filt_ptr + 1) & 0x03ff;
//    temp3 = (temp3 >> 1) - (temp4 >> 1);
    filt[filt_ptr] = temp3;
    asm volatile ( \
      "mul %1, %2 \n\t"\
      "mov %0, r1 \n\t"\
      "clr r1 \n\t"\
      : \
      "=&r" (temp3) \
      : \
      "r" (temp3), \
      "r" (amp1) \
      );
    out_temp1 = temp3 & mask1;
  }
  else out_temp1 = 0;
  if (ramp2 & 0x01) {
    signal2 += frequency2;
    byte temp2 = pgm_read_byte_near(cymbal + (signal2 >> 3));
    asm volatile ( \
      "mul %1, %2 \n\t"\
      "mov %0, r1 \n\t"\
      "clr r1 \n\t"\
      : \
      "=&r" (temp2) \
      : \
      "r" (temp2), \
      "r" (amp2) \
      );
    out_temp2 = temp2 & mask2;
  }
  else out_temp2 = 0;
  if (ramp3 & 0x01) {
    signal3 += frequency3;
    byte temp2 = pgm_read_byte_near(sinewave + (signal3 >> 6));
    asm volatile ( \
      "mul %1, %2 \n\t"\
      "mov %0, r1 \n\t"\
      "clr r1 \n\t"\
      : \
      "=&r" (temp2) \
      : \
      "r" (temp2), \
      "r" (amp3) \
      );
    out_temp3 = temp2 & mask3;
  }
  else out_temp3 = 0;
  if (ramp4 & 0x01) {
    signal4 += frequency4;
    byte temp2 = pgm_read_byte_near(sinewave + (signal4 >> 6));
    asm volatile ( \
      "mul %1, %2 \n\t"\
      "mov %0, r1 \n\t"\
      "clr r1 \n\t"\
      : \
      "=&r" (temp2) \
      : \
      "r" (temp2), \
      "r" (amp4) \
      );
    out_temp4 = temp2 & mask4;
  }
  else out_temp4 = 0;
  fm_count++;
  if (fm_count == fm_freq) {
    fm_count = 0;
    if (trig_fm) {
      if (fm_mod1) filt_freq_temp += fm_mod1;
      if (fm_mod3) frequency3 += fm_mod3;
      if (fm_mod4) frequency4 += fm_mod4;
      trig_fm = 0;
    }
    else {
      if (fm_mod1) filt_freq_temp -= fm_mod1;
      if (fm_mod3) frequency3 -= fm_mod3;
      if (fm_mod4) frequency4 -= fm_mod4;
      trig_fm = 1;
    }
  }
}
 
ISR(TIMER1_COMPA_vect) {
  if (ramp1 == 1) {
    amp1 += attack1;
    if (amp1 > 0xff - attack1) {
      amp1 = 0xff;
      ramp1 = 3;
    }
  }
  if (ramp2 == 1) {
    amp2 += attack2;
    if (amp2 > 0xff - attack2) {
      ramp2 = 3;
      amp2 = 0xff;
    }
  }
  if (ramp3 == 1) {
    amp3 += attack3;
    if (amp3 > 0xff - attack3) {
      ramp3 = 3;
      amp3 = 0xff;
    }
  }
  if (ramp4 == 1) {
    amp4 += attack4;
    if (amp4 > 0xff - attack4) {
      ramp4 = 3;
      amp4 = 0xff;
    }
  }
  timer = (timer + 1) & 0x07;
  if (timer == 0) { // do decays
    if (ramp1 == 3) {
      amp1 -= decay1;
      filt_freq_temp += sweep1;
      if (amp1 < decay1) {
        ramp1 = 0;
        amp1 = 0;
      }
    }
    if (ramp2 == 3) {
      amp2 -= decay2;
      frequency2 -= sweep2;
      if (amp2 < decay2) {
        ramp2 = 0;
        amp2 = 0;
      }
    }
    if (ramp3 == 3) {
      amp3 -= decay3;
      frequency3 -= sweep3;
      if (amp3 < decay3) {
        ramp3 = 0;
        amp3 = 0;
      }
    }
    if (ramp4 == 3) {
      amp4 -= decay4;
      frequency3 -= sweep4;
      if (amp4 < decay4) {
        amp4 = 0;
        ramp4 = 0;
      }
    }
  }
}

ISR(TIMER1_OVF_vect) {
  if (trig_fm) {
    if (fm_mod1) frequency1 += fm_mod1;
    if (fm_mod2) frequency2 += fm_mod2;
    if (fm_mod3) frequency3 += fm_mod3;
    if (fm_mod4) frequency4 += fm_mod4;
    trig_fm = 0;
  }
  else {
    if (fm_mod1) frequency1 -= fm_mod1;
    if (fm_mod2) frequency2 -= fm_mod2;
    if (fm_mod3) frequency3 -= fm_mod3;
    if (fm_mod4) frequency4 -= fm_mod4;
    trig_fm = 1;
  }
}

ISR_ALIAS(TIMER1_COMPB_vect, TIMER1_COMPA_vect);

ISR_ALIAS(TIMER1_CAPT_vect, TIMER1_COMPA_vect);