#include "p33FJ128GP802.h"
/*
pwm out - rb5
s1 - an0/ra0
s2 - an4/rb2
s3 - an5/rb3
*/
#define FCY		(40000000UL)/1
#define	INT_PERIOD		25
//#define	INT_PERIOD_65k	40*65536

#define	S6_0	LATAbits.LATA0
#define	S6_T	TRISAbits.TRISA0
#define	S6_A	0
#define	S5_0	LATAbits.LATA1
#define	S5_T	TRISAbits.TRISA1
#define	S5_A	1
#define	S4_0	LATBbits.LATB0
#define	S4_T	TRISBbits.TRISB0
#define	S4_A	2
#define	S3_0	LATBbits.LATB1
#define	S3_T	TRISBbits.TRISB1
#define	S3_A	3
#define	S2_0	LATBbits.LATB2
#define	S2_T	TRISBbits.TRISB2
#define	S2_A	4
#define	S1_0	LATBbits.LATB3
#define	S1_T	TRISBbits.TRISB3
#define	S1_A	5

#define	C1_O	LATAbits.LATA2
#define	C1_T	TRISAbits.TRISA2
#define	C2_O	LATAbits.LATA3
#define	C2_T	TRISAbits.TRISA3
#define	C3_O	LATBbits.LATB4
#define	C3_T	TRISBbits.TRISB4
#define	C4_O	LATAbits.LATA4
#define	C4_T	TRISAbits.TRISA4
#define	C5_O	LATBbits.LATB5
#define	C5_T	TRISBbits.TRISB5
#define	C6_O	LATBbits.LATB6
#define	C6_T	TRISBbits.TRISB6

#define	LED1_O	LATBbits.LATB15
#define	LED1_T	TRISBbits.TRISB15
#define	LED2_O	LATBbits.LATB14
#define	LED2_T	TRISBbits.TRISB14
#define	LED3_O	LATBbits.LATB13
#define	LED3_T	TRISBbits.TRISB13



unsigned int counter1, counter2;
unsigned int add1,add2;
volatile float freq,tim,temp;
volatile int test;
unsigned int adcres1,adcres2,adcres3;

typedef struct _VOICE {
	unsigned int add;	
	unsigned long counter;
	unsigned char volume;
	int	sample;
	unsigned char key;
} VOICE;

unsigned int calculate_add (float freq);
void delay_ms(unsigned int number);
void delay_cyc (unsigned int number);
inline void set_io_pin(unsigned char lat_tris, unsigned char pin, unsigned char state);
inline void set_io_comm (unsigned char lat_tris, unsigned char pin, unsigned char state);
inline void set_adc (unsigned char pin);
unsigned int get_state (unsigned char pin, unsigned char comm);
unsigned char search_assigned_key (unsigned char key);
unsigned char search_free_generator (void);


#define	MAX_VOICES	8UL
#define	PWM_RES		512
#define	PWM_DIV		(512*MAX_VOICES)/PWM_RES

volatile VOICE	voices[MAX_VOICES];

unsigned char keys[36];
unsigned char keys_x,keys_y;
unsigned int key_temp;

#define		SHIFT_FACTOR	10
#define		MODULUS_FACTOR	0x0007FFFF

#define c1 523.251130601197
#define cis1 554.365261953744
#define d1 587.329535834815
#define dis1 622.253967444162
#define e1 659.25511382574
#define f1 698.456462866008
#define fis1 739.988845423269
#define g1 783.990871963499
#define gis1 830.609395159891
#define a1 880
#define ais1 932.32752303618
#define b1 987.766602512249
#define c2 1046.50226120239
#define cis2 1108.73052390749
#define d2 1174.65907166963
#define dis2 1244.50793488832
#define e2 1318.51022765148
#define f2 1396.91292573202
#define fis2 1479.97769084654
#define g2 1567.981743927
#define gis2 1661.21879031978
#define a2 1760
#define ais2 1864.65504607236
#define b2 1975.5332050245
#define c3 2093.00452240479
#define cis3 2217.46104781498
#define d3 2349.31814333926
#define dis3 2489.01586977665
#define e3 2637.02045530296
#define f3 2793.82585146403
#define fis3 2959.95538169308
#define g3 3135.963487854
#define gis3 3322.43758063956
#define a3 3520
#define ais3 3729.31009214472
#define b3 3951.066410049

float key_array[36] = 
		{ 
			c1,cis1,d1,dis1,e1,f1,fis1,g1,gis1,a1,ais1,b1,
			c2,cis2,d2,dis2,e2,f2,fis2,g2,gis2,a2,ais2,b2,
			c3,cis3,d3,dis3,e3,f3,fis3,g3,gis3,a3,ais3,b3
		};


