/*
 * Slave Firmware
 *
 * Wireless Observational Modular Aerial Network
 * 
 * Ethan Zonca
 * Matthew Kanning
 * Kyle Ripperger
 * Matthew Kroening
 *
 */

#include <stdint.h>
#include <avr/io.h>
#include <avr/interrupt.h> 
#include <util/delay.h>
#include <avr/pgmspace.h>


PROGMEM extern const prog_uchar afsk_sine_table[512] = {
  127, 129, 130, 132, 133, 135, 136, 138, 139, 141, 143, 144, 146, 147, 149, 150, 152, 153, 155, 156, 158, 
  159, 161, 163, 164, 166, 167, 168, 170, 171, 173, 174, 176, 177, 179, 180, 182, 183, 184, 186, 187, 188, 
  190, 191, 193, 194, 195, 197, 198, 199, 200, 202, 203, 204, 205, 207, 208, 209, 210, 211, 213, 214, 215, 
  216, 217, 218, 219, 220, 221, 223, 224, 225, 226, 227, 228, 228, 229, 230, 231, 232, 233, 234, 235, 236, 
  236, 237, 238, 239, 239, 240, 241, 242, 242, 243, 244, 244, 245, 245, 246, 247, 247, 248, 248, 249, 249, 
  249, 250, 250, 251, 251, 251, 252, 252, 252, 253, 253, 253, 253, 254, 254, 254, 254, 254, 254, 254, 254, 
  254, 254, 255, 254, 254, 254, 254, 254, 254, 254, 254, 254, 254, 253, 253, 253, 253, 252, 252, 252, 251, 
  251, 251, 250, 250, 249, 249, 249, 248, 248, 247, 247, 246, 245, 245, 244, 244, 243, 242, 242, 241, 240, 
  239, 239, 238, 237, 236, 236, 235, 234, 233, 232, 231, 230, 229, 228, 228, 227, 226, 225, 224, 223, 221, 
  220, 219, 218, 217, 216, 215, 214, 213, 211, 210, 209, 208, 207, 205, 204, 203, 202, 200, 199, 198, 197, 
  195, 194, 193, 191, 190, 188, 187, 186, 184, 183, 182, 180, 179, 177, 176, 174, 173, 171, 170, 168, 167, 
  166, 164, 163, 161, 159, 158, 156, 155, 153, 152, 150, 149, 147, 146, 144, 143, 141, 139, 138, 136, 135, 
  133, 132, 130, 129, 127, 125, 124, 122, 121, 119, 118, 116, 115, 113, 111, 110, 108, 107, 105, 104, 102, 
  101,  99,  98,  96,  95,  93,  91,  90,  88,  87,  86,  84,  83,  81,  80,  78,  77,  75,  74,  72,  71, 
   70,  68,  67,  66,  64,  63,  61,  60,  59,  57,  56,  55,  54,  52,  51,  50,  49,  47,  46,  45,  44, 
   43,  41,  40,  39,  38,  37,  36,  35,  34,  33,  31,  30,  29,  28,  27,  26,  26,  25,  24,  23,  22, 
   21,  20,  19,  18,  18,  17,  16,  15,  15,  14,  13,  12,  12,  11,  10,  10,   9,   9,   8,   7,   7, 
    6,   6,   5,   5,   5,   4,   4,   3,   3,   3,   2,   2,   2,   1,   1,   1,   1,   0,   0,   0,   0, 
    0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   1,   1,   1,   1, 
    2,   2,   2,   3,   3,   3,   4,   4,   5,   5,   5,   6,   6,   7,   7,   8,   9,   9,  10,  10,  11, 
   12,  12,  13,  14,  15,  15,  16,  17,  18,  18,  19,  20,  21,  22,  23,  24,  25,  26,  26,  27,  28, 
   29,  30,  31,  33,  34,  35,  36,  37,  38,  39,  40,  41,  43,  44,  45,  46,  47,  49,  50,  51,  52, 
   54,  55,  56,  57,  59,  60,  61,  63,  64,  66,  67,  68,  70,  71,  72,  74,  75,  77,  78,  80,  81, 
   83,  84,  86,  87,  88,  90,  91,  93,  95,  96,  98,  99, 101, 102, 104, 105, 107, 108, 110, 111, 113, 
  115, 116, 118, 119, 121, 122, 124, 125
};

inline uint8_t afsk_read_sample(int phase)
{
  return pgm_read_byte_near(afsk_sine_table + phase);
}

volatile uint16_t state = 0;
volatile uint8_t up = 1;

int main(void)
{

  // Source timer2 from clkIO (datasheet p.164)
  ASSR &= ~(_BV(EXCLK) | _BV(AS2));

  // Set fast PWM mode with TOP = 0xff: WGM22:0 = 3  (p.150)
  TCCR2A |= _BV(WGM21) | _BV(WGM20);
  TCCR2B &= ~_BV(WGM22);

  // Do non-inverting PWM on pin OC2B (arduino pin 3) (p.159).
  // OC2A (arduino pin 11) stays in normal port operation:
  // COM2B1=1, COM2B0=0, COM2A1=0, COM2A0=0
  TCCR2A = (TCCR2A | _BV(COM2B1)) & ~(_BV(COM2B0) | _BV(COM2A1) | _BV(COM2A0));

  // No prescaler (p.162)
  //TCCR2B = (TCCR2B & ~(_BV(CS22) | _BV(CS21))) | _BV(CS20);
  TCCR2B = CS22 | CS21 |  CS20;
    
  // Enable interrupt when TCNT2 reaches TOP (0xFF) (p.151, 163)
	//TIMSK2 |= _BV(TOIE2);
	TIMSK2 |= 0b00000111;

	
	
	

	// Initialize PORTA
	DDRA = 0xff;
	PORTA = 0x00;
	
	DDRD = 0xff;
	PORTD = 0xff;
	
	sei();

		
	while(1) {
		//PORTA ++;
		/*
		if(state < 512) {
			OCR2B = afsk_sine_table[state];
		}
		else {
			state = 0;
			OCR2B = afsk_sine_table[state];
		}
			*/
		/*
		OCR2B = afsk_sine_table[state];
		

		if(up==1)
			state++;
		else
			state--;

		if(state <= 0)
			up = 1;
		else if(state >= 254)
			up = 0;
			*/
		_delay_us(10);
	}
}


volatile uint8_t meh = 1;

void frick() {
	meh = meh + 5;
	//OCR2B = 0x09;
	OCR2B += meh;// = meh;// = afsk_read_sample(meh);
}

ISR(TIMER2_OVF_vect) {
/*
	PORTA = 0xff;
	_delay_ms(500);
	PORTA = 0x00;
	_delay_ms(500);  */
	frick();
	//meh++;
	//OCR2B = 0x90;
	/*
  // [...] load the next sample
  if(state < 512) {
	OCR2B = afsk_sine_table[state];
	}
	else {
		state = 0;
	}*/


			
	return;
}