#include "getGeometryInfo.h"
#include "../config/geometryConfig.h"
#include "Function1D.h"
#include "Function.h"
#include "Function2D.h"
#include "Matrix.h"
#include "src/common/getMeasurementMetaData.h"

#include <algorithm>
#include <cstddef>
#include <type_traits>
#include <vector>

using namespace Recon;
using namespace Aurora;

namespace
{
Matrix loadSensitivity()
{
    double* senseRevise = new double[Recon::ANGLE_CORRECTION_SIZE - 1];
    std::reverse_copy(ANGLE_CORRECTION.getData(), ANGLE_CORRECTION.getData() + Recon::ANGLE_CORRECTION_SIZE - 1, senseRevise);
    size_t fullSens1DSize = Recon::ANGLE_CORRECTION_SIZE * 2 - 1;
    double* fullSens1DData = new double[fullSens1DSize];
    std::copy(ANGLE_CORRECTION.getData(), ANGLE_CORRECTION.getData() + Recon::ANGLE_CORRECTION_SIZE, fullSens1DData);
    std::copy(senseRevise, senseRevise + Recon::ANGLE_CORRECTION_SIZE - 1, fullSens1DData + Recon::ANGLE_CORRECTION_SIZE);
    Matrix fullSens1D = Matrix::fromRawData(fullSens1DData, 1, fullSens1DSize);
    Matrix fullSens2D = repmat(fullSens1D, fullSens1DSize, 1);
    Matrix fullSens2DTransposed = transpose(fullSens2D);
    fullSens2D = sqrt(fullSens2D * fullSens2DTransposed);
    fullSens2D = fullSens2D / max(fullSens2D,FunctionDirection::All);

    Matrix x0 = linspace(-90,90,fullSens1D.getDataSize());
    Matrix y0 = linspace(-90,90,fullSens1D.getDataSize());
    Matrix x1 = linspace(-90,90,181);
    Matrix y1 = linspace(-90,90,181);
    double* resultData = new double[y1.getDataSize() * x1.getDataSize()];
    Matrix result = Matrix::fromRawData(resultData, y1.getDataSize(), x1.getDataSize());
    std::vector<Matrix> resultTemp(181);
    #pragma omp parallel for
    for(int i=0; i<181; ++i)
    {
        Matrix y = repmat(Matrix::copyFromRawData(y1.getData() + i, 1),181,1);
        resultTemp[i]=interpn(x0,y0,fullSens2D,x1,y,InterpnMethod::Linear);
    }  
    for(int i=0; i<181; ++i)
    {
        std::copy(resultTemp[i].getData(), resultTemp[i].getData() +  resultTemp[i].getDataSize(), resultData);
        resultData += resultTemp[i].getDataSize();
    }
    return result;
}

Matrix rotateAndTranslate(const Matrix& transMat, Matrix&  aGeometryMatrix)
{
    padding(aGeometryMatrix, aGeometryMatrix.getDataSize(), 1);
    Matrix out = transMat.mul(aGeometryMatrix);
    out.forceReshape(out.getDimSize(0) - 1, out.getDimSize(1), out.getDimSize(2));
    return out;
}

void transformGeometry(const Matrix& aMotorPos, const Matrix& aTransformationMatrices,
                       const Matrix aRlList, const Matrix aRnList, const Matrix aSlList, const Matrix aSnList, GeometryInfo& aOutput)
{
    double motorPos = max(aMotorPos)[0];
    size_t receiverMatrixSize = motorPos * max(aRlList)[0] *  max(aRnList)[0] * 3;
    size_t senderMatrixSize = motorPos * max(aSlList)[0] *  max(aSnList)[0] * 3;

    for(int m=0; m<motorPos; ++m)
    {
        Matrix transMat = aTransformationMatrices($,$,m).toMatrix();
        Matrix transMatNormals = transpose(inv(transMat));
        double* receiverNormalsData = new double[receiverMatrixSize];
        double* receiverNormalsDataCopyPointer = receiverNormalsData;
        double* receiverPositionsData = new double[receiverMatrixSize];
        double* receiverPositionsCopyPointer = receiverPositionsData;
        for(int i=0; i<aRlList.getDataSize(); ++i)
        {
            for(int j=0; j<aRnList.getDataSize(); ++j)
            {
                Matrix receiverNormalsMatrix = Recon::receiverNormals[i](j).toMatrix();
                Matrix rotateData = rotateAndTranslate(transMatNormals, receiverNormalsMatrix);
                size_t rotateSize = rotateData.getDimSize(0);
                std::copy(rotateData.getData(), rotateData.getData() + rotateSize, receiverNormalsDataCopyPointer);
                receiverNormalsDataCopyPointer += rotateSize;

                Matrix receiverPositionsMatrix = Recon::receiverPositions[i](j).toMatrix();
                rotateData = rotateAndTranslate(transMat, receiverPositionsMatrix);
                rotateSize = rotateData.getDimSize(0);
                std::copy(rotateData.getData(), rotateData.getData() + rotateSize, receiverPositionsCopyPointer);
                receiverPositionsCopyPointer += rotateSize;
            }
        }
        aOutput.receiverNormals.push_back(Matrix::fromRawData(receiverNormalsData, 3, aRnList.getDataSize(), aRlList.getDataSize()));
        aOutput.receiverPositions.push_back(Matrix::fromRawData(receiverPositionsData, 3, aRnList.getDataSize(), aRlList.getDataSize()));

        double* senderNormalsData = new double[senderMatrixSize];
        double* senderNormalsDataCopyPointer = senderNormalsData;
        double* senderPositionsData = new double[senderMatrixSize];
        double* senderPositionsCopyPointer = senderPositionsData;
        for(int i=0; i<aSlList.getDataSize(); ++i)
        {
            for(int j=0; j<aSnList.getDataSize(); ++j)
            {
                Matrix senderNormalsMatrix = Recon::emitterNormals[i](j).toMatrix();
                Matrix rotateData = rotateAndTranslate(transMatNormals, senderNormalsMatrix);
                size_t rotateSize = rotateData.getDimSize(0);
                std::copy(rotateData.getData(), rotateData.getData() + rotateSize, senderNormalsDataCopyPointer);
                senderNormalsDataCopyPointer += rotateSize;

                Matrix senderPositionsMatrix = Recon::emitterPositions[i](j).toMatrix();
                rotateData = rotateAndTranslate(transMat, senderPositionsMatrix);
                rotateSize = rotateData.getDimSize(0);
                std::copy(rotateData.getData(), rotateData.getData() + rotateSize, senderPositionsCopyPointer);
                senderPositionsCopyPointer += rotateSize;
            }
        }
        aOutput.senderNormals.push_back(Matrix::fromRawData(senderNormalsData, 3, aSnList.getDataSize(), aSlList.getDataSize()));
        aOutput.senderPositions.push_back(Matrix::fromRawData(senderPositionsData, 3, aSnList.getDataSize(), aSlList.getDataSize()));
    }
}

void getApertureBoundingBox(GeometryInfo& aOutput)
{
    //Emitter
    double* minEmitterData = new double[3];
    double* maxEmitterData = new double[3];

    size_t emitterSize = emitterPositions.size() * emitterPositions[0].getDimSize(0);
    double* emitterXData = new double[emitterSize];
    Matrix emitterX = Matrix::fromRawData(emitterXData, emitterSize);
    double* emitterYData = new double[emitterSize];
    Matrix emitterY = Matrix::fromRawData(emitterYData, emitterSize);
    double* emitterZData = new double[emitterSize];
    Matrix emitterZ = Matrix::fromRawData(emitterZData, emitterSize);
    for(int i=0; i<emitterPositions.size(); ++i)
    {
        Matrix x = emitterPositions[i]($,0,$).toMatrix();
        std::copy(x.getData(), x.getData() + x.getDataSize(), emitterXData);
        emitterXData += x.getDataSize();

        Matrix y = emitterPositions[i]($,1,$).toMatrix();
        std::copy(y.getData(), y.getData() + y.getDataSize(), emitterYData);
        emitterYData += y.getDataSize();

        Matrix z = emitterPositions[i]($,2,$).toMatrix();
        std::copy(z.getData(), z.getData() + z.getDataSize(), emitterZData);
        emitterZData += z.getDataSize();
    }
    minEmitterData[0] = min(emitterX, FunctionDirection::All)[0];
    minEmitterData[1] = min(emitterY, FunctionDirection::All)[0];
    minEmitterData[2] = min(emitterZ, FunctionDirection::All)[0];
    maxEmitterData[0] = max(emitterX, FunctionDirection::All)[0];
    maxEmitterData[1] = max(emitterY, FunctionDirection::All)[0];
    maxEmitterData[2] = max(emitterZ, FunctionDirection::All)[0];

    aOutput.minEmitter = Matrix::fromRawData(minEmitterData, 3);
    aOutput.maxEmitter = Matrix::fromRawData(maxEmitterData, 3);

    //Receiver
    double* minReceiverData = new double[3];
    double* maxReceiverData = new double[3];

    size_t receiverSize = receiverPositions.size() * receiverPositions[0].getDimSize(0);
    double* receiverXData = new double[receiverSize];
    Matrix receiverX = Matrix::fromRawData(receiverXData, receiverSize);
    double* receiverYData = new double[receiverSize];
    Matrix receiverY = Matrix::fromRawData(receiverYData, receiverSize);
    double* receiverZData = new double[receiverSize];
    Matrix receiverZ = Matrix::fromRawData(receiverZData, receiverSize);
    for(int i=0; i<receiverPositions.size(); ++i)
    {
        Matrix x = receiverPositions[i]($,0,$).toMatrix();
        std::copy(x.getData(), x.getData() + x.getDataSize(), receiverXData);
        receiverXData += x.getDataSize();

        Matrix y = receiverPositions[i]($,1,$).toMatrix();
        std::copy(y.getData(), y.getData() + y.getDataSize(), receiverYData);
        receiverYData += y.getDataSize();

        Matrix z = receiverPositions[i]($,2,$).toMatrix();
        std::copy(z.getData(), z.getData() + z.getDataSize(), receiverZData);
        receiverZData += z.getDataSize();
    }
    minReceiverData[0] = min(receiverX, FunctionDirection::All)[0];
    minReceiverData[1] = min(receiverY, FunctionDirection::All)[0];
    minReceiverData[2] = min(receiverZ, FunctionDirection::All)[0];
    maxReceiverData[0] = max(receiverX, FunctionDirection::All)[0];
    maxReceiverData[1] = max(receiverY, FunctionDirection::All)[0];
    maxReceiverData[2] = max(receiverZ, FunctionDirection::All)[0];

    aOutput.minReceiver = Matrix::fromRawData(minReceiverData, 3);
    aOutput.maxReceiver = Matrix::fromRawData(maxReceiverData, 3);

    //border
    aOutput.minSize = min(horzcat(aOutput.minEmitter, aOutput.minReceiver),FunctionDirection::Row);
    aOutput.maxSize = max(horzcat(aOutput.maxEmitter, aOutput.maxReceiver),FunctionDirection::Row);

}
}

GeometryInfo Recon::getGeometryInfo(const Matrix& aMotorPos, const Matrix& aTransformationMatrices,
                                    const Matrix aRlList, const Matrix aRnList, const Matrix aSlList, const Matrix aSnList)
{
    GeometryInfo result;
    result.headTable = Recon::HEAD_TABLE;
    result.sensChar = loadSensitivity();
    transformGeometry(aMotorPos, aTransformationMatrices, aRlList, aRnList, aSlList, aSnList, result);
    getApertureBoundingBox(result);
    result.maxSL = emitterPositions.size();
    result.maxSN = emitterPositions[0].getDimSize(0);
    result.maxRL = receiverPositions.size();
    result.maxRN = receiverPositions[0].getDimSize(0);
    result.numTAS = (result.maxSL >= result.maxRL) ? result.maxSL : result.maxRL;

    return result;
}