//--------------------------------------------------------------
//  SD カードのテキスト･ファイルの読み書きの例
//
//  mount(), unmount() を使うことを除けば通常のテキストファイルの
//  読み書きと同じ．
//
//  2016/11/14, Copyright (c) 2016 MIKAMI, Naoki
//--------------------------------------------------------------

#include "mbed.h"
#include "SDFileSystem.h"   // SDFileSystem クラスを使うため
#include <stdio.h>
#include <math.h>

#define N 1024  // FFTデータの大きさ 1024,2048,4096
#define M 10    //DFT演算ステージ数    10,11,12
#define PI 3.14159265359//円周率

Timer t;
Ticker t2;

AnalogIn x(A5);
AnalogIn y(A3);
AnalogIn z(A4);

volatile int done = false;
volatile int sampleIndex = 0;

float meas_x[N];
float meas_y[N];
float meas_z[N];

float meas_X[N];
float meas_Y[N];
float meas_Z[N];

float meas_XI[N];
float meas_YI[N];
float meas_ZI[N];

void FFT_x(float *Freq);
void adc_xyz(void);
void FFT_makeTABLE( int n , float *tS , float *tC);
void bit_reverse(float *X, float *RX);
void butterfly(float *XR,float *XI,float *tS,float *tC);

float rev_meas_X[N] ,rev_meas_XI[N];
float rev_meas_Y[N] ,rev_meas_YI[N];
float rev_meas_Z[N] ,rev_meas_ZI[N];



SDFileSystem sd("sd");      // SDFileSystem: SD 用のクラス, 引数の文字列は任意

int main()
{
    Serial pc (SERIAL_TX, SERIAL_RX);
    pc.baud(115200);
  
    float tSIN[ N/2 ] ,tCOS[ N/2 ];
    float Xydb[N],Yydb[N],Zydb[N];
    float freq[N];
    float off_v = 1.65;
    int i;
    char Tm[30];
    char Name[30];
    
    FFT_x(freq);
    
    printf("FFT : started\r\n");
    
    time_t seconds = time(NULL) + (60 * 60 * 9); // JST
    struct tm *t = localtime(&seconds);
 
    
      
    sprintf(Tm,"FFT_%04d%02d%02d_%02d%02d%02d",t->tm_year + 1900, t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec);
    sprintf(Name,"/sd/%s.csv",Tm);
    printf("%s\n",Tm);
    
    FFT_makeTABLE(N , tSIN , tCOS);
    
    t2.attach_us(&adc_xyz,200);
    while (!done) {
        ;
    }
    t2.detach();
    
    for(i=0;i<N;i++){
          meas_X[i] = meas_x[i] * 3.3f; // Converts value in the 0V-3.3V range
          meas_Y[i] = meas_y[i] * 3.3f;
           meas_Z[i] = meas_z[i] * 3.3f;
    }
    /* 
    for(i=0;i<N;i++){
          meas_X[i] = meas_X[i] - off_v;
         meas_Y[i] = meas_Y[i] - off_v;
          meas_Z[i] = meas_Z[i] - off_v;
    }
    */
    butterfly(meas_X,meas_XI,tSIN,tCOS);
    bit_reverse(meas_X,rev_meas_X);
    bit_reverse(meas_XI,rev_meas_XI);
    
    butterfly(meas_Y,meas_YI,tSIN,tCOS);
    bit_reverse(meas_Y,rev_meas_Y);
    bit_reverse(meas_YI,rev_meas_YI);
    
    butterfly(meas_Z,meas_ZI,tSIN,tCOS);
    bit_reverse(meas_Z,rev_meas_Z);
    bit_reverse(meas_ZI,rev_meas_ZI);
    
    sd.mount(); // SD 用

    // SD へ書き込み
    //sprintf(Name,"/sd/FFT_xyz.csv");
    FILE *fp = fopen(Name, "w");
    if (fp == NULL)
    {
        fprintf(stderr, "Open error for writing!!\r\n");
        while (true) {}
    }
    fprintf(fp,"Freq,Xziku,Yziku,Zziku,XdB,YdB,ZdB\n");
    
    for(i=0;i<N;i++){
         Xydb[i] = sqrt((rev_meas_X[i]*rev_meas_X[i]) + (rev_meas_XI[i]*rev_meas_XI[i])) / (N/2);
         Yydb[i] = sqrt((rev_meas_Y[i]*rev_meas_Y[i]) + (rev_meas_YI[i]*rev_meas_YI[i])) / (N/2);
         Zydb[i] = sqrt((rev_meas_Z[i]*rev_meas_Z[i]) + (rev_meas_ZI[i]*rev_meas_ZI[i])) / (N/2);
         fprintf(fp,"%f,%f,%f,%f,%f,%f,%f\n",freq[i],meas_X[i],meas_Y[i],meas_Z[i],Xydb[i],Yydb[i],Zydb[i]);
    }
    fclose(fp);
    
    // SD から読み出し
    fp = fopen(Name, "r");
    if (fp == NULL)
    {
        fprintf(stderr, "Open error for reading!!\r\n");
        while (true) {}
    }
    while (true)
    {
        int chr = fgetc(fp);
        if (chr == EOF) break;
        printf("%c", chr);
        if (chr == '\n') printf("\r");
    }
 
    fclose(fp);
 
    sd.unmount();    // SD 用

    printf("FFT : completed\n");

}

void FFT_x(float *Freq)
{
    int a;
    float b;
    Freq[0] = 0;
    b = (float)1024/5000;
    for(a=1;a<N;a++){
        Freq[a] = (float)a/b;
    }
}


void adc_xyz()
{
    meas_x[sampleIndex] = x.read();
    meas_y[sampleIndex] = y.read();
    meas_z[sampleIndex] = z.read();

    if (sampleIndex >= N) {
        done = true;
    }
    sampleIndex++;
}


void FFT_makeTABLE( int n , float *tS , float *tC)
{
    int i;
    float a;
    float b;

    a=0.0;
    b= (PI * 2.0) / (float)N;

    for( i=0 ; i<n/2 ; i++ ){
        tS[i] = sin(a);
        tC[i] = cos(a);
        a=a+b;
    }

}


//bit逆順
void bit_reverse(float *X, float *RX)
{
    unsigned long m, k, j;
    j = 0;
    for (k = 0; k < N; k++) {
        RX[k] = X[j];
        m = N >> 1;
        while (m >= 1 && j >= m) {
            j -= m;
            m >>= 1;
        }
        j += m;
    }

}

//バタフライ演算
void butterfly(float *XR,float *XI,float *tS,float *tC)
{
    float a,b;

    //バタフライ演算
    int G,L,H,K,P,I,J,Q;

    L = N;
    H = 1;
    a = 0.0;
    b = 0.0;

    for( G=1 ; G<=M ; G++ ){
        L = L/2;
        K = 0;
        for( Q=1 ; Q<=H; Q++ ){
            P = 0;
            for( I=K ; I<=(L+K-1) ; I++){
                J = I+L;
                a = XR[I] - XR[J];
                b = XI[I] - XI[J];
                XR[I] = XR[I] + XR[J];
                XI[I] = XI[I] + XI[J];
                if( P==0){
                    XR[J] = a;
                    XI[J] = b;
                }else{
                    XR[J] = a * tC[P] + b * tS[P];
                    XI[J] = b * tC[P] - a * tS[P];
                }
                P = P+H;
            }
            K = K +L +L;
        }
        H = H+H;
    }
}
