#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;

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], *fp2[2];
	char filename[2][30] = {"data/servo1.dat", "data/servo2.dat"};
	char filename2[2][30] = {"data/timeresp1.dat", "data/timeresp2.dat"};

	int j, repeat, count;
	static double torq0[LENGTH_MAX];
	static double speed0[LENGTH_MAX];
	static double torq1[LENGTH_MAX];
	static double speed1[LENGTH_MAX];

	double amp = AMP, offset = OFFSET;
	double f, f10;
	double y1_u_gain, y1_u_phase, y2_u_gain, y2_u_phase;

	float ts[LENGTH_MAX];
	double T;

	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;
	}
	
	for(msg->motor_num = 0; msg->motor_num<=1; msg->motor_num++){

         switch(msg->motor_num){
          case 0:
           msg->motorD_num =1;
           break;
          case 1:
           msg->motorD_num =0;
           break;
          }

	  msg->command = STOP_TASK;
	  
	  signal(SIGINT, emergency_stop);

	  msg->amp = amp;
	  msg->offset = offset;
		
	    fp[msg->motor_num] = fopen(filename[msg->motor_num], "w");
	    fp2[msg->motor_num] = fopen(filename2[msg->motor_num], "w");
	    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;
			msg->omega = 2*M_PI / T;

			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){
				ts[j] = shm[count++]; 
				torq0[j] = shm[count++];
				speed0[j] = shm[count++];
				torq1[j] = shm[count++];
				speed1[j] = shm[count++];

#ifdef DEBUG			
				fprintf(stdout, "%g %g %g %g %g\n", ts[j], torq0[j], speed0[j], torq1[j], speed1[j]);
                                fprintf(fp2[msg->motor_num], "%g %g %g %g %g\n", ts[j], torq0[j], speed0[j], torq1[j], speed1[j]);
#endif
				j++;
			}

//#ifndef DEBUG
		
			y1_u_gain = sqrt(msg->ya1 * msg->ya1 + msg->yb1 * msg->yb1) / sqrt(msg->ua * msg->ua + msg->ub * msg->ub);
			y1_u_phase = ((atan(msg->yb1 / msg->ya1) + M_PI * (msg->ya1 < 0)) - (atan(msg->ub / msg->ua) + M_PI * (msg->ua < 0))) * 180.0 / M_PI;
			y2_u_gain = sqrt(msg->ya2 * msg->ya2 + msg->yb2 * msg->yb2) / sqrt(msg->ua * msg->ua + msg->ub * msg->ub);
			y2_u_phase = ((atan(msg->yb2 / msg->ya2) + M_PI * (msg->ya2 < 0)) - (atan(msg->ub / msg->ua) + M_PI * (msg->ua < 0))) * 180.0 / M_PI;

			fprintf(stderr, "%f[Hz] %f    %f[deg]    %f    %f[deg]\n",f, y1_u_gain, y1_u_phase, y2_u_gain, y2_u_phase);
			fprintf(fp[msg->motor_num], "%f    %f    %f    %f    %f\n",f, y1_u_gain, y1_u_phase, y2_u_gain, y2_u_phase);

//#endif
			f10 += F10_STEP;

#ifndef DEBUG
		}
#endif

		//	  }

	  fclose(fp[msg->motor_num]);
	  fclose(fp2[msg->motor_num]);
	}

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

	//	fclose(fp);
	//	fclose(fp2);

	return 0;
}