AltImuAnalyze.Unity/Assets/AnalyzeFFT.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;
using System.Numerics;

public class AnalyzeFFT : MonoBehaviour {


    public enum Axis {
        X, Y, Z
    }

    public Axis SourceAxis = Axis.X;

    public Accelerometer Accelerometer;

    public bool UseAbsoluteScale = false;
    public float AbsoluteScale = 0.2f;

    private long _samplesTotalCount = 0;
    //input-output values for the FFT
    double[] X_FFT_inputValues;
    double[] Y_FFT_inputValues;
    double[] Y_output;


    public const int AccelerometerSamplingFrequency = 2000;

    public int windowSize = AccelerometerSamplingFrequency;


    public double maxf_obtained;

    //counts the steps taken
    long count = 0;

    //the chart transforms for the time and frequency
    [Space(10)]
    [Header("CHARTS FOR DRAWING")]
    [Space]
    public Transform tfTime;
    public Transform tfFreq;


    void Start() {
        Accelerometer.OnNewSample += Accelerometer_OnNewSample;
        Accelerometer.OnStarted += Accelerometer_OnStarted;
        Accelerometer.OnStopped += Accelerometer_OnStopped;
    }

    private void Accelerometer_OnStarted(Accelerometer obj) {
        _samplesTotalCount = 0;
        _processedSamplesCount = 0;
        Debug.Log("Accelerometer started");
    }

    private void Accelerometer_OnStopped(Accelerometer obj) {
       
        Debug.Log("Accelerometer stopped");
    }

    private void Accelerometer_OnNewSample(Accelerometer arg1, UnityEngine.Vector3 arg2) {
        if (_ySourceSamples == null || _ySourceSamples.Length != windowSize) {
            _ySourceSamples = new double[windowSize];
            _yWriteOffset = 0;
        }

        float data = arg2.x;
        if(SourceAxis == Axis.Y) {
            data = arg2.y;
        }else if (SourceAxis == Axis.Z) {
            data = arg2.z;
        }


        _ySourceSamples[_yWriteOffset] = data;


        _yWriteOffset = (_yWriteOffset + 1) % windowSize;

        _samplesTotalCount++;
    }




    double[] _ySourceSamples;
    int _yWriteOffset = 0;

    private long _processedSamplesCount = 0;

    private void Update() {
        if (X_FFT_inputValues == null || X_FFT_inputValues.Length != windowSize) {
            X_FFT_inputValues = new double[windowSize];
            for (int i = 0; i < windowSize; i++) {
                X_FFT_inputValues[i] = i;
            }
        }

        if (Y_FFT_inputValues == null || Y_FFT_inputValues.Length != windowSize) {
            Y_FFT_inputValues = new double[windowSize];
        }

        TimeToFrequency();
    }

    void TimeToFrequency() {
        if(_processedSamplesCount == _samplesTotalCount) {
            return;
        }

        //this is the window size
        for (int i = 0; i < windowSize; i++) {
            Y_FFT_inputValues[i] = _ySourceSamples[(_yWriteOffset + i) % windowSize];
        }



        //perform complex opterations and set up the arrays
        Complex[] inputSignal_Time = new Complex[windowSize];
        Complex[] outputSignal_Freq = new Complex[windowSize];


        inputSignal_Time = FastFourierTransform.doubleToComplex(Y_FFT_inputValues);


        //result is the iutput values once DFT has been applied
        outputSignal_Freq = FastFourierTransform.FFT(inputSignal_Time, false);



        Y_output = new double[windowSize];
        //get module of complex number
        for (int ii = 0; ii < windowSize; ii++) {
            //Debug.Log(ii);
            Y_output[ii] = (double)Complex.Abs(outputSignal_Freq[ii]);
        }


        // find peak VALUES on the FFT
        double MaxPeak = -1000;
        int peakIndex = 0;
        for (int i = 1; i < Y_output.Length / 2 - 1; i++) {
            if (Y_output[i] > MaxPeak) {
                MaxPeak = Y_output[i];
                peakIndex = i;
            }
        }

        //store results
        maxf_obtained = (double)peakIndex / (double)windowSize /*/ 2*/ * (double)AccelerometerSamplingFrequency;

        
        

    }



    void FixedUpdate() {
        if(X_FFT_inputValues == null) {
            return;
        }

        if(Y_FFT_inputValues != null) {
            Drawing.drawChart(tfTime, X_FFT_inputValues, Y_FFT_inputValues, "time");
        }



        if(Y_output != null) {


            Drawing.drawChart(tfFreq, X_FFT_inputValues, Y_output, "frequency", UseAbsoluteScale ? AbsoluteScale : null);
        }
      
    }







}