const int sintable[512] = 
{
0, 		3, 		6, 		9, 		13, 	16, 	19, 	22, 	25, 	28, 	31, 	34, 	37, 	41, 	44, 	47, 
50, 	53, 	56, 	59, 	62, 	65, 	68, 	71, 	74, 	77, 	80, 	83, 	86, 	89, 	92, 	95, 
98, 	100, 	103, 	106, 	109, 	112, 	115, 	117, 	120, 	123, 	126, 	128, 	131, 	134, 	136, 	139, 
142, 	144, 	147, 	149, 	152, 	154, 	157, 	159, 	162, 	164, 	167, 	169, 	171, 	174, 	176, 	178, 
180, 	182, 	185, 	187, 	189, 	191, 	193, 	195, 	197, 	199, 	201, 	203, 	205, 	207, 	208, 	210, 
212, 	214, 	215, 	217, 	219, 	220, 	222, 	223, 	225, 	226, 	228, 	229, 	230, 	232, 	233, 	234, 
236, 	237, 	238, 	239, 	240, 	241, 	242, 	243, 	244, 	245, 	246, 	247, 	247, 	248, 	249, 	249, 
250, 	251, 	251, 	252, 	252, 	253, 	253, 	253, 	254, 	254, 	254, 	255, 	255, 	255, 	255, 	255, 
255, 	255, 	255, 	255, 	255, 	255, 	254, 	254, 	254, 	253, 	253, 	253, 	252, 	252, 	251, 	251, 
250, 	249, 	249, 	248, 	247, 	247, 	246, 	245, 	244, 	243, 	242, 	241, 	240, 	239, 	238, 	237, 
236, 	234, 	233, 	232, 	231, 	229, 	228, 	226, 	225, 	223, 	222, 	220, 	219, 	217, 	215, 	214, 
212, 	210, 	209, 	207, 	205, 	203, 	201, 	199, 	197, 	195, 	193, 	191, 	189, 	187, 	185, 	183, 
180, 	178, 	176, 	174, 	171, 	169, 	167, 	164, 	162, 	159, 	157, 	155, 	152, 	149, 	147, 	144, 
142, 	139, 	137, 	134, 	131, 	129, 	126, 	123, 	120, 	118, 	115, 	112, 	109, 	106, 	103, 	101, 
98, 	95, 	92, 	89, 	86, 	83, 	80, 	77, 	74, 	71, 	68, 	65, 	62, 	59, 	56, 	53, 
50, 	47, 	44, 	41, 	38, 	34, 	31, 	28, 	25, 	22, 	19, 	16, 	13, 	10, 	6, 		3, 
0, 		-3, 	-6, 	-9, 	-12, 	-15, 	-19, 	-22, 	-25, 	-28, 	-31, 	-34, 	-37, 	-40, 	-43, 	-47, 
-50, 	-53, 	-56, 	-59, 	-62, 	-65, 	-68, 	-71, 	-74, 	-77, 	-80, 	-83, 	-86, 	-89, 	-92, 	-95, 
-97, 	-100, 	-103, 	-106, 	-109, 	-112, 	-114, 	-117, 	-120, 	-123, 	-126, 	-128, 	-131, 	-134, 	-136, 	-139, 
-142, 	-144, 	-147, 	-149, 	-152, 	-154, 	-157, 	-159, 	-162, 	-164, 	-166, 	-169, 	-171, 	-173, 	-176, 	-178, 
-180, 	-182, 	-185, 	-187, 	-189, 	-191, 	-193, 	-195, 	-197, 	-199, 	-201, 	-203, 	-205, 	-207, 	-208, 	-210, 
-212, 	-214, 	-215, 	-217, 	-219, 	-220, 	-222, 	-223, 	-225, 	-226, 	-228, 	-229, 	-230, 	-232, 	-233, 	-234, 
-236, 	-237, 	-238, 	-239, 	-240, 	-241, 	-242, 	-243, 	-244, 	-245, 	-246, 	-247, 	-247, 	-248, 	-249, 	-249, 
-250, 	-251, 	-251, 	-252, 	-252, 	-253, 	-253, 	-253, 	-254, 	-254, 	-254, 	-255, 	-255, 	-255, 	-255, 	-255, 
-255, 	-255, 	-255, 	-255, 	-255, 	-255, 	-254, 	-254, 	-254, 	-253, 	-253, 	-253, 	-252, 	-252, 	-251, 	-251, 
-250, 	-250, 	-249, 	-248, 	-247, 	-247, 	-246, 	-245, 	-244, 	-243, 	-242, 	-241, 	-240, 	-239, 	-238, 	-237, 
-236, 	-234, 	-233, 	-232, 	-231, 	-229, 	-228, 	-226, 	-225, 	-224, 	-222, 	-220, 	-219, 	-217, 	-216, 	-214, 
-212, 	-210, 	-209, 	-207, 	-205, 	-203, 	-201, 	-199, 	-197, 	-195, 	-193, 	-191, 	-189, 	-187, 	-185, 	-183, 
-180, 	-178, 	-176, 	-174, 	-171, 	-169, 	-167, 	-164, 	-162, 	-160, 	-157, 	-155, 	-152, 	-150, 	-147, 	-144, 
-142, 	-139, 	-137, 	-134, 	-131, 	-129, 	-126, 	-123, 	-120, 	-118, 	-115, 	-112, 	-109, 	-106, 	-104, 	-101, 
-98, 	-95, 	-92, 	-89, 	-86, 	-83, 	-80, 	-77, 	-74, 	-71, 	-68, 	-65, 	-62, 	-59, 	-56, 	-53, 
-50, 	-47, 	-44, 	-41, 	-38, 	-35, 	-32, 	-28, 	-25, 	-22, 	-19, 	-16, 	-13, 	-10, 	-7, 	-3, 

};

