#include "mbed.h"
#include "MARMEX_OB_oled.h"
#define SAMPLEN 256
#include "fftfloat.c"

MARMEX_OB_oled   oled1( PC_12, PC_10, PA_14, PH_1, PA_13 ); // mosi, sclk, cs, rst, power_control

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[]={0xc00000,0xc00000,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軸の測定値が揃ったかどうかのフラグ 
char intflag=0;
float st;//SAMPLEN個のサンプリングに要した秒数

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

char font[]={
0x00,0x00,0x00,0x00,0xf0,0x0f,0xf8,0x1f,0x1c,0x38,0x0e,0x70,0x06,0x60,0x06,0x60,//0
0x06,0x60,0x06,0x60,0x0e,0x70,0x1c,0x38,0xf8,0x1f,0xf0,0x0f,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x00,0x18,0x00,0x18,0x00,0x1c,0x00,//1
0xfe,0x7f,0xfe,0x7f,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x70,0x70,0x7c,0x78,0x1c,0x7c,0x0e,0x6e,0x06,0x67,//2
0x06,0x63,0x86,0x63,0x86,0x61,0xce,0x61,0xfc,0x60,0xf8,0x60,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x38,0x1e,0x3c,0x3e,0x0e,0x70,0x06,0x60,0x86,0x61,//3
0x86,0x61,0x86,0x61,0x86,0x61,0xce,0x73,0xfc,0x3f,0x78,0x1e,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x07,0x80,0x07,0xc0,0x07,0xf0,0x06,0x78,0x06,//4
0x1c,0x06,0x0e,0x06,0xfe,0x7f,0xfe,0x7f,0x00,0x06,0x00,0x06,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0xe0,0x1d,0xfe,0x3d,0xfe,0x71,0xe6,0x60,0x66,0x60,//5
0x66,0x60,0x66,0x60,0x66,0x70,0xe6,0x30,0xc6,0x3f,0x80,0x1f,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0xf0,0x0f,0xf8,0x1f,0xdc,0x39,0xee,0x70,0x66,0x60,//6
0x66,0x60,0x66,0x60,0x66,0x60,0xee,0x70,0xdc,0x3f,0x98,0x1f,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x06,0x00,0x06,0x00,0x06,0x00,0x06,0x60,0x06,0x7c,//7
0x06,0x3f,0xc6,0x07,0xfe,0x01,0x7e,0x00,0x0e,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x02,0x03,0xe2,0x43,0xe2,0x41,0x22,0x41,0x3e,0x7f,0x3e,0x3f,0x10,0x40,0x90,0x47,//強
0x9c,0x47,0x9f,0x44,0xf3,0x7f,0xf8,0x7f,0x8a,0x24,0x8e,0x34,0x9c,0x77,0x98,0x67,
0x02,0x11,0xf2,0x11,0xf2,0x1b,0x92,0x4a,0x92,0x4c,0x92,0x64,0x9e,0x3f,0x9e,0x1f,//弱
0x02,0x11,0xf2,0x11,0xf2,0x1b,0x92,0x4a,0x92,0x4c,0x92,0x64,0x9e,0x3f,0x9e,0x1f,
0x18,0x70,0x9a,0x7f,0x8a,0x4f,0xaa,0x48,0xaa,0x7a,0xaa,0x7a,0xaa,0x6a,0xbe,0x2a,//震
0xbe,0x2a,0xaa,0x1a,0xaa,0x3a,0xaa,0x2a,0xaa,0x7a,0x8a,0x58,0x9a,0x48,0x98,0x48,
0x00,0x40,0x00,0x70,0xfc,0x3f,0xfc,0x4f,0x24,0x42,0x24,0x42,0xf4,0x66,0xf4,0x2e,//度
0xa7,0x3a,0xa7,0x12,0xf4,0x3a,0xf4,0x2e,0x24,0x66,0x24,0x42,0x24,0x40,0x04,0x40,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,//blank
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
};

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

void putfont(int x,int y,int chr,int mul){
    int col;
    for(int lx=0;lx<16;lx++){
        for(int ly=0;ly<8;ly++){
            if(font[lx*2+chr*32]&(1<<ly)){
                col=0xFF0000;
            }else{
                col=0x000000;
            }
            intflag=1;
            wait_us(10);
            oled1.fill( lx*mul+x, ly*mul+y , mul, mul,  col );
            intflag=0;
        }
        for(int ly=8;ly<16;ly++){
            if(font[lx*2+1+chr*32]&(1<<(ly-8))){
                col=0xFF0000;
            }else{
                col=0x000000;
            }
            intflag=1;
            wait_us(10);
            oled1.fill( lx*mul+x, ly*mul+y , mul, mul,  col );
            intflag=0;
        }
    }   
}

int main() {
    float kshindo;
    int i;
    int outfont,outjk;
    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, 10); 
    
    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);//震度計算

            outfont=0;outjk=12;
            if(kshindo>=0.5f){ledkaiten=1;outfont=1;}else{ledkaiten=0;outfont=0;}
            if(kshindo>=1.5f){outfont=2;}
            if(kshindo>=2.5f){outfont=3;}
            if(kshindo>=3.5f){outfont=4;}
            if(kshindo>=4.5f){outfont=5;outjk=9;}
            if(kshindo>=5.0f){outfont=5;outjk=8;}
            if(kshindo>=5.5f){outfont=6;outjk=9;}
            if(kshindo>=6.0f){outfont=6;outjk=8;}
            if(kshindo>=6.5f){outfont=7;outjk=12;}

            putfont(0,0,10,1);
            putfont(16,0,11,1);
            putfont(0,16,outfont,4);
            putfont(64,16,outjk,4);
            intflag=1;
            oled1.locate( 0, 12 );
            oled1.printf( "X:%04X Y:%04X",kt[0],kt[1]);
            oled1.locate( 0, 13 );
            oled1.printf( "Z:%04X k:%1.1f",kt[2],kshindo);
            oled1.locate( 0, 14 );
            oled1.printf( "t:%1.2f",st);
            intflag=0;
        }
    }
}

void adsconv(){
        int rch0,rch1,rch2;
        if(intflag==1){return;}
        intflag=1;

        if (drdy==1) {//pin8 input
            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);
}