#include "mbed.h"
#define SAMPLEN 512
#include "fftfloat.c"
#include "TextLCD.h"
TextLCD lcd(D2,D3,D4,D5,D6,D7); // rs, e, d4-d7

SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK); // mosi, miso, sclk
Timer utime;//タイマー
Ticker ads;
DigitalOut ledkaiten(D0);
DigitalIn drdy(D8);

float calkshindo(float (*fx)[2],float (*fy)[2],float (*fz)[2],float ftime);
float dummycal(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];//測定値をSAMPLEN個保存
float wx[SAMPLEN][2];
float wy[SAMPLEN][2];
float wz[SAMPLEN][2];
float XX[SAMPLEN][2];
float synthe[SAMPLEN];


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

float st;//SAMPLEN個のサンプリングに要した秒数

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

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

int main() {
    float kshindo;
    int i;
    spi.format(8,1);
    spi.frequency(750000);
    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(0x00);// 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;//軸フラグクリア

    utime.start();
    utime.reset();
    ads.attach_us(&adsconv, 3333); 
    
    set_time(0);

    while(1)
    {
        if(uf>=SAMPLEN ){
            for(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(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;
            }
            kshindo=calkshindo(fx,fy,fz,st);//震度計算
            lcd.locate(0,0);
            lcd.printf( "%04X %04X\n",kt[0],kt[1]);
            lcd.printf( "%04X ",kt[2]);
            lcd.printf( "k%1.1f",kshindo);
//            if(kshindo>=0.5f){ledkaiten=1;}else{ledkaiten=0;}
        }
    }
}

void adsconv(){
        int rch0,rch1,rch2;
        static char intflag=0;
        if(intflag==1){return;}
        intflag=1;
        if (drdy==1) {
            intflag=0;
            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;
        p[adcsel]=true;//軸フラグセット
        if(p[0]&&p[1]&&p[2]){
            k[0]=(k[0]-ref[0])*980.0f/(ref[2]-ref[0]);//galに変換
            k[1]=(k[1]-ref[1])*980.0f/(ref[2]-ref[1]);
            k[2]=(k[2]-(ref[0]+ref[1])/2.0f)*980.0f/(ref[2]-(ref[0]+ref[1])/2.0f)-980.0f;

            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
        intflag=0;
}

float calkshindo(float (*fx)[2],float (*fy)[2],float (*fz)[2],float ftime){
    float hz_i,avex,avey,avez,kshindo,a;
    float y,f,filter;
    int i;
    hz_i=1/ftime;//1番目の要素の周波数
    avex=0;avey=0;avez=0;    //平均を減算

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

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

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

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

    for(i = 0; i < SAMPLEN; i++){
        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.694f*pow(y,2) + 0.241f*pow(y,4) + 0.0557f*pow(y,6) + 0.009664f*pow(y,8) + 0.00134f*pow(y,10) + 0.000155f*pow(y,12));
            filter*=sqrt(1-exp(- pow((f/0.5f),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,XX);//逆フーリエ変換
    ifft(SAMPLEN,wy, fy,XX);
    ifft(SAMPLEN,wz, fz,XX);

    for(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);//強い順に並べる
    a=synthe[(int)(0.3f*SAMPLEN/ftime)];//0.3秒の所を取り出す
    kshindo=floor((round((log10(a)*2.0f+0.94f)*100.0f)/100.0f)*10.0f)/10.0f;//計測震度計算
    if(kshindo<0.0f){kshindo=0.0f;}
    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);
}