void __attribute__((interrupt, auto_psv)) _T1Interrupt(void)
{
unsigned char i;
volatile int sum;
LED1_O = 1;
IFS0bits.T1IF = 0;
sum = 0;
for (i=0;i<MAX_VOICES;i++)
	{
	voices[i].counter = (voices[i].counter + voices[i].add) & MODULUS_FACTOR;
	voices[i].sample =  (voices[i].volume*sintable[ voices[i].counter>>SHIFT_FACTOR])/100;
	sum = sum + voices[i].sample;
	}
	OC1RS = (PWM_RES/2) + (sum/8);
LED1_O = 0;
}


void __attribute__((interrupt, auto_psv)) _T3Interrupt(void)
{
IFS0bits.T3IF = 0;

}


void main (void)
{
test = PWM_DIV;


// Configure PLL prescaler, PLL postscaler, PLL divisor
PLLFBD=150;
CLKDIVbits.PLLPOST=0b00; // 
CLKDIVbits.PLLPRE=5; // 
// Initiate Clock Switch to Primary Oscillator with PLL (NOSC=0b011)
// Wait for Clock switch to occur

__builtin_write_OSCCONL(OSCCON & 0xbf); //clear the bit 6 of OSCCONL to

										//unlock Pin Re-map
RPOR4bits.RP9R = 0b10010;

__builtin_write_OSCCONL(OSCCON | 0x40); //set the bit 6 of OSCCONL to

OC1CONbits.OCM = 0b000; // Disable Output Compare Module
OC1R = 100; // Write the duty cycle for the first PWM pulse
OC1RS = 200; // Write the duty cycle for the second PWM pulse
OC1CONbits.OCTSEL = 0; // Select Timer 2 as output compare time base
OC1R = 100; // Load the Compare Register Value
OC1CONbits.OCM = 0b110; // Select the Output Compare mode

// Initialize and enable Timer2
T2CONbits.TON = 0; // Disable Timer
T2CONbits.TCS = 0; // Select internal instruction cycle clock
T2CONbits.TGATE = 0; // Disable Gated Timer mode
T2CONbits.TCKPS = 0b00; // Select Prescaler
TMR2 = 0x00; // Clear timer register
PR2 = PWM_RES; // Load the period value
T2CONbits.TON = 1; // Start Timer

AD1CHS0 = 0x00;
AD1CSSL=0;
AD1CON3 = 0x9F00;
AD1CON2 = 0x0000;
AD1CON1bits.ADON = 1;

PR1 = (unsigned int)((FCY / 1 / 1000000)*INT_PERIOD);
T1CONbits.TCS = 0;
T1CONbits.TCKPS = 0b00;
T1CONbits.TON = 1;
IEC0bits.T1IE = 1;

PR3 = (unsigned int)((FCY / 256 / 1000)*500);
T3CONbits.TCS = 0;
T3CONbits.TCKPS = 0b11;
T3CONbits.TON = 1;
IEC0bits.T3IE = 1;

LED1_T=0;
LED2_T=0;
LED3_T=0;

test = 0;
for (keys_x=0;keys_x<MAX_VOICES;keys_x++)
	{
	voices[keys_x].key = 255;
	voices[keys_x].volume = 0;
	}
while (1)
{
for (keys_x=0;keys_x<6;keys_x++)
	{
	for (keys_y=0;keys_y<6;keys_y++)
		{
		key_temp = get_state(keys_x+1,keys_y+1);
		if (key_temp<350)
				keys[keys_y+(keys_x*6)] = key_temp/2;
		else 	keys[keys_y+(keys_x*6)] = 0;
		}
	}

for (keys_x=0;keys_x<36;keys_x++)
	{
	//scan for all keys and check, whether there is generator assigned for key
	keys_y = search_assigned_key(keys_x);
	//if key is active
	if (keys[keys_x]>0)
		{
		//and if generator is not assigned, then it is freshly pressed key
		if (keys_y==255)
			{
			//assign key to first free generator
			keys_y = search_free_generator();
			voices[keys_y].add = calculate_add(key_array[keys_x]/2.0);
			voices[keys_y].volume = 100;
			voices[keys_y].key = keys_x;
			}
		}
	else
		{
		if (keys_y!=255)
			{
			//key released
			//unassign key from generator		
			voices[keys_y].volume = 0;
			voices[keys_y].key = 255;
			}
		}
	}
keys_y = 0;
for (keys_x=0;keys_x<36;keys_x++)
	{
	if (keys[keys_x]!=0) keys_y=1;
	}
if (keys_y==1) LED2_O = 1;
	else	LED2_O = 0;
delay_ms(10);
LED3_O = ~ LED3_O;
}

}

