//*****************************************************
// A/D convert -> Auto-Correlation then LPF ON/OFF -> Results on LCD
// 7.37MHz Internal RC oscillator, 16x PLL enabled
// Fcy=7.37MHzx16/4=29.48MHz, Tcy=33.92ns
// Toshio Iwata at DIGITALFILTER.COM all rights reserved
// Last modified on 2011/08/05
//*****************************************************

#include <p30f2012.h>
#include <adc12.h>
#include <timer.h>
#include <dsp.h>
#include "xlcd.h"
#include <stdio.h>
#include <outcompare.h>

#define checkCorr 28800
int		writePoint;
int 	bt_th, lt_th;

// configuration
_FWDT(WDT_OFF);
_FGS(CODE_PROT_OFF);
_FOSC(CSW_FSCM_OFF & FRC_PLL16);
_FBORPOR(PBOR_OFF & PWRT_64 & MCLR_EN);

// A/D control register
unsigned int _ADCON1;
unsigned int _ADCON2;
unsigned int _ADCON3;
unsigned int _ADCHS;
unsigned int _ADPCFG;
unsigned int _ADCSSL;

// PWM control register
unsigned int _PTCON;
unsigned int _PWMCON1;
unsigned int _PWMCON2;

// data
unsigned int resultData;
int indata, outdata;
int ACR[16];

char msg1[]={"XXXXXXXXX\0"};
unsigned char dotpat[8];
int	lcdData[16];

int FilType;
int DL[71];
int Coeff[71];

void CoeffInitLpf() { // FIR型LPFの係数を代入（2kHz以下を通すフィルタ）
//Generated by DSPLinks
//http://digitalfilter.com/products/dsplinks/jpdsplinks.html
//U_8
//Remez Algorithm LPF
//Sampling Frequency =    16000.0
//cutoff1 = 2100.0000000
//cutoff2 = 2900.0000000
//Tap Count = 71
//attenuate =   -80.00
//ripple factor =    0.1000000
//Quantized by 16 [bits]
Coeff[	0	] = 	-2	;
Coeff[	1	] = 	-15	;
Coeff[	2	] = 	-33	;
Coeff[	3	] = 	-46	;
Coeff[	4	] = 	-35	;
Coeff[	5	] = 	3	;
Coeff[	6	] = 	51	;
Coeff[	7	] = 	67	;
Coeff[	8	] = 	25	;
Coeff[	9	] = 	-57	;
Coeff[	10	] = 	-116	;
Coeff[	11	] = 	-84	;
Coeff[	12	] = 	39	;
Coeff[	13	] = 	167	;
Coeff[	14	] = 	176	;
Coeff[	15	] = 	19	;
Coeff[	16	] = 	-204	;
Coeff[	17	] = 	-301	;
Coeff[	18	] = 	-138	;
Coeff[	19	] = 	204	;
Coeff[	20	] = 	450	;
Coeff[	21	] = 	336	;
Coeff[	22	] = 	-133	;
Coeff[	23	] = 	-611	;
Coeff[	24	] = 	-642	;
Coeff[	25	] = 	-60	;
Coeff[	26	] = 	766	;
Coeff[	27	] = 	1119	;
Coeff[	28	] = 	487	;
Coeff[	29	] = 	-895	;
Coeff[	30	] = 	-1990	;
Coeff[	31	] = 	-1550	;
Coeff[	32	] = 	980	;
Coeff[	33	] = 	4897	;
Coeff[	34	] = 	8470	;
Coeff[	35	] = 	9912	;
Coeff[	36	] = 	8470	;
Coeff[	37	] = 	4897	;
Coeff[	38	] = 	980	;
Coeff[	39	] = 	-1550	;
Coeff[	40	] = 	-1990	;
Coeff[	41	] = 	-895	;
Coeff[	42	] = 	487	;
Coeff[	43	] = 	1119	;
Coeff[	44	] = 	766	;
Coeff[	45	] = 	-60	;
Coeff[	46	] = 	-642	;
Coeff[	47	] = 	-611	;
Coeff[	48	] = 	-133	;
Coeff[	49	] = 	336	;
Coeff[	50	] = 	450	;
Coeff[	51	] = 	204	;
Coeff[	52	] = 	-138	;
Coeff[	53	] = 	-301	;
Coeff[	54	] = 	-204	;
Coeff[	55	] = 	19	;
Coeff[	56	] = 	176	;
Coeff[	57	] = 	167	;
Coeff[	58	] = 	39	;
Coeff[	59	] = 	-84	;
Coeff[	60	] = 	-116	;
Coeff[	61	] = 	-57	;
Coeff[	62	] = 	25	;
Coeff[	63	] = 	67	;
Coeff[	64	] = 	51	;
Coeff[	65	] = 	3	;
Coeff[	66	] = 	-35	;
Coeff[	67	] = 	-46	;
Coeff[	68	] = 	-33	;
Coeff[	69	] = 	-15	;
Coeff[	70	] = 	-2	;
}

