#define SAMPLEN 512
#include <rxduino.h>
#include <liquidcrystal.h>
#include <spi.h>
#include "fftfloat.c"

LiquidCrystal lcd(4,5,6,7,2,9,3);
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[]={0xc05900,0xc07800,0xd94b00};//基準値
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];//テンポラリ
char p[3];//3軸の測定値が揃ったかどうかのフラグ

unsigned long stmp;//サンプリングを開始した時刻
unsigned long st;//SAMPLEN個のサンプリングに要した秒数

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

void timer_interrupt()
{
    int rch0,rch1,rch2;
    static char intflag=0;
    static float lc[]={-0.6,-0.1,0.4,0.9};
    if(intflag==1){return;}
    intflag=1;
    if (digitalRead(PIN_P20)) {
        intflag=0;
        return;
    }
    for(int i=0;i<4;i++){
        lc[i]+=0.002;
        if(lc[i]>0.99){lc[i]=-1;}
    }
    analogWrite(PIN_LED0, pow(lc[0],4)*255);
    analogWrite(PIN_LED1, pow(lc[1],4)*255);
    analogWrite(PIN_LED2, pow(lc[2],4)*255);
    analogWrite(PIN_LED3, pow(lc[3],4)*255);

    SPI.transfer(0x10);          // Data Read Command
    rch0 = SPI.transfer(0xff);          // NOP:ffh
    rch1 = SPI.transfer(0xff);
    rch2 = SPI.transfer(0xff);
    if(rch0>0x7f){
        rch0-=0x80;
    }else{
        rch0+=0x80;
    }
    k[adcsel]=(float)((rch0<<16)|(rch1<<8)|rch2);
    kt[adcsel]=(rch0<<8)|rch1;
    p[adcsel]=1;//軸フラグセット
    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=timer_get_ms()-stmp;stmp=timer_get_ms();}//超えたら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.transfer(0x40);     // write register 00h
    switch(adcsel){
        case 0:
            SPI.transfer(0x91);  // AIN1-GND GAIN:1 PGA:off
            adcsel=1;
            break;
        case 1:
            SPI.transfer(0xa1);  // AIN2-GND GAIN:1 PGA:off
            adcsel=2;
            break;
        case 2:
            SPI.transfer(0x81);  // AIN0-GND GAIN:1 PGA:off
            adcsel=0;
            break;
    }
    SPI.transfer(0x08);     //start
    intflag=0;
}

void setup()
{
    lcd.begin(16, 2);

    pinMode(PIN_P20, INPUT);//IO1
    SPI.begin();
    SPI.setBitLength(8);
    SPI.setBitOrder(MSBFIRST);
    SPI.setDataMode(SPI_MODE1);
    SPI.setClockDivider(SPI_CLOCK_DIV128);

    pinMode(PIN_P21, OUTPUT);
    pinMode(PIN_LED0, OUTPUT);
    pinMode(PIN_LED1, OUTPUT);
    pinMode(PIN_LED2, OUTPUT);
    pinMode(PIN_LED3, OUTPUT);
    analogWriteFrequency(300);

    delay(112);
    SPI.transfer(0x06);//reset
    delayMicroseconds(320);    //15us:NG 16us:OK
    SPI.transfer(0x43);// write register 00h-03h
    SPI.transfer(0x81);// AIN0-GND GAIN:1 PGA:off
    SPI.transfer(0x80);// 330sps nomalmode single temp:off burn:off
    SPI.transfer(0xc0);// ref:AVDD
    SPI.transfer(0x00);// DOUT/DRDY

    SPI.transfer(0x40);     // write register 00h
    SPI.transfer(0x81);  // AIN1-GND GAIN:1 PGA:off
    SPI.transfer(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;//軸フラグクリア
    stmp=timer_get_ms();

    timer_regist_userfunc(timer_interrupt);
}

void loop()
{
    int rch0,rch1,rch2;
    char str[16];
    char *p;
    p=str;
    float kshindo;
    int i;

    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,((float)st)/1000);//震度計算
        if(kshindo>=0.5){
            digitalWrite(PIN_P21, 1);
        }else{
            digitalWrite(PIN_P21, 0);
        }
        p=str;
        p += sprintf(p,"X:%04X Y:%04X ",kt[0],kt[1]);
        lcd.setCursor(0,0);
        lcd.print(str);
        p=str;
        p += sprintf(p,"Z:%04X k%1.1f",kt[2],kshindo);
        lcd.setCursor(0,1);
        lcd.print(str);
    }
}

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.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,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.3*SAMPLEN/ftime)];//0.3秒の所を取り出す
    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);
}