#include <math.h>
#include <Wire.h>
#include "ADS1115.h"
#include "fft.c"
#include <LiquidCrystal_I2C.h>
#define SAMPLEN 512
double calkshindo(double (*fx)[2],double (*fy)[2],double (*fz)[2],double ftime);

ADS1115 adc0(ADS1115_DEFAULT_ADDRESS); //ADDR:GND
LiquidCrystal_I2C lcd(0x20,8,2);//I2Cアドレス、8行x2

int up=0;//upサンプルの現在位置
int uf=0;//uf最大位置
double st;//SAMPLEN個のサンプリングに要した秒数
double ref[]={12968,13029,18562};//基準値
double u[SAMPLEN][3];//測定値をSAMPLEN個保存
double k[3];//3軸の測定値が揃うまで一時保存
unsigned long lt=millis();//最後に震度を計算した時刻
unsigned long nt;
double (*fx)[2];//時間軸サンプル
double (*fy)[2];
double (*fz)[2];

void setup() {                
    Wire.begin();  // I2C開始
    adc0.initialize(); // ADS1115 初期化
    lcd.init();       // 液晶初期化
    adc0.setMode(ADS1115_MODE_SINGLESHOT);//シングルショット･モード
    adc0.setRate(ADS1115_RATE_475);//サンプリングレート475sps
    adc0.setGain(ADS1115_PGA_4P096);//フルスケール±4.096V
    fx = (double (*)[2])malloc(2 * SAMPLEN * sizeof(double));
    fy = (double (*)[2])malloc(2 * SAMPLEN * sizeof(double));
    fz = (double (*)[2])malloc(2 * SAMPLEN * sizeof(double));
}

void loop() 
{
        double tmpx,tmpy,tmpz;
        k[0]=adc0.getConversionP3GND();
        k[1]=adc0.getConversionP2GND();
        k[2]=adc0.getConversionP1GND();
        tmpx=(int)k[0];tmpy=(int)k[1];tmpz=(int)k[2];

	k[0]=(k[0]-ref[0])*980.0/(ref[2]-ref[0]);//galに変換
	k[1]=(k[1]-ref[1])*980.0/(ref[2]-ref[1]);
	k[2]=(k[2]-(ref[0]+ref[1])/2.0)*980.0/(ref[2]-(ref[0]+ref[1])/2.0)-980.0;

	up++;//現在位置を進める
	if(up>=SAMPLEN){up=0;}//超えたら0に戻る
	u[up][0]=k[0];u[up][1]=k[1];u[up][2]=k[2];//測定値を入れる
	nt=millis();//nt現在時刻
	if(uf<SAMPLEN){uf++;st=nt-lt;}

	if((uf>=SAMPLEN) && (nt-lt>1000)){
                lcd.setCursor(0,0);
                lcd.print("X:");
                lcd.print(tmpx);
                lcd.setCursor(8,0);
                lcd.print("Y:");
                lcd.print(tmpy);  
                lcd.setCursor(0,1);
                lcd.print("Z:");
                lcd.print(tmpz);  
/*
                lcd.setCursor(0,0);
                lcd.print("X:");
                lcd.print(k[0]);
                lcd.setCursor(8,0);
                lcd.print("Y:");
                lcd.print(k[1]);
                lcd.setCursor(0,1);
                lcd.print("Z:");
                lcd.print(k[2]);
*/
		lt=nt;//最後に震度計算した時刻更新
                for(int i = 0; i < SAMPLEN-up; i++){//ループになってる配列を整形する
                    fx[i][0]=u[i+up][0];fx[i][1]=0;
                    fy[i][0]=u[i+up][1];fy[i][1]=0;
                    fz[i][0]=u[i+up][2];fz[i][1]=0;
                }
                for(int i = SAMPLEN-up; i < SAMPLEN; i++){
                    fx[i][0]=u[i+up-SAMPLEN][0];fx[i][1]=0;
                    fy[i][0]=u[i+up-SAMPLEN][1];fy[i][1]=0;
                    fz[i][0]=u[i+up-SAMPLEN][2];fz[i][1]=0;
                }

		double kshindo=calkshindo(fx,fy,fz,st/1000.0);//震度計算
/*
		if(kshindo<0.5){lcdout(0,0);}
		if(kshindo>=0.5 && 1.5>kshindo){lcdout(1,0);}//lcd出力
		if(kshindo>=1.5 && 2.5>kshindo){lcdout(2,0);}
		if(kshindo>=2.5 && 3.5>kshindo){lcdout(3,0);}
		if(kshindo>=3.5 && 4.5>kshindo){lcdout(4,0);}
		if(kshindo>=4.5 && 5.0>kshindo){lcdout(5,1);}
		if(kshindo>=5.0 && 5.5>kshindo){lcdout(5,2);}
		if(kshindo>=5.5 && 6.0>kshindo){lcdout(6,1);}
		if(kshindo>=6.0 && 6.5>kshindo){lcdout(6,2);}
		if(kshindo>=6.5){lcdout(7,0);}
*/
                lcd.setCursor(8,1);
                lcd.print("k:");
                lcd.print(kshindo);
	}
}