void CoeffInitThru() { // フラットな特性の係数を代入
	int i, m;
	for (i = 0; i < 71; i++) Coeff[i] = 0;
	m = 43;				// 遅延量は m/16kHz, m=43 -> 2.69ms
	Coeff[m] = 32767;	// 係数は1個だけ +1.0 -> 0x7FFF -> 32767
}

void PushDelay(int din) { // 遅延器のプッシュ
	int tmpDL[71];
	VectorCopy(71-1, tmpDL+1, DL); // 配列を一個ずらしてコピー（DSPライブラリ使用）
	tmpDL[0] = din; // 新しいデータを入れる
	VectorCopy(71, DL, tmpDL); // グローバル配列にコピー（DSPライブラリ使用）
}

int CalOutWave() { // デジタルフィルタの出力を計算
	int tmp;
	tmp = VectorDotProduct(71, DL, Coeff); // データと係数積和演算（DSPライブラリ使用）
	return tmp;
}

// Timer subroutine
void __attribute__((__interrupt__, __shadow__, no_auto_psv)) _AltT3Interrupt(void)
{
	int minval, minindex;
	int maxval, maxindex;
	int n;
	int val;
	int topbar[16];
	int btmbar[16];
	// --- A/D is 12bits, filtering is 12bits
	while(!IFS0bits.ADIF);	// wait ADC
	resultData = ReadADC12(0);	// 12bit resolution (16kHz sampling)

	indata = resultData - 2048;			// 符号付きにする
	PushDelay(indata);				// 遅延器のプッシュ
	outdata = CalOutWave();			// デジタルフィルタの出力を計算
	outdata += 2048;				// 符号なしにする
	if(outdata > 4095) outdata = 4095;		// オーバフローリミッタ
	if(outdata < 0) outdata = 0;		// アンダーフローリミッタ
	resultData = outdata >> 2;			// 10ビットにする

	if ( writePoint == checkCorr-1 ) { // 1秒に1回だけ自己相関値を計算する

		for(n = 0; n < 16; n++) { // ACR[0]からACR[15]まで16個の自己相関値を計算
			ACR[n] = VectorDotProduct(32, DL, DL+n);
		}

		minval = VectorMin(16, ACR, &minindex); // 最小値が0になるようにｵﾌｾｯﾄをかける
		for(n = 0; n < 16; n++) {
			ACR[n] -= minval;
		}

		maxval = VectorMax(8, ACR+8, &maxindex); // ACR[8]からACR[15]のうちの最大値
		if(maxval > 20) { // 最大値が20を越えた回数をカウント
			bt_th += 1;
			lt_th = 0;
		} else {
			bt_th = 0;
			lt_th += 1;
		}
		if(bt_th == 3) { // 3回連続で越えたらLPFを選択する
			bt_th = 0;
			FilType = 1; // LPF
			CoeffInitLpf();
			PORTCbits.RC13 = 1;	// LED緑消灯
		} 
		if(lt_th == 3) { // 3回連続で下回ったらスルーを選択
			lt_th = 0;
			FilType = 0; // Through
			CoeffInitThru();
			PORTCbits.RC13 = 0;	// LED緑点灯
		}

		for(n = 0; n < 16; n++) { // ACR[0]からACR[15]までLCDに表示する
			val = ACR[n] / 10;
			if(val > 16) val = 16;
			if(val == 0) { topbar[n] = 32; btmbar[n] =0+8; }
			else if(val == 1) { topbar[n] = 32; btmbar[n] = 1+8; }
			else if(val == 2) { topbar[n] = 32; btmbar[n] = 2+8; }
			else if(val == 3) { topbar[n] = 32; btmbar[n] = 3+8; }
			else if(val == 4) { topbar[n] = 32; btmbar[n] = 4+8; }
			else if(val == 5) { topbar[n] = 32; btmbar[n] = 5+8; }
			else if(val == 6) { topbar[n] = 32; btmbar[n] = 6+8; }
			else if(val == 7) { topbar[n] = 32; btmbar[n] = 7+8; }
			else if(val == 8) { topbar[n] = 0+8; btmbar[n] = 7+8; }
			else if(val == 9) { topbar[n] = 1+8; btmbar[n] = 7+8; }
			else if(val == 10) { topbar[n] = 2+8; btmbar[n] = 7+8; }
			else if(val == 11) { topbar[n] = 3+8; btmbar[n] = 7+8; }
			else if(val == 12) { topbar[n] = 4+8; btmbar[n] = 7+8; }
			else if(val == 13) { topbar[n] = 5+8; btmbar[n] = 7+8; }
			else if(val == 14) { topbar[n] = 6+8; btmbar[n] = 7+8; }
			else { topbar[n] = 7+8; btmbar[n] = 7+8; }
		}

		// 棒グラフキャラクタを16個表示
		sprintf(msg1,"%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
			topbar[0], topbar[1], topbar[2], topbar[3], topbar[4], topbar[5], topbar[6], topbar[7],
			topbar[8], topbar[9], topbar[10], topbar[11], topbar[12], topbar[13], topbar[14], topbar[15]);
		SetDDRamAddr(0x00);
		PutsXLCD(msg1);
		sprintf(msg1,"%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
			btmbar[0], btmbar[1], btmbar[2], btmbar[3], btmbar[4], btmbar[5], btmbar[6], btmbar[7],
			btmbar[8], btmbar[9], btmbar[10], btmbar[11], btmbar[12], btmbar[13], btmbar[14], btmbar[15]);
		SetDDRamAddr(0x40);
		PutsXLCD(msg1);
	}

	writePoint++;
	if ( writePoint == checkCorr )  {
		writePoint = 0;
	}

	SetDCOC2PWM(resultData); // 処理後の音声をPWM出力
	IFS0bits.ADIF = 0;	// clear A/D interuppt flag
	IFS0bits.T3IF = 0;	// clear T3 interuppt flag
}