unsigned char search_assigned_key (unsigned char key)
{
unsigned char i;
for (i=0;i<MAX_VOICES;i++)
	{
	if (voices[i].key == key) return i;
	}
return 255;
}

unsigned char search_free_generator (void)
{
unsigned char i;
for (i=0;i<MAX_VOICES;i++)
	{
	if (voices[i].key == 255) return i;
	}
return 255;
}


unsigned int calculate_add (float freq)
{
float temp;
	temp = MODULUS_FACTOR * freq * INT_PERIOD/1000000.0;
	return (temp + 0.5);
}

void delay_ms(unsigned int number)
{
int i;
TMR4 = 0;
PR4 = FCY/1000;
IFS1bits.T4IF = 0;
T4CONbits.TON = 1;
for (i=0;i<number;i++) 
	{
	IFS1bits.T4IF = 0;
	while (IFS1bits.T4IF == 0);
	}
T4CONbits.TON = 0;
}


void delay_cyc (unsigned int number)
{
TMR4 = 0;
PR4 = number;
IFS1bits.T4IF = 0;
T4CONbits.TON = 1;
IFS1bits.T4IF = 0;
while (IFS1bits.T4IF == 0);
T4CONbits.TON = 0;
}



unsigned int get_state (unsigned char pin, unsigned char comm)
{
unsigned int temp;
set_adc(pin);
set_io_comm(1,comm,0);
set_io_comm(0,comm,0);
set_io_pin(1,pin,0);
set_io_pin(0,pin,0);
delay_cyc(50);
set_io_pin(0,pin,1);
IEC0bits.T1IE = 0;
set_io_comm(1,comm,1);
set_io_comm(0,comm,0);
AD1CON1bits.SAMP = 1; // start sampling
delay_cyc(40);
AD1CON1bits.SAMP = 0; // start the conversion
IEC0bits.T1IE = 1;
while (!AD1CON1bits.DONE); // wait for the conversion to complete
temp = ADC1BUF0;
set_io_comm(1,comm,0);
set_io_comm(0,comm,0);
set_io_pin(1,pin,0);
set_io_pin(0,pin,0);
return temp;
}

inline void set_adc (unsigned char pin)
{
if (pin==1) AD1CHS0 = S1_A;
if (pin==2) AD1CHS0 = S2_A;
if (pin==3) AD1CHS0 = S3_A;
if (pin==4) AD1CHS0 = S4_A;
if (pin==5) AD1CHS0 = S5_A;
if (pin==6) AD1CHS0 = S6_A;

}

inline void set_io_pin(unsigned char lat_tris, unsigned char pin, unsigned char state)
{
if (lat_tris==1)
	{
	if (pin==1)	S1_0 = state;
	if (pin==2)	S2_0 = state;
	if (pin==3)	S3_0 = state;
	if (pin==4)	S4_0 = state;
	if (pin==5)	S5_0 = state;
	if (pin==6)	S6_0 = state;

	}
else
	{
	if (pin==1)	S1_T = state;
	if (pin==2)	S2_T = state;
	if (pin==3)	S3_T = state;
	if (pin==4)	S4_T = state;
	if (pin==5)	S5_T = state;
	if (pin==6)	S6_T = state;
	}
}

inline void set_io_comm (unsigned char lat_tris, unsigned char pin, unsigned char state)
{
if (lat_tris==1)
	{
	if (pin==1)	C1_O = state;
	if (pin==2)	C2_O = state;
	if (pin==3)	C3_O = state;
	if (pin==4)	C4_O = state;
	if (pin==5)	C5_O = state;
	if (pin==6)	C6_O = state;

	}
else
	{
	if (pin==1)	C1_T = state;
	if (pin==2)	C2_T = state;
	if (pin==3)	C3_T = state;
	if (pin==4)	C4_T = state;
	if (pin==5)	C5_T = state;
	if (pin==6)	C6_T = state;
	}

}