double calkshindo(double (*fx)[2],double (*fy)[2],double (*fz)[2],double ftime){
	double (*wx)[2];//周波数軸サンプル
	double (*wy)[2];
	double (*wz)[2];
	double (*synthe); 
	wx = (double (*)[2])malloc(2 * SAMPLEN * sizeof(double));
	wy = (double (*)[2])malloc(2 * SAMPLEN * sizeof(double));
	wz = (double (*)[2])malloc(2 * SAMPLEN * sizeof(double));
	synthe=(double (*))malloc(SAMPLEN * sizeof(double));

	double hz_i=1/ftime;//1番目の要素の周波数
	double avex=0;double avey=0;double avez=0;    //平均を減算
	for(int i = 0; i < SAMPLEN; i++){
  		avex+=fx[i][0];
		avey+=fy[i][0];
		avez+=fz[i][0];
	}

	avex/=SAMPLEN;
	avey/=SAMPLEN;
	avez/=SAMPLEN;

	for(int i = 0; i < SAMPLEN; i++){
		fx[i][0]-=avex;
		fy[i][0]-=avey;
		fz[i][0]-=avez;
	}

	fft(SAMPLEN,fx,wx);//fft変換
	fft(SAMPLEN,fy,wy);
	fft(SAMPLEN,fz,wz);

	for(int i = 0; i < SAMPLEN; i++){
		double y,f,filter;
		if(SAMPLEN/2<i){//周波数を割り出す
			f=hz_i*(SAMPLEN-i);//左右対称
		}else{
			f=hz_i*i;
		}
		if(f==0){//直流は消す
			filter=0;
		}else{
			y=f/10;//フィルタを計算
			filter=sqrt(1/f);
			filter*=1/sqrt(1+ 0.694*pow(y,2) + 0.241*pow(y,4) + 0.0557*pow(y,6) + 0.009664*pow(y,8) + 0.00134*pow(y,10) + 0.000155*pow(y,12));
			filter*=sqrt(1-exp(- pow((f/0.5),3) ));
		}
		wx[i][0]*=filter;//フィルタをかける
		wx[i][1]*=filter;
		wy[i][0]*=filter;
		wy[i][1]*=filter;
		wz[i][0]*=filter;
		wz[i][1]*=filter;
	}
	ifft(SAMPLEN,wx, fx);//逆フーリエ変換
	ifft(SAMPLEN,wy, fy);
	ifft(SAMPLEN,wz, fz);

	for(int i = 0; i < SAMPLEN; i++){//合成
		synthe[i]=sqrt(
		pow(fx[i][0],2)+
		pow(fy[i][0],2)+
		pow(fz[i][0],2));
	}
	rsort(synthe,0,SAMPLEN-1);//強い順に並べる
	double a=synthe[(int)(0.3*SAMPLEN/ftime)];//0.3秒の所を取り出す
	double kshindo=floor((round((log10(a)*2.0+0.94)*100.0)/100.0)*10.0)/10.0;//計測震度計算
	if(kshindo<0.0){kshindo=0.0;}
	free(wx);
	free(wy);
	free(wz);
	free(synthe);

	return kshindo;
}

//$out 震度表示
//$kj 無0 弱1 強2
void lcdout(byte out,byte kj){
        lcd.setCursor(0,1);
  	lcd.print("shindo:");
	lcd.print(out);
	if(kj==1){
		lcd.print("-");
	}
	if(kj==2){
		lcd.print("+");
	}
}

void rsort(double arr[], int left, int right) {
      int i = left, j = right;
      double tmp;
      double pivot = arr[(left + right) / 2];
      /* partition */
      while (i <= j) {
            while (arr[i] > pivot)
                  i++;
            while (arr[j] < pivot)
                  j--;
            if (i <= j) {
                  tmp = arr[i];
                  arr[i] = arr[j];
                  arr[j] = tmp;
                  i++;
                  j--;
            }
      };
      /* recursion */
      if (left < j)
            rsort(arr, left, j);
      if (i < right)
            rsort(arr, i, right);
}