// Main routine
int main(void)
{
	// port init
	TRISB=0x01FF;

	TRISCbits.TRISC13 = 0;				// LED出力
	PORTCbits.RC13 = 0;					// LED緑点灯
	ADPCFG = 0x03f5; // for in-circuit programmer

	// LCD init
	OpenXLCD(FOUR_BIT & TWO_LINE & SEG1_50_SEG51_100 & COM1_COM16);
	// turn on
	WriteCmdXLCD(DON & CURSOR_OFF & BLINK_OFF);

	SetDDRamAddr(0x00);

	// 棒グラフキャラクタの作成(8個)
	dotpat[0] = 0x00;
	dotpat[1] = 0x00;
	dotpat[2] = 0x00;
	dotpat[3] = 0x00;
	dotpat[4] = 0x00;
	dotpat[5] = 0x00;
	dotpat[6] = 0x00;
	dotpat[7] = 0x1F;
	lcd_putram(0, dotpat);//code=0, CGRAM data set

	dotpat[0] = 0x00;
	dotpat[1] = 0x00;
	dotpat[2] = 0x00;
	dotpat[3] = 0x00;
	dotpat[4] = 0x00;
	dotpat[5] = 0x00;
	dotpat[6] = 0x1F;
	dotpat[7] = 0x1F;
	lcd_putram(1, dotpat);//code=1, CGRAM data set

	dotpat[0] = 0x00;
	dotpat[1] = 0x00;
	dotpat[2] = 0x00;
	dotpat[3] = 0x00;
	dotpat[4] = 0x00;
	dotpat[5] = 0x1F;
	dotpat[6] = 0x1F;
	dotpat[7] = 0x1F;
	lcd_putram(2, dotpat);//code=2, CGRAM data set

	dotpat[0] = 0x00;
	dotpat[1] = 0x00;
	dotpat[2] = 0x00;
	dotpat[3] = 0x00;
	dotpat[4] = 0x1F;
	dotpat[5] = 0x1F;
	dotpat[6] = 0x1F;
	dotpat[7] = 0x1F;
	lcd_putram(3, dotpat);//code=3, CGRAM data set

	dotpat[0] = 0x00;
	dotpat[1] = 0x00;
	dotpat[2] = 0x00;
	dotpat[3] = 0x1F;
	dotpat[4] = 0x1F;
	dotpat[5] = 0x1F;
	dotpat[6] = 0x1F;
	dotpat[7] = 0x1F;
	lcd_putram(4, dotpat);//code=4, CGRAM data set

	dotpat[0] = 0x00;
	dotpat[1] = 0x00;
	dotpat[2] = 0x1F;
	dotpat[3] = 0x1F;
	dotpat[4] = 0x1F;
	dotpat[5] = 0x1F;
	dotpat[6] = 0x1F;
	dotpat[7] = 0x1F;
	lcd_putram(5, dotpat);//code=5, CGRAM data set

	dotpat[0] = 0x00;
	dotpat[1] = 0x1F;
	dotpat[2] = 0x1F;
	dotpat[3] = 0x1F;
	dotpat[4] = 0x1F;
	dotpat[5] = 0x1F;
	dotpat[6] = 0x1F;
	dotpat[7] = 0x1F;
	lcd_putram(6, dotpat);//code=6, CGRAM data set

	dotpat[0] = 0x1F;
	dotpat[1] = 0x1F;
	dotpat[2] = 0x1F;
	dotpat[3] = 0x1F;
	dotpat[4] = 0x1F;
	dotpat[5] = 0x1F;
	dotpat[6] = 0x1F;
	dotpat[7] = 0x1F;
	lcd_putram(7, dotpat);//code=7, CGRAM data set

	// Use Alternate Interrupt Vector Table
	INTCON2bits.ALTIVT=1;
	// Confirm to turn off ADC
	ADCON1bits.ADON=0;
	// ADC init
	_ADCHS=	ADC_CH0_POS_SAMPLEA_AN3 &
			ADC_CH0_NEG_SAMPLEA_NVREF;		// A-D channel-3 select
	SetChanADC12(_ADCHS);
	_ADCON1=ADC_MODULE_ON &
			ADC_IDLE_CONTINUE &
			ADC_FORMAT_INTG &
			ADC_CLK_TMR &
			ADC_AUTO_SAMPLING_ON &
			ADC_SAMP_OFF;
	_ADCON2=ADC_VREF_AVDD_AVSS &
			ADC_SCAN_OFF &
			ADC_SAMPLES_PER_INT_1 &
			ADC_ALT_BUF_OFF &
			ADC_ALT_INPUT_OFF;
	// Tad={Tcy(ADCS+1)}/2>334ns, Then ADCS>18.7, Tad=10*Tcy
	_ADCON3=ADC_SAMPLE_TIME_1 &
			ADC_CONV_CLK_SYSTEM &
			ADC_CONV_CLK_10Tcy;
	_ADPCFG=ENABLE_AN3_ANA;	// A-D channel-3 analog input
	_ADCSSL=SCAN_NONE;
	OpenADC12(_ADCON1, _ADCON2, _ADCON3, _ADPCFG, _ADCSSL);

	// Timer3 setting
	OpenTimer3(T3_ON & T3_GATE_OFF & T3_PS_1_1 & T3_SOURCE_INT, 1843-1);	// 16kHz sampling ADC
	ConfigIntTimer3(T3_INT_PRIOR_5 & T3_INT_ON);

	// Timer2 setting 
	OpenTimer2(T2_ON & T2_GATE_OFF & T2_PS_1_1 & T2_SOURCE_INT, 1843-1);	// 16kHz carrier PWM
	OpenOC2(OC_IDLE_CON & OC_TIMER2_SRC &
		OC_PWM_FAULT_PIN_DISABLE, 0, 0);

	// enable interrupt
	DISICNT = 0x0000;

	CoeffInitThru(); // 最初はフラットな特性
	writePoint = 0;
	FilType = 0;
	bt_th = 0;
	lt_th = 0;

	// main loop
	while(1)
	{
		// A/D result store in resultData
	}
}