#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/time.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <rtl_fifo.h>
#include <rtl_time.h>
#include <mbuff.h>
#include <signal.h>
#include <math.h>

#include "freqresp.h"

#define __STR(f)	#f
#define	STR(f)		__STR(f)

volatile float * shm;
volatile struct my_msg_struct * msg;

extern void realfft(double *, unsigned int, double *);

static void
emergency_stop(int d)
{
        msg->command = STOP_TASK;
	mbuff_free("data",(void*)shm);
	mbuff_free("msg",(void*)msg);

	exit(1);
}

int
main(int argc, char *argv[])
{
	FILE *fp[2]; // fp[0]:SPK2 用, fp[1]:SPK1 用
	int j, l, repeat, count;
	static double u_time[LENGTH_MAX]; // 制御入力の時間波形
	static double y0_time[LENGTH_MAX]; // エラーマイク出力の時間波形
	static double y1_time[LENGTH_MAX]; // リファレンスマイク出力の時間波形
	static double u_fft[LENGTH_MAX], y0_fft[LENGTH_MAX], y1_fft[LENGTH_MAX];
	double amp[2] = {AMP_CH0, AMP_CH1};
	double f, f10;
	double u_a, u_b, y0_a, y0_b, y1_a, y1_b;
	double y0_u_gain, y0_u_phase, y1_u_gain, y1_u_phase;
	float t;
	int div;
	double T, Ts;

	fp[0] = fopen("data/spk1.dat", "w");
	fp[1] = fopen("data/spk2.dat", "w");

	if((shm = (volatile float*) mbuff_alloc("data", BUF_LEN * LENGTH_MAX * sizeof(float))) == NULL){
		fprintf(stderr, "mbuff_alloc failed!!!\n");
		return -1;
	}

	if((msg = (volatile struct my_msg_struct*) mbuff_alloc("msg", sizeof(struct my_msg_struct))) == NULL){
		fprintf(stderr, "mbuff_alloc failed \n");
		return -1;
	}

	msg->command = STOP_TASK;

	signal(SIGINT, emergency_stop);

	for(msg->spk_num = 0; msg->spk_num <= 1; msg->spk_num++){
		
		msg->amp = amp[msg->spk_num];

		f10 = F10_LOW;
#ifndef DEBUG
		while(f10 < F10_HIGH){
#endif
			msg->sampling_period= SAMPLING_PERIOD;
			f = pow(10.0, f10);
				
			T = 1. / f;
			msg->resolution = T / msg->sampling_period + 1; // 10Hz 時，1999 になってしまうのを 2000 に補正．
			msg->sampling_period  = T / (double)msg->resolution;

			repeat = LENGTH_MAX / msg->resolution; // LENGTH_MAX のデータ長で，正弦波が何個記録できるか?
			msg->recording_samples = repeat * msg->resolution;
			shm[BUF_LEN * (msg->recording_samples - 1)] = -1.; /* 時間を格納する配列の最後に，-1 をフラグとして入れておく */
					
			msg->canceling_samples = WAITING_TIME / msg->sampling_period; /* 記録せずに捨てるサンプルの点数 */
			fprintf(stderr, "sampling_period=%.8f, resolution=%d, recording_samples=%d, canceling_samples=%d\n", msg->sampling_period, msg->resolution, msg->recording_samples, msg->canceling_samples);
			
			/* now start the tasks */
			fprintf(stderr, "Starting real time task ... ");
			msg->command = START_TASK;
			fprintf(stderr, "done.\n");
				
			fprintf(stderr, "Waiting for record ... ");
			while (shm[BUF_LEN * (msg->recording_samples - 1)] < 0.); /* 正常にデータが書きこまれれば，フラグは正になるはず */
			fprintf(stderr, "done.\n");

			j = 0;
			count = 0;
			while(count < BUF_LEN * msg->recording_samples){
				t = shm[count++]; /* ダミー */
				u_time[j] = shm[count++];
				y0_time[j] = shm[count++];
				y1_time[j] = shm[count++];
#ifdef DEBUG			
				fprintf(stdout, "%g %g %g %g\n", t, u_time[j], y0_time[j], y1_time[j]);
#endif
				j++;
			}

#ifndef DEBUG
			realfft(u_time, msg->recording_samples, u_fft);
			u_b = u_fft[repeat * 2 - 1];
			u_a = u_fft[repeat * 2];

			realfft(y0_time, msg->recording_samples, y0_fft);
			y0_b = y0_fft[repeat * 2 - 1];
			y0_a = y0_fft[repeat * 2];
					
			realfft(y1_time, msg->recording_samples, y1_fft);
			y1_b = y1_fft[repeat * 2 - 1];
			y1_a = y1_fft[repeat * 2];
					
			y0_u_gain = sqrt(y0_a * y0_a + y0_b * y0_b) / sqrt(u_a * u_a + u_b * u_b);
			y0_u_phase = (((atan(y0_b / y0_a) + M_PI * (y0_a < 0)) -(atan(u_b / u_a) + M_PI * (u_a < 0))) * 180.0 / M_PI);

			y1_u_gain = sqrt(y1_a * y1_a + y1_b * y1_b) / sqrt(u_a * u_a + u_b * u_b);
			y1_u_phase = (((atan(y1_b / y1_a) + M_PI * (y1_a < 0)) -(atan(u_b / u_a) + M_PI * (u_a < 0))) * 180.0 / M_PI);


			fprintf(stderr, "%f[Hz] %f %f[deg] %f %f[deg]\n", f, y0_u_gain, y0_u_phase, y1_u_gain, y1_u_phase);
			
			fprintf(fp[msg->spk_num], "%f %e %f %e %f\n", f, y0_u_gain, y0_u_phase, y1_u_gain, y1_u_phase);

			f10 += F10_STEP;
		}
#endif
	}

	mbuff_free("data",(void*)shm);
	mbuff_free("msg", (void*)msg);

	fclose(fp[0]);
	fclose(fp[1]);

	return 0;
}