#include <math.h>
#include "mbed.h"
#define SAMPLEN 512
#include "fftfloat.c"
#define uselcd
//#define useserial

#ifdef uselcd
#include "TextLCD_SB1602E.h"
I2C                i2c( p9, p10 );        // sda, scl
TextLCD_SB1602E    lcd( &i2c );           //液晶表示
#endif

#ifdef useserial
Serial pc(USBTX, USBRX);                  //シリアルでモニタ
#endif

SPI spi(p5, p6, p7); // mosi, miso, sclk
Timer mtime;  //タイマー 1秒毎に計測震度を計算する
Timer utime;  //タイマー サンプリングに要した時間
Ticker ads;   //1ms毎にADコンバータからデータを受け取る
DigitalOut led1(LED1);//震度1-4表示
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

float calkshindo(float (*fx)[2],float (*fy)[2],float (*fz)[2],float ftime);
void rsort(float arr[], int left, int right);
void adsconv();

int up=0;//upサンプルの現在位置
int uf=0;//uf最大位置
float ref[]={0xc04200,0xc02200,0xd9d000};//基準値
float u[SAMPLEN][3] __attribute__((section("AHBSRAM0")));//測定値をSAMPLEN個保存
float wx[SAMPLEN][2] __attribute__((section("AHBSRAM0")));//fft用
float wy[SAMPLEN][2] __attribute__((section("AHBSRAM1")));
float wz[SAMPLEN][2] __attribute__((section("AHBSRAM1")));
float synthe[SAMPLEN] __attribute__((section("AHBSRAM1")));//合成値用

char adcsel=0;//変換中の軸
float k[3];//3軸の測定値が揃うまで一時保存
int kt[3];//テンポラリ
bool p[3];//3軸の測定値が揃ったかどうかのフラグ 

float st;//SAMPLEN個のサンプリングに要した秒数
//float lt;//最後に震度を計算した時刻
//float ut;//u reset time
//float nt;//現在時間

float (*fx)[2];//時間軸サンプル
float (*fy)[2];
float (*fz)[2];

float round(float x){
  return(float(int(x+0.5)));
}

int main() {
    spi.format(8,1);
    spi.frequency(750000);
#ifdef useserial
    pc.baud(115200);
#endif
    wait_ms(112);    //55ms:NG 56ms:OK
    spi.write(0x06);//reset
    wait_us(32);    //15us:NG 16us:OK
    spi.write(0x43);// write register 00h-03h
    spi.write(0x81);// AIN0-GND GAIN:1 PGA:off
    spi.write(0x80);// 330sps nomalmode single temp:off burn:off
    spi.write(0xc0);// ref:AVDD
    spi.write(0x02);// DOUT/DRDY

    spi.write(0x08);// START

    fx = (float (*)[2])malloc(2 * SAMPLEN * sizeof(float));
    fy = (float (*)[2])malloc(2 * SAMPLEN * sizeof(float));
    fz = (float (*)[2])malloc(2 * SAMPLEN * sizeof(float));
    adcsel=0;
    p[0]=0;p[1]=0;p[2]=0;//軸フラグクリア
    mtime.start();//タイマースタート
    mtime.reset();
    utime.start();//タイマースタート
    utime.reset();
    ads.attach_us(&adsconv, 1000); //割り込み設定

    while(1)
    {
//        if((uf>=SAMPLEN) && (mtime.read()>1)){//1秒毎に計測震度を計算する
        if(uf>=SAMPLEN){//ずっと計測震度を計算する
            mtime.reset();//タイマーリセット
#ifdef uselcd
            lcd.printf( 0, "%04X%04X\r",kt[0],kt[1]);//lcd測定値表示
            lcd.printf( 1, "%04X",kt[2]);
#endif
            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;
            }

            float kshindo=calkshindo(fx,fy,fz,st);//震度計算

#ifdef uselcd
            lcd.printf( 1, "k%1.1f\r",kshindo);//lcd計測震度表示
#endif
            if(kshindo>=0.5){led1=1;}else{led1=0;}//ledに震度表示
            if(kshindo>=1.5){led2=1;}else{led2=0;}
            if(kshindo>=2.5){led3=1;}else{led3=0;}
            if(kshindo>=3.5){led4=1;}else{led4=0;}
#ifdef useserial
            pc.printf( "%04X %04X %04X ",kt[0],kt[1],kt[2]);//シリアルに送信
            pc.printf( "k%1.1f\r\n",kshindo);
#endif
        }
    }
}

void adsconv(){
        int rch0,rch1,rch2;
        if (LPC_GPIO0->FIOPIN & (1UL << 8)) {//pin6 input
            return;
        }

        spi.write(0x10);          // Data Read Command
        rch0 = spi.write(0xff);          // NOP:ffh
        rch1 = spi.write(0xff);
        rch2 = spi.write(0xff);
        if(rch0>0x7f){
            rch0-=0x80;
        }else{
            rch0+=0x80;
        }
        k[adcsel]=(float)((rch0<<16)|(rch1<<8)|rch2);
        kt[adcsel]=(rch0<<8)|rch1;//モニタ用上位16bit
        p[adcsel]=true;//軸フラグセット
        if(p[0]&&p[1]&&p[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;st=utime.read();utime.reset();}//超えたら0に戻る
            u[up][0]=k[0];u[up][1]=k[1];u[up][2]=k[2];//測定値を入れる
            if(uf<SAMPLEN){uf++;}//
            p[0]=0;p[1]=0;p[2]=0;//軸フラグクリア
        }
        spi.write(0x40);     // write register 00h
        switch(adcsel){
        case 0:
            spi.write(0x91);  // AIN1-GND GAIN:1 PGA:off
            adcsel=1;
            break;
        case 1:
            spi.write(0xa1);  // AIN2-GND GAIN:1 PGA:off
            adcsel=2;
            break;
        case 2:
            spi.write(0x81);  // AIN0-GND GAIN:1 PGA:off
            adcsel=0;
            break;
        }
        spi.write(0x08);     //start
}

float calkshindo(float (*fx)[2],float (*fy)[2],float (*fz)[2],float ftime){

    float hz_i=1/ftime;//1番目の要素の周波数
    float avex=0;float avey=0;float 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++){
        float 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);//強い順に並べる
    float a=synthe[(int)(0.3*SAMPLEN/ftime)];//0.3秒の所を取り出す
    float kshindo=floor((round((log10(a)*2.0+0.94)*100.0)/100.0)*10.0)/10.0;//計測震度計算
    if(kshindo<0.0){kshindo=0.0;}
    return kshindo;
}

void rsort(float arr[], int left, int right) {//ソート
    int i = left, j = right;
    float tmp;
    float 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);
}