#include "common.h"
#include <iostream>

#include "Function.h"
#include "Function1D.h"
#include "Function2D.h"
#include "Function3D.h"
#include "Matrix.h"

namespace Recon
{
    Aurora::Matrix reconstructBandpasssubsampling(Aurora::Matrix aMatrix, double aAScanReconstructionFreq, double aSampleRate)
    {
        double downsamplingfactor = aAScanReconstructionFreq / aSampleRate;
        double minimalExpectedAScanLength = 48;
        int expectedAScanlengthDS = (int)std::ceil(minimalExpectedAScanLength/downsamplingfactor);
        double expectedAScanLength = std::max(expectedAScanlengthDS*downsamplingfactor, aMatrix.getDimSize(0)*downsamplingfactor);
        expectedAScanlengthDS = std::ceil(expectedAScanLength/downsamplingfactor);

        double s1 = aAScanReconstructionFreq; 
        double f1Stride = s1 / (expectedAScanLength - 1);
        double * f1Data = Aurora::malloc(expectedAScanLength);
        auto f1 = Aurora::Matrix::New(f1Data,expectedAScanLength,1,1);
        for (int i = 0; i<expectedAScanLength; ++i) {
            f1Data[i] = i*f1Stride;
        }
        auto f2 = Aurora::Matrix::New(f1Data,expectedAScanLength,1,1);

        auto Data2 =  Aurora::zeros(expectedAScanLength,aMatrix.getDimSize(1));
        auto datafft = Aurora::fft(aMatrix, expectedAScanlengthDS);
        Aurora::fftshift(datafft);
        datafft(expectedAScanlengthDS/2+1) = 0.0;
        auto idx_l = (f1 > (aSampleRate/2)) *(f1 <= aSampleRate);
        auto idx_r = (f1 > (aAScanReconstructionFreq-aSampleRate)) * (f1 <= (aAScanReconstructionFreq-aSampleRate/2));
        if (Aurora::sum(idx_l).getScalar() == 0.0 || Aurora::sum(idx_r).getScalar() == 0.0){
            std::cerr<<"downsampled data, you have to set AScanReconstructionFreq higher"<<std::endl;
            return Aurora::Matrix();
        }
        if (datafft.getDimSize(0)/2 != idx_r.getDataSize())
        {
            std::cerr<<"error: Fourier resolution (->length) needs to be fixed by padding"<<std::endl;
            return Aurora::Matrix();
        }
        
        //TODO:2代逻辑，暂时不写了
        //Code:
        //for j = 1:size(Data, 2)
        //     Data2(idx_l(1:end-0), j) = (datafft(1:floor(size(datafft, 1)/2)-0, j));
        //     Data2(idx_r(1:end-0)+1, j) = (flipud(conj(datafft(1:(size(datafft, 1) / 2)-0, j))));
        // end
        // % in old version without scaling to downsamplingfactor!
        // Data2 = ifft(Data2, [], 1);
        return Aurora::Matrix();
    }

    Aurora::Matrix convertToLinearIndices(Aurora::Matrix aVMatrixSize, Aurora::Matrix aMCoordinates)
    {
        if (aVMatrixSize.getDataSize() != aMCoordinates.getDimSize(1)){
            std::cerr<<"convertToLinearIndices fail, Dimension of coordinates and matrix do not fit."<<std::endl;
            return Aurora::Matrix();
        }
        size_t columns = aMCoordinates.getDimSize(1);
        auto coord_col1= aMCoordinates(Aurora::$,0).toMatrix();
        auto coord_col2Sub1= aMCoordinates(Aurora::$,1).toMatrix()-1;
        auto matrixSize_1 = aVMatrixSize.getData()[0];
        switch (columns) {
            case 3:
            {
                auto matrixSize_2 = aVMatrixSize.getData()[1];
                auto coord_col3Sub1= aMCoordinates(Aurora::$,2).toMatrix()-1;
                return coord_col1 + coord_col2Sub1 * matrixSize_1 + coord_col3Sub1 * matrixSize_1 * matrixSize_2;
            }
            case 2:
            {
                return coord_col1 + coord_col2Sub1 * matrixSize_1;
            }
            default:
                std::cerr<<"convertToLinearIndices fail,NotImplemented, Converting into linear indices only implemented for 2D and 3D matrices."<<std::endl;
                return Aurora::Matrix();
        }
    }
}