#include <vector>
#include <stdexcept>
#include <fstream>
#include "Graph.h"
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/trim_all.hpp>
#include <boost/algorithm/string/join.hpp>
#include <iostream>
#include <boost/regex.hpp>

inline const string BoolToString(bool b)
{
    return b ? "true" : "false";
}

Graph::Graph(vector<vector<int>> adjacencyMatrix) {
    nodes = vector<string>();
    for (int i = 0; i < adjacencyMatrix[0].size(); i++) {
        nodes.push_back(to_string(i));
    }
    if (isSquareMatrix(adjacencyMatrix)) {
        this->adjacencyMatrix = adjacencyMatrix;
    } else {
        throw invalid_argument("adjacency matrix has to be symmetrical");
    };
}

Graph::Graph(vector<vector<int>> adjacencyMatrix, vector<string> node_names) {
    cout << "node names: " << endl;
    for (string str : node_names) {
        cout << str << endl;
    }
    if (isSquareMatrix(adjacencyMatrix)) {
        this->adjacencyMatrix = adjacencyMatrix;
    } else {
        throw invalid_argument("adjacency matrix has to be symmetrical");
    };

    if (node_names.size() == getNumberOfNodes()) {
        this->nodes = vector<string>();
        for (string node : node_names) {
            nodes.push_back(node);
        }
    } else {
        cout << "Nodes: " << getNumberOfNodes() << ", nodes in argument: " << node_names.size() << endl;
        throw invalid_argument("Nodes number does not match adjencency matrix:\nNodes: ");
    }
}

Graph::Graph(string matlabMatrix) {

    // replace all tabs
    boost::replace_all(matlabMatrix, "\t", " ");
    // remove leading, trailing spaces and compress double spaces
    boost::trim_all(matlabMatrix);

    // replace spaces before and after semicolons
    boost::replace_all(matlabMatrix, " ; ", ";");
    boost::replace_all(matlabMatrix, "; ", ";");
    boost::replace_all(matlabMatrix, " ;", ";");

    if (!boost::starts_with(matlabMatrix, "[") || !boost::ends_with(matlabMatrix, "]")) {
        throw invalid_argument("A valid matlab matrix notation must start with '[' and end up with ']'.");
    }

    // remove square braces and arising (trailing) spaces
    matlabMatrix.erase(0, 1);
    matlabMatrix.erase(matlabMatrix.size() - 1);
    boost::trim(matlabMatrix);

    // split string at ';' ==> get rows of the matrix
    vector<std::string> rows;
    boost::split(rows, matlabMatrix, boost::is_any_of(";"));
    vector<vector<int>> adjacencyMatrix = getZeroizedMatrix((int) rows.size(), (int) rows.size());
    int columnMax = 0;

    // get columns of the matrix
    for (unsigned long rIndex = 0; rIndex < rows.size(); rIndex++) {
        vector<std::string> columns;
        boost::split(columns, rows[rIndex], boost::is_any_of(" "));
        boost::trim(rows[rIndex]);

        if (columnMax == 0) {
            columnMax = (int) columns.size();
        }

        // Error: There is a row with more ore less columns
        if (columnMax != columns.size()) {
            throw invalid_argument("There must be a syntax error, because the number of columns are different.");
        }

        // matrix must be square!
        if (rows.size() != columns.size()) {
            throw invalid_argument("adjacency matrix has to be symmetrical");
        }

        // add values to matrix
        for (unsigned long cIndex = 0; cIndex < columns.size(); cIndex++) {
            try {
                adjacencyMatrix.at(rIndex).at(cIndex) = stoi(columns[cIndex]);
            } catch (const invalid_argument &e) {
                throw invalid_argument("matrix entries must be and contain at least one integer.");
            }

        }
    }

    this->adjacencyMatrix = adjacencyMatrix;

    nodes = vector<string>();
    for (int i = 0; i < adjacencyMatrix[0].size(); i++) {
        nodes.push_back(to_string(i));
    }
}

bool Graph::isSquareMatrix(const vector<vector<int>> &adjacencyMatrix) const {
    int rows = (int) adjacencyMatrix.size();
    int cols = 0;
    for (vector<int> row : adjacencyMatrix) {
        // row.size() = number of cols in the current row
        if (row.size() > cols) {
            cols = (int) row.size();
        }
    }

    return rows == cols;
}

const vector<vector<int>> &Graph::getAdjacencyMatrix() const {
    return adjacencyMatrix;
}

/**
 * Detect the number of nodes of the given graph.
 * @return number of nodes.
 */
int Graph::getNumberOfNodes() const {
    return (int) this->adjacencyMatrix.size();
}

int Graph::getInDeg(int vertexIndex) {

    if (vertexIndex < 0 || vertexIndex >= this->getNumberOfNodes()) {
        throw invalid_argument("vertex index has to be between 0 and n, but is " + to_string(vertexIndex) + ".");
    }

    // no value cached ==> calculate degree
    if (this->inDeg.empty() || this->inDeg[vertexIndex] < 1) {

        // count edges
        int countIngoingEdges = 0;
        for (int row = 0; row < this->getNumberOfNodes(); row++) {
            // adds ingoing edges
            countIngoingEdges += this->adjacencyMatrix[row][vertexIndex];

            // In undirected graphs loops are counted twice:
            if (adjacencyMatrix[row][vertexIndex] > 0 && row == vertexIndex && !this->isDirected()) {
                countIngoingEdges++;
            }
        }

        // setting cache value
        this->inDeg.resize((unsigned long) this->getNumberOfNodes());
        this->inDeg[vertexIndex] = countIngoingEdges;
    }

    return this->inDeg[vertexIndex];
}

int Graph::getOutDeg(int vertexIndex) {

    if (vertexIndex < 0 || vertexIndex >= this->getNumberOfNodes()) {
        throw invalid_argument("vertex index has to be between 0 and n, but is " + to_string(vertexIndex) + ".");
    }

    // no value cached ==> calculate degree
    if (this->outDeg.empty() || this->outDeg[vertexIndex] < 1) {

        // count edges
        int countOutgoingEdges = 0;
        for (int col = 0; col < this->getNumberOfNodes(); col++) {
            // adds ingoing edges
            countOutgoingEdges += this->adjacencyMatrix[vertexIndex][col];

            // In undirected graphs loops are counted twice:
            if (adjacencyMatrix[vertexIndex][col] > 0 && col == vertexIndex && !this->isDirected()) {
                countOutgoingEdges++;
            }
        }

        // setting cache value
        this->outDeg.resize((unsigned long) this->getNumberOfNodes());
        this->outDeg[vertexIndex] = countOutgoingEdges;
    }

    return this->outDeg[vertexIndex];
}

bool Graph::isDirected() {

    // Get cached result.
    switch (this->type) {
        case DIRECTED:
            return true;

        case UNDIRECTED:
            return false;

        case UNCHECKED:
            // check if values of the adjacency matrix are set symmetrically ==> undirected graph
            // otherwise diagraph
            for (int row = 0; row < this->getNumberOfNodes(); row++) {
                // check only upper triangle part of matrix
                for (int col = row + 1; col < this->getNumberOfNodes(); col++) {
                    // values must be the same, otherwise its a digraph
                    if (this->getAdjacencyMatrix()[row][col] != this->getAdjacencyMatrix()[col][row]) {
                        // cache result
                        this->type = DIRECTED;
                        return true;
                    }
                }
            }

            break;
    }

    // cache result
    this->type = UNDIRECTED;
    return false;
}

bool Graph::isFreeOfLoops() {

    if (isFreeOfLoopsFlag) {
        return isFreeOfLoopsCache;
    }

    isFreeOfLoopsFlag = true;

    for (int mainDiagonal = 0; mainDiagonal < this->getNumberOfNodes(); ++mainDiagonal) {
        // check if there is a loop
        if (this->getAdjacencyMatrix()[mainDiagonal][mainDiagonal] > 0) {
            isFreeOfLoopsCache = false;
            return isFreeOfLoopsCache;
        }
    }

    isFreeOfLoopsCache = true;
    return isFreeOfLoopsCache;
}

bool Graph::isMultigraph() {

    if (isMultigraphFlag) {
        return isMultigraphCache;
    }

    isMultigraphFlag = true;

    /*
     * if the adjacency matrix contains a value greater than 1 the matrix
     * has multiple edges and therefore the graph is a multigraph.
     */
    for (int row = 0; row < this->getNumberOfNodes(); ++row) {
        for (int col = 0; col < this->getNumberOfNodes(); ++col) {
            if (this->getAdjacencyMatrix()[row][col] > 1) {
                isMultigraphCache = true;
                return isMultigraphCache;
            }
        }
    }

    isMultigraphCache = false;
    return isMultigraphCache;
}

bool Graph::isSimple() {
    return !this->isDirected() && !isMultigraph() && isFreeOfLoops();
}

bool Graph::isComplete() {

    if (isCompleteFlag) {
        return isCompleteCache;
    }

    isCompleteFlag = true;

    /*
     * the graph is complete if the adjacency matrix has only entries of 1
     * except the main diagonal which must be 0.
     */
    if (!isFreeOfLoops()) {
        isCompleteCache = false;
        return isCompleteCache;
    }

    for (int row = 0; row < this->getNumberOfNodes(); row++) {
        for (int col = 0; col < this->getNumberOfNodes(); col++) {
            // only 1 is allowed now
            if (row != col && this->getAdjacencyMatrix()[row][col] != 1) {
                isCompleteCache = false;
                return isCompleteCache;
            }
        }
    }

    isCompleteCache = true;
    return isCompleteCache;
}

bool Graph::isRegular() {

    if (isRegularFlag) {
        return isRegularCache;
    }

    isRegularFlag = true;

    // get in/out deg of the first vertex
    int inDegOfFirstVertex = this->getInDeg(0);
    int outDegOfFirstVertex = this->getOutDeg(0);

    // all other vertices must have the same deg,
    // otherwise this graph is not regular
    for (int vertex = 1; vertex < this->getNumberOfNodes(); vertex++) {

        // directed graph ==> indeg and outdeg can differ ==> we have to test both
        // undirected graph ==> indeg and outdeg must be the same ==> we only must test one
        // ==> we can use the following commands for both types
        if (this->getInDeg(vertex) != inDegOfFirstVertex || this->getOutDeg(vertex) != outDegOfFirstVertex) {
            isRegularCache = false;
            return isRegularCache;
        }

    }

    isRegularCache = true;
    return isRegularCache;
}

/**
 * Transform the graph to json-format.
 * @return string json.
 */
string Graph::graphToJson() {

    string edgeType = "line";

    if (this->isDirected())
        string edgeType = "arrow";

    int size = (int) this->adjacencyMatrix.size();

    string nodes = "\"nodes\": [";

    // parse json nodes
    for (int i = 0; i < size; i++) {
        nodes += "{ ";
        nodes += "\"id\": \"" + to_string(i) + "\",";
        nodes += "\"label\": \"" + to_string(i) + "\",";
        nodes += "\"x\": \"" + to_string(rand() % 10) + "\",";
        nodes += "\"y\": \"" + to_string(rand() % 10) + "\",";
        nodes += "\"size\": \"" + to_string(1) + "\"";

        if (i < (size - 1)) {
            nodes += "},";
        } else {
            nodes += "}";
        }
    }

    nodes += "]";

    string edges = "\"edges\": [";
    int y = 0;
    int id = 0;

    // parse json edges
    for (vector<int> row : adjacencyMatrix) {

        // column
        for (int x = 0; x < row.size(); x++) {

            // if there is a connection between two nodes.
            if (row[x] == 1) {

                if (id == 0) {
                    // first element
                    edges += "{ ";
                } else {
                    edges += ", { ";
                }

                // set properties
                edges += "\"id\": " + to_string(id) + ",";
                edges += "\"source\": " + to_string(y) + ",";
                edges += "\"target\": " + to_string(x) + ",";
                edges += "\"type\": \"" + edgeType + "\",";
                edges += "\"size\": " + to_string(1);

                edges += "}";

                id++;
            }
        }
        y++;
    }

    edges += "]";

    string properties = "\"properties\": {" ;

    properties += "\"Vertices\": \"" + to_string(getNumberOfNodes()) + "\",";
    properties += "\"Edges\": \"" + to_string(getNumberOfEdges()) + "\",";
    properties += "\"isDirected\": \"" + BoolToString(isDirected()) + "\",";
    properties += "\"isComplete\": \"" + BoolToString(isComplete()) + "\",";
    properties += "\"isMultigraph\": \"" + BoolToString(isMultigraph()) + "\",";
    properties += "\"isRegular\": \"" + BoolToString(isRegular()) + "\",";
    properties += "\"isSimple\": \"" + BoolToString(isSimple()) + "\",";
    properties += "\"hasCycle\": \"" + BoolToString(hasCycle()) + "\",";
    properties += "\"isFreeOfLoops\": \"" + BoolToString(isFreeOfLoops()) + "\",";
    properties += "\"isForest\": \"" + BoolToString(isForest()) + "\"";

    properties += "}";

    return "{ \"graph\": {" + nodes + ", " + edges + "}, " + properties + " }";
}

/**
 * Export a given file with custom data.
 * @param fileName string file name.
 * @param data string of data.
 */
void Graph::exportFile(const string fileName, const string data) const {
    ofstream file;
    file.open(fileName);
    file << data;
    file.close();
}

void Graph::exportAdjazenzmatrixFile(const string fileName) const {
    return exportFile(fileName, getAdjacencyMatrixString());
}

/**
 * Checks the graph if a cycle exists. DFS.
 * @return true if a cycle exists, else false
 */
bool Graph::hasCycle() {

    if (hasCycleFlag) {
        return hasCycleCache;
    }

    int nodes = this->getNumberOfNodes();

    // array for the visited nodes
    bool *visited = new bool[nodes];
    // array for the stack
    bool *stack = new bool[nodes];

    // set the default values to the stack and the visited nodes arrays
    for (int i = 0; i < nodes; i++) {
        visited[i] = false;
        stack[i] = false;
    }

    for (int i = 0; i < nodes; i++) {
        if (hasCycleRec(i, visited, stack)) {
            hasCycleCache = true;
            hasCycleFlag = true;
            return true;
        }
    }

    hasCycleFlag = true;
    hasCycleCache = false;
    return false;
}

/**
 * Private helper function for hasCycle function.
 * @param i current index.
 * @param visited array contains visited elements.
 * @param stack for recursion.
 * @return true if cycle found else false
 */
bool Graph::hasCycleRec(const int i, bool *visited, bool *stack) const {

    // check if node was not visited.
    if (!visited[i]) {
        visited[i] = true;
        stack[i] = true;

        // iterate through the given adja matrix row (i)
        for (int x = 0; x < this->getNumberOfNodes(); ++x) {
            if (adjacencyMatrix[x][i] > 0) {
                // if the current node has an edge
                if (!visited[x] && hasCycleRec(x, visited, stack)) {
                    // if node was not visited and there is a existing cycle
                    return true;
                } else if (stack[x]) {
                    // if the stack contains the current node
                    return true;
                }
            }
        }

    }

    // delete node from stack if the node was visited
    stack[i] = false;
    return false;
}

/**
 * Checks if the graph contains the given edge.
 * @param from node number
 * @param to node number
 * @return true if the edge exists, else if not
 */
bool Graph::hasEdge(const int from, const int to) {

    if (isDirected()) {
        return adjacencyMatrix[from][to] > 0;
    } else {
        return adjacencyMatrix[from][to] > 0 && adjacencyMatrix[to][from] > 0;
    }

}

/**
 * Checks, if the graph contains the given path.
 * @param path vector with path nodes.
 * @return true if graph contains the path, else false
 */
bool Graph::hasPath(const vector<int> path) const {

    if (path.size() == 0) {
        throw invalid_argument("The vector must contain at least one value!");
    }

    for (int i = 0; i < path.size(); i++) {
        if (path[i] >= this->getNumberOfNodes()) {
            throw invalid_argument("The vector contains not existing nodes!");
        } else if (i + 1 < path.size() && adjacencyMatrix[path[i]][path[i + 1]] == 0) {
            return false;
        }
    }

    return true;
}

bool Graph::hasConnectivity(int s, int t) {

    if (s < 0 || s > getNumberOfNodes() - 1) {
        throw invalid_argument("invalid s");
    } else if (t < 0 || t > getNumberOfNodes() - 1) {
        throw invalid_argument("invalid t");
    }

    // there is a direct connection form s to t
    if (getAdjacencyMatrix()[s][t] > 0) {
        return true;
    }

    // loops are not allowed if adjacency matrix has none there
    if (s == t && getAdjacencyMatrix()[s][t] == 0) {
        return false;
    }

    // there is no direct connection from s to t
    // test if there are any connections using idea from
    // https://de.wikipedia.org/wiki/Adjazenzmatrix#Pfadl.C3.A4nge_in_Graphen_berechnen
    // we skip identity (A^0) and original adjacency matrix (A^1) ==> start at k = 2
    for (int k = 2; k < getNumberOfNodes(); ++k) {

        vector<vector<int>> tmp = powerMatrix(getAdjacencyMatrix(), k);

        if (tmp[s][t] > 0) {
            return true;
        }
    }

    return false;
}

bool Graph::isSameMatrix(const vector<vector<int>> &A, const vector<vector<int>> &B) {

    if (A.size() != B.size() || A[0].size() != B[0].size()) {
        return false;
    }

    // rows of A
    for (int i = 0; i < A.size(); i++) {
        // cols of A
        for (int j = 0; j < A[0].size(); j++) {
            if (A[i][j] != B[i][j]) {
                return false;
            }
        }
    }

    return true;
}

vector<vector<int>> Graph::getZeroizedMatrix(int rows, int cols) {

    // initialize an empty square matrix
    vector<vector<int>> resultMatrix = vector<vector<int>>((unsigned long) rows);

    // only diagonal is 1
    for (int diagonal = 0; diagonal < rows; diagonal++) {
        // set row to 0
        resultMatrix[diagonal] = vector<int>((unsigned long) cols, 0);
    }

    return resultMatrix;
}

vector<vector<int>> Graph::getIdentityMatrix(int size) {

    // initialize an empty square matrix
    vector<vector<int>> resultMatrix = getZeroizedMatrix(size, size);

    // set diagonal to 1
    for (int diagonal = 0; diagonal < size; diagonal++) {
        resultMatrix[diagonal][diagonal] = 1;
    }

    return resultMatrix;
}

vector<vector<int>> Graph::powerMatrix(vector<vector<int>> squareMatrix, int exponent) {

    // initialize an empty square matrix
    vector<vector<int>> resultMatrix;

    if (exponent < 0) {
        throw invalid_argument("not implemented yet");

    } else if (exponent == 0) {
        resultMatrix = getIdentityMatrix((int) squareMatrix.size());

    } else if (exponent == 1) {
        return squareMatrix;

    } else {
        resultMatrix = getIdentityMatrix((int) squareMatrix.size());

        for (int multiplyIndex = 0; multiplyIndex < exponent; multiplyIndex++) {
            resultMatrix = multiplyMatrix(resultMatrix, squareMatrix);
        }
    }

    return resultMatrix;
}

vector<vector<int>> Graph::multiplyMatrix(const vector<vector<int>> &A, const vector<vector<int>> &B) {
    // rows of A
    int l = (int) A.size();

    // cols of A MUST BE rows of B
    int m = (int) A[0].size();
    if (A[0].size() != B.size()) {
        throw invalid_argument("Matrix A must have as much rows as B columns. ");
    }

    // cols of B
    int n = (int) B[0].size();

    vector<vector<int>> C = getZeroizedMatrix(l, n);

    // multiply matrix c_{ij} = \sum_{j=1}^{m} a_{ij} \cdot \b_{jk}
    // loop over rows of C
    for (int i = 0; i < l; i++) {
        // loop over columns of C
        for (int k = 0; k < n; k++) {
            // loop over columns of A/rows of B
            for (int j = 0; j < m; j++) {
                C[i][k] = C[i][k] + A[i][j] * B[j][k];
            }
        }
    }

    return C;
}

vector<vector<int>> Graph::addMatrix(const vector<vector<int>> &A, const vector<vector<int>> &B) {

    if (A.size() != B.size() || A[0].size() != B[0].size()) {
        throw invalid_argument("Matrix A and B must have the same size (rows and columns). ");
    }

    // rows of A
    int m = (int) A.size();

    // cols of A MUST BE rows of B
    int n = (int) A[0].size();

    vector<vector<int>> C = getZeroizedMatrix(m, n);

    // add matrix
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            C[i][j] = A[i][j] + B[i][j];
        }
    }

    return C;
}

int Graph::getNumberOfEdges() {
    if (adjacencyMatrix.size() <= 0) {
        return 0;
    }
    int count = 0;
    long end = adjacencyMatrix.size();
    for (long i = 0; i < end; i++) {
        for (long j = 0; j < end; j++) {
            if (adjacencyMatrix[i][j]) {
                count++;
            }
        }
    }
    if (!isDirected()) {
        count /= 2;
    }
    return count;
}

string Graph::exportDot() {
    string data = "";
    if (isDirected()) { data += "digraph {\n"; } else { data += "graph {\n"; }

    //create nodes - in case one node has no edges
    for (int i = 0; i < adjacencyMatrix.size(); i++) {
        data += nodes[i] + "\n";
    }

    for (int x = 0; x < adjacencyMatrix.size(); x++) {
        if (!isDirected()) {
            for (long y = 0; y <= x; y++) {
                if (adjacencyMatrix[x][y] > 0) {
                    data += "\t" + nodes[x];
                    data += " -- ";
                    data += nodes[y];
                }
            }
        } else {
            for (int y = 0; y < adjacencyMatrix.size(); y++) {
                if (adjacencyMatrix[x][y] > 0) {
                    data += "\t" + nodes[x];
                    data += " -> ";
                    data += nodes[y];
                }
            }
        }
    }
    data += "\n}\n";
    return data;
}

bool Graph::areNeighbours(int from, int to) {

    if (from < 0 || to < 0 || from > getNumberOfNodes() - 1 || to > getNumberOfNodes() - 1) {
        throw out_of_range("from and to have to be in the range. ");
    }

    if (isDirected()) {
        return getAdjacencyMatrix()[from][to] > 0;
    } else {
        return getAdjacencyMatrix()[from][to] > 0 && getAdjacencyMatrix()[to][from] > 0;
    }
}

string Graph::exportDot(vector<int> path) {
    if (!hasPath(path)) {
        return "error: Not a valid path";
    }

    string data = "";
    if (isDirected()) { data += "digraph {\n"; } else { data += "graph {\n"; }

    //create nodes - in case one node has no edges
    for (int i = 0; i < adjacencyMatrix.size(); i++) {
        data += nodes[i] + "\n";
    }

    for (int x = 0; x < adjacencyMatrix.size(); x++) {
        if (!isDirected()) {
            for (long y = 0; y <= x; y++) {
                if (adjacencyMatrix[x][y] > 0) {
                    data += "\t" + nodes[x];
                    data += " -- ";
                    data += nodes[y];
                    //Create path color
                    for (int i = 1; i <= path.size(); i++) {
                        if (path[i - 1] == x && path[i] == y) {
                            data += "[color= maroon];";
                        }
                    }
                }

            }
        } else {
            for (int y = 0; y < adjacencyMatrix.size(); y++) {
                if (adjacencyMatrix[x][y] > 0) {
                    data += "\t" + nodes[x];
                    data += " -> ";
                    data += nodes[y];
                    //Create path color
                    for (int i = 1; i <= path.size(); i++) {
                        if (path[i - 1] == x && path[i] == y) {
                            data += "[color= maroon];";
                        }
                    }
                }
            }
        }
    }
    data += "\n}\n";
    return data;
}

const string Graph::getAdjacencyMatrixString() const {
    stringstream ss;

    long end = adjacencyMatrix.size();
    for (long i = 0; i < end; i++) {
        auto mat = adjacencyMatrix[i];

        std::vector<std::string> list;
        transform(mat.begin(), mat.end(), std::back_inserter(list),
                  [](const int i) { return to_string(i); });

        ss << boost::algorithm::join(list, ",");
        ss << endl;
    }

    return ss.str();
}

bool Graph::isForest() {
    if (isForestCache) {
        return isForestFlag;
    }

    isForestCache = true;
    if(hasCycle() || !isDirected()) {
        isForestFlag = false;
        return false;
    }
    for (int i = 0; i < adjacencyMatrix.size(); i++) {
        if(getInDeg(i) > 1) {
            isForestFlag = false;
            return false;
        }
    }
    isForestFlag = true;
    return true;
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_TRANSFORM3DVECTOR_HPP
#define RW_MATH_TRANSFORM3DVECTOR_HPP

/**
 * @file Transform3DVector.hpp
 */

#if !defined(SWIG)

#include <rw/core/Ptr.hpp>
#include <rw/math/EAA.hpp>
#include <rw/math/Quaternion.hpp>
#include <rw/math/Rotation3DVector.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/math/Vector3D.hpp>

#include <Eigen/Core>

#endif
namespace rw { namespace math {
    /** @addtogroup math */
    /* @{*/

    /**
     * @brief this class is a interpolatable Transform3D, consisting of a Vecor3D and a Quaternion.
     * It is implemented to be very Interconvertable with a Transform3D, and allow operations souch
     * as Transform * scalar and Transform + Transform.
     */
    template< class T = double > class Transform3DVector
    {
      public:
        typedef Eigen::Matrix< T, 7, 1 > type;
        typedef rw::core::Ptr< Transform3DVector< T > > Ptr;

        /**
         * @brief default constructor
         */
        Transform3DVector ():Transform3DVector(Vector3D<T>(),Quaternion<T>()) {}

        /**
         * @brief Constuct a Transformation matrix as a Vector
         * @param vec [in] the vector of the transform
         * @param rot [in] the rotation of the transform
         */
        Transform3DVector (const Vector3D< T >& vec, const Quaternion< T >& rot)
        {
            _t3d[0] = vec[0];
            _t3d[1] = vec[1];
            _t3d[2] = vec[2];
            _t3d[3] = rot[0];
            _t3d[4] = rot[1];
            _t3d[5] = rot[2];
            _t3d[6] = rot[3];
        }

        /**
         * @brief Constuct a Transformation matrix as a Vector
         * @param vec [in] the vector of the transform
         * @param rot [in] the rotation of the transform
         */
        Transform3DVector (const Vector3D< T >& vec, const Rotation3DVector< T >& rot)
        {
            Quaternion< T > rotq (rot);
            _t3d[0] = vec[0];
            _t3d[1] = vec[1];
            _t3d[2] = vec[2];
            _t3d[3] = rotq[0];
            _t3d[4] = rotq[1];
            _t3d[5] = rotq[2];
            _t3d[6] = rotq[3];
        }

        /**
         * @brief Constuct a Transform3DVector from a Transform3D
         * @param t3d [in] Transform3D
         */
        Transform3DVector (const Transform3D< T >& t3d) :
            Transform3DVector (t3d.P (), Quaternion< T > (t3d.R ()))
        {}

        /**
         * @brief Constuct a Transform3DVector from a EigenVector
         * @param vec [in] Transform3D
         */
        Transform3DVector (const type& vec) : _t3d (vec) {}

        /**
         * @brief destructor
         */
        ~Transform3DVector () {}

        // ###################################################
        // #                 Math Operators                  #
        // ###################################################

        // ########## Vector3D Operators

        /**
         * @brief add a Vector3D to the position of the transform
         * @param rhs [in] the right hand side value
         * @return result of devision
         */
        Transform3DVector< T > operator+ (const Vector3D< T >& rhs) const
        {
            Transform3DVector< T > ret = *this;
            for (size_t i = 0; i < rhs.size (); i++) {
                ret[i] += rhs[i];
            }
            return ret;
        }

        /**
         * @brief subtract a Vector3D from the position of the transform
         * @param rhs [in] the right hand side value
         * @return result of devision
         */
        Transform3DVector< T > operator- (const Vector3D< T >& rhs) const
        {
            Transform3DVector< T > ret = *this;
            for (size_t i = 0; i < rhs.size (); i++) {
                ret[i + 3] -= rhs[i];
            }
            return ret;
        }

        /**
         * @brief Matrix vector multiplication with Vector3D. Same as Transform3D<> * Vector3D<>;
         * @param rhs [in] the right hand side value
         * @return result of multiplication
         */
        Vector3D< T > operator* (const Vector3D< T >& rhs) const
        {
            return this->toTransform3D () * rhs;
        }

        // ########## Quaternion Operators

        /**
         * @brief add a Quaternion to the rotation of the transform
         * @param rhs [in] the right hand side value
         * @return result of devision
         */
        Transform3DVector< T > operator+ (const Quaternion< T >& rhs) const
        {
            Transform3DVector< T > ret = *this;
            for (size_t i = 0; i < rhs.size (); i++) {
                ret[i + 3] += rhs[i];
            }
            return ret;
        }

        /**
         * @brief subtract a Quaternion from the rotation of the transform
         * @param rhs [in] the right hand side value
         * @return result of devision
         */
        Transform3DVector< T > operator- (const Quaternion< T >& rhs) const
        {
            Transform3DVector< T > ret = *this;
            for (size_t i = 0; i < rhs.size (); i++) {
                ret[i + 3] -= rhs[i];
            }
            return ret;
        }

        /**
         * @brief element wise devide a Quaternion with the Quaternion rotation of the transform
         * @param rhs [in] the right hand side value
         * @return result of devision
         */
        Transform3DVector< T > operator/ (const Quaternion< T >& rhs) const
        {
            Transform3DVector< T > ret = *this;
            for (size_t i = 0; i < rhs.size (); i++) {
                ret[i + 3] /= rhs[i];
            }
            return ret;
        }

        /**
         * @brief element wise multiply a Quaternion with the Quaternion rotation of the transform
         * @param rhs [in] the right hand side value
         * @return result of multiplication
         */
        Transform3DVector< T > operator* (const Quaternion< T >& rhs) const
        {
            Transform3DVector< T > ret = *this;
            for (size_t i = 0; i < rhs.size (); i++) {
                ret[i + 3] *= rhs[i];
            }
            return ret;
        }

        // ########## Transform3DVector Operations

        /**
         * @brief element wise add two Transform3DVectors
         * @param rhs [in] the Transform3D vector to be added with
         * @return the sum of the two objects
         */
        Transform3DVector< T > operator+ (const Transform3DVector< T >& rhs) const
        {
            Transform3DVector< T > ret = *this;
            for (size_t i = 0; i < ret.size (); i++) {
                ret[i] += rhs[i];
            }
            return ret;
        }

        /**
         * @brief element wise subtract two Transform3DVectors
         * @param rhs [in] the Transform3D vector to be subtracted with
         * @return the difference of the two objects
         */
        Transform3DVector< T > operator- (const Transform3DVector< T >& rhs) const
        {
            Transform3DVector< T > ret = *this;
            for (size_t i = 0; i < ret.size (); i++) {
                ret[i] -= rhs[i];
            }
            return ret;
        }

        // ########## Scalar Operations

        /**
         * @brief Scalar multiplication
         * @param rhs [in] the scalar to multiply with
         * @return product of the multiplication
         */
        Transform3DVector< T > operator* (const T& rhs) const
        {
            return Transform3DVector< T > (this->_t3d * rhs);
        }

#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar multiplication
         * @param lhs [in] the scalar to multiply with
         * @param rhs [in] the Transform3DVector being multiplied with a scalar
         * @return product of the multiplication
         */
        friend Transform3DVector< T > operator* (const T& lhs, const Transform3DVector< T >& rhs)
        {
            return Transform3DVector< T > (lhs * rhs._t3d);
        }
#endif
        /**
         * @brief Scalar addition
         * @param rhs [in] the scalar to add
         * @return the sum
         */
        Transform3DVector< T > operator+ (const T& rhs) const
        {
            Transform3DVector< T > ret = *this;
            for (size_t i = 0; i < ret.size (); i++) {
                ret[i] += rhs;
            }
            return ret;
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar addition
         * @param lhs [in] the scalar to subtraction
         * @param rhs [in] the Transform3DVector being subtracted from
         * @return the difference
         */
        friend Transform3DVector< T > operator+ (const T& lhs, const Transform3DVector< T >& rhs)
        {
            Transform3DVector< T > ret = rhs;
            for (size_t i = 0; i < ret.size (); i++) {
                ret[i] += lhs;
            }
            return ret;
        }
#endif
        /**
         * @brief Scalar subtraction
         * @param rhs [in] the scalar to subtract
         * @return the difference
         */
        Transform3DVector< T > operator- (const T& rhs) const
        {
            Transform3DVector< T > ret = *this;
            for (size_t i = 0; i < ret.size (); i++) {
                ret[i] -= rhs;
            }
            return ret;
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar subtraction
         * @param lhs [in] the scalar to subtract
         * @param rhs [in] the Transform3DVector being subtracted from
         * @return the difference
         */
        friend Transform3DVector< T > operator- (const T& lhs, const Transform3DVector< T >& rhs)
        {
            Transform3DVector< T > ret = rhs;
            for (size_t i = 0; i < ret.size (); i++) {
                ret[i] = lhs - rhs[i];
            }
            return ret;
        }
#endif

        /**
         * @brief Scalar devision
         * @param rhs [in] the scalar to devide with
         * @return the result
         */
        Transform3DVector< T > operator/ (const T& rhs) const
        {
            return Transform3DVector< T > (this->_t3d / rhs);
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar devision
         * @param lhs [in] the scalar to devide with
         * @param rhs [in] the Transform3DVector being devided
         * @return the result
         */
        friend Transform3DVector< T > operator/ (const T& lhs, const Transform3DVector< T >& rhs)
        {
            Transform3DVector< T > ret = rhs;
            for (size_t i = 0; i < ret.size (); i++) {
                ret[i] = lhs / rhs[i];
            }
            return ret;
        }
#endif

        // ###################################################
        // #                Acces Operators                  #
        // ###################################################

#if !defined(SWIG)
        /**
         * @brief acces operator
         * @param i [in] index of the Vector
         * @return requested value
         */
        T& operator() (size_t i) { return _t3d[i]; }

        /**
         * @brief acces operator
         * @param i [in] index of the Vector
         * @return requested value
         */
        T operator() (size_t i) const { return _t3d[i]; }

        /**
         * @brief acces operator
         * @param i [in] index of the Vector
         * @return requested value
         */
        T& operator[] (size_t i) { return _t3d[i]; }

        /**
         * @brief acces operator
         * @param i [in] index of the Vector
         * @return requested value
         */
        T operator[] (size_t i) const { return _t3d[i]; }
#else
        ARRAYOPERATOR (T);

#endif
        /**
         * @brief get the size. Index 0-2 Vector, 3-6 Quaternion
         * @return always 7
         */
        size_t size () const { return 7u; }

        /**
         * @brief get the position Vector of the transform
         * @return a copy of the position vector
         */
        Vector3D< T > toVector3D () const { return Vector3D< T > (_t3d[0], _t3d[1], _t3d[2]); }

        /**
         * @brief get the Rotation of the transform
         * @return a copy of the quaternion rotation
         */
        Quaternion< T > toQuaternion () const
        {
            return Quaternion< T > (_t3d[3], _t3d[4], _t3d[5], _t3d[6]);
        }

        /**
         * @brief convert the rotation part to EAA
         * @return toration in EAA form
         */
        EAA< T > toEAA () const { return EAA< T > (this->toQuaternion ().toRotation3D ()); }

        /**
         * @brief Returns the corresponding Trandforma3D matrix
         * @return The transformation matrix
         */
        Transform3D< T > toTransform3D () const
        {
            return Transform3D< T > (this->toVector3D (), this->toQuaternion ());
        }

        /**
         * @brief get the underling eigen type
         * @return reference to eigen vector
         */
        Eigen::Matrix< T, 7, 1 >& e () { return _t3d; }
#if !defined(SWIG)
        friend std::ostream& operator<< (std::ostream& os, const Transform3DVector< T >& t)
        {
            return os << "Transform3DVector(" << t.toVector3D () << ", " << t.toQuaternion ()
                      << ")";
        }
#endif
        // ###################################################
        // #             assignement Operators               #
        // ###################################################

#if !defined(SWIGPYTHON)
        /**
         * @brief copy the transform
         * @param rhs the transform to copy
         * @return a copy of this object
         */
        Transform3DVector< T >& operator= (const Transform3DVector< T >& rhs)
        {
            this->_t3d = rhs._t3d;
            return *this;
        }
#endif
        /**
         * @brief add to the transform
         * @param rhs the transform to be added
         * @return a copy of this object
         */
        Transform3DVector< T >& operator+= (const Transform3DVector< T >& rhs)
        {
            this->_t3d = this->_t3d + rhs._t3d;
            return *this;
        }

        /**
         * @brief subtract from the transform
         * @param rhs the transform to be subtracted
         * @return a copy of this object
         */
        Transform3DVector< T >& operator-= (const Transform3DVector< T >& rhs)
        {
            this->_t3d = this->_t3d - rhs._t3d;
            return *this;
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief Override the Roation of the transform
         * @param rhs the new rotation
         * @return a copy of this object
         */
        Transform3DVector< T >& operator= (const Quaternion< T >& rhs)
        {
            *this = Transform3DVector< T > (this->toVector3D (), rhs);
            return *this;
        }
#endif
        /**
         * @brief add to the Roation of the transform
         * @param rhs the rotation to be added
         * @return a copy of this object
         */
        Transform3DVector< T >& operator+= (const Quaternion< T >& rhs)
        {
            *this = *this + rhs;
            return *this;
        }

        /**
         * @brief add to the Roation of the transform
         * @param rhs the rotation to be added
         * @return a copy of this object
         */
        Transform3DVector< T >& operator-= (const Quaternion< T >& rhs)
        {
            *this = *this - rhs;
            return *this;
        }

#if !defined(SWIGPYTHON)
        /**
         * @brief convert and copy the transform
         * @param rhs the transform to copy
         * @return a copy of this object
         */
        Transform3DVector< T >& operator= (const Transform3D< T >& rhs)
        {
            this->_t3d = Transform3DVector< T > (rhs)._t3d;
            return *this;
        }
#endif
#if !defined(SWIGPYTHON)
        /**
         * @brief Override the position of the transform
         * @param rhs the new position
         * @return a copy of this object
         */
        Transform3DVector< T >& operator= (const Vector3D< T >& rhs)
        {
            *this = Transform3DVector< T > (rhs, this->toQuaternion ());
            return *this;
        }
#endif
        /**
         * @brief add to the position of the transform
         * @param rhs the new position
         * @return a copy of this object
         */
        Transform3DVector< T >& operator+= (const Vector3D< T >& rhs)
        {
            *this = *this + rhs;
            return *this;
        }

        /**
         * @brief subtract from the position of the transform
         * @param rhs the new position
         * @return a copy of this object
         */
        Transform3DVector< T >& operator-= (const Vector3D< T >& rhs)
        {
            *this = *this - rhs;
            return *this;
        }
#if !defined(SWIGPYTHON)
        /**
         * @brief copy the transform from an eigen vector
         * @param rhs the transform to copy
         * @return a copy of this object
         */
        Transform3DVector< T >& operator= (const type& rhs)
        {
            this->_t3d = rhs;
            return *this;
        }
#endif
#if !defined(SWIG)
        /**
         * @brief implicit conversion to transform
         */
        operator Transform3D< T > () const { return this->toTransform3D (); }
#endif

      private:
        type _t3d;
    };

#if defined(SWIGPYTHON)

#endif

#if !defined(SWIG)
    extern template class rw::math::Transform3DVector< double >;
    extern template class rw::math::Transform3DVector< float >;

    using Transform3DVectord = Transform3DVector< double >;
    using Transform3DVectorf = Transform3DVector< float >;

#endif
    /**@}*/
}}    // namespace rw::math

#endif    // end include guard


// Standard C++ libraries
#include <iomanip>

// Boost libraries
#include <boost/property_tree/ptree.hpp>
#include <boost/math/tools/roots.hpp>

// Standard user-made libraries
#include <fparameters/SpaceIterator.h>
#include <fparameters/parameters.h>
#include <fluminosities/thermalSync.h>
#include <fluminosities/thermalBremss.h>
#include <fluminosities/luminosityHadronic.h>
#include <fluminosities/luminositySynchrotron.h>
#include <fluminosities/blackBody.h>
#include <fluminosities/reflection.h>
#include <fluminosities/probexact.h>
#include <fluminosities/luminosityNTHadronic.h>
#include <fmath/mathFunctions.h>
#include <fmath/physics.h>
#include <fmath/fbisection.h>
#include <fparameters/Dimension.h>
#include "absorption.h"
#include <fmath/RungeKutta.h>
// Project headers
#include "globalVariables.h"
#include "messages.h"
#include "read.h"
#include "thermalCompton.h"
#include "thermalProcesses.h"
#include "adafFunctions.h"
#include "write.h"

// Namespaces
using namespace std;

void localProcesses(State& st, Matrix& lumOutSy, Matrix& lumOutBr, Matrix& lumOutpp,
						Vector& energies, const int flags[], Matrix& lumOut)
{
	size_t jE=0;
	st.photon.ps.iterate([&](const SpaceIterator& itE) {
		energies[jE++] = itE.val(DIM_E);
	},{-1,0,0});

	#pragma omp parallel for
	for (size_t jE=0; jE<nE; jE++) {
		double frequency=energies[jE]/planck;
		double lumSync1,lumSync2,lumBremss1,lumBremss2;
		lumSync1 = lumSync2 = lumBremss1 = lumBremss2 = 0.0;
		size_t jR=0;
		st.photon.ps.iterate([&](const SpaceIterator& itER) {
			double r = itER.val(DIM_R);
			double thetaH = st.thetaH.get(itER);
			double rB1 = r/sqrt(paso_r);
			double rB2 = r*sqrt(paso_r);
			//double area = 4.0*pi*rB2*rB2*cos(thetaH);
			double area = 2.0*pi*( (height_method == 0) ? rB2*rB2*(2.0*cos(thetaH)+P2(sin(thetaH))) :
									2.0*rB2*height_fun(r)+(rB2*rB2-rB1*rB1) );
			double fluxToLum = area;
			double vol = volume(r);
			double emissToLum = vol*4.0*pi;
			double lumBr = 0.0;
			double lumSy = 0.0;
			double lumRJ = 0.0;

			double temp = st.tempElectrons.get(itER);
			double temp_i = st.tempIons.get(itER);
			double magf = st.magf.get(itER);
			double dens_i = st.denf_i.get(itER);
			double dens_e = st.denf_e.get(itER);
			double jSy = jSync(energies[jE],temp,magf,dens_e);
			if (flags[0]) {
				lumRJ = pi*bb(frequency,temp)*fluxToLum;
				lumSy = jSy*emissToLum;
			}
			if (flags[1])
				lumBr = jBremss(energies[jE],temp,dens_i,dens_e)*emissToLum;
			if (flags[2] && temp_i > 1.0e11 && frequency > 1.0e20 && frequency < 1.0e26) {
				double jpp = luminosityHadronic(energies[jE],dens_i,temp_i);
				lumOutpp[jE][jR] = jpp*vol;
			}
            if (flags[0]) {
				if (jR > 0)
					lumSync2 = min((1.0-scattAA[jR-1][jR])*lumSync1+lumSy,lumRJ);
				else
					lumSync2 = min(lumSy,lumRJ);
				if (lumSync2 > lumSync1)
					lumOutSy[jE][jR] = lumSync2-lumSync1;
				lumSync1 = lumSync2;
            }

			if (flags[1]) {
				if (flags[0] && frequency < 1.0e14) {
					if (lumBr > lumRJ)
						lumBr = 0.0;
				}
				lumOutBr[jE][jR]=lumBr;
			}

			lumOut[jE][jR] = lumOutSy[jE][jR]+lumOutBr[jE][jR]+lumOutpp[jE][jR];
			jR++;
		},{jE,-1,0});
	}
}

void localProcesses2(State& st, Matrix& lumOutSy, Matrix& lumOutBr,
						Vector& energies, const int flags[], Matrix& lumOut)
{
	size_t jE=0;
	st.photon.ps.iterate([&](const SpaceIterator& itE) {
		energies[jE++] = itE.val(DIM_E);
	},{-1,0,0});

	#pragma omp parallel for
	for (size_t jE=0;jE<nE;jE++) {
		size_t jR=0;
		st.photon.ps.iterate([&](const SpaceIterator& itER) {
			double r = itER.val(DIM_R);
			double rB2 = r*sqrt(paso_r);
			double rB1 = rB2/paso_r;
			double temp_e = st.tempElectrons.get(itER);
			double temp_i = st.tempIons.get(itER);
			double magf = st.magf.get(itER);
			double dens_i = st.denf_i.get(itER);
			double dens_e = st.denf_e.get(itER);
			double xSy,xBr;
			double unrEnergy = energies[jE]/redshift_to_inf[jR];
			unrEnergy = energies[jE];
			xSy = xBr = 0.0;
			if (flags[0])
				xSy = jSync(unrEnergy,temp_e,magf,dens_e)*4.0*pi;
			if (flags[1])
				xBr = jBremss(unrEnergy,temp_e,dens_i,dens_e)*4.0*pi;

			SpaceCoord psc = {jE, jR, 0};
			double b_nu = bb(unrEnergy/planck,temp_e);
			double kappa = (xSy+xBr)/(4.0*pi*b_nu);
			double tau = 0.5*sqrt(pi)*kappa*height_fun(r);
			double fluxSy = (2.0*sqrt(3.0)*tau > 1.0e-9) ?
							2.0*pi/sqrt(3.0)*b_nu*(1.0-exp(-2.0*sqrt(3.0)*tau)) :
							0.5*sqrt(pi)*xSy*height_fun(r);
			double fluxBr = (2.0*sqrt(3.0)*tau < 1.0e-3) ? 0.5*sqrt(pi)*xBr*height_fun(r) : 0.0;

			lumOutSy[jE][jR] = 2.0 * pi*(rB2*rB2-rB1*rB1) * fluxSy;
			lumOutBr[jE][jR] = 2.0 * pi*(rB2*rB2-rB1*rB1) * fluxBr;
			lumOut[jE][jR] = lumOutSy[jE][jR] + lumOutBr[jE][jR];
			jR++;
		},{jE,-1,0});
	}
}

void localProcesses3(State& st, Matrix& lumOutSy, Matrix& lumOutBr, Matrix& lumOutpp,
						Vector& energies, const int flags[], Matrix& lumOut)
{
	size_t jE=0;
	st.photon.ps.iterate([&](const SpaceIterator& itE) {
		energies[jE++] = itE.val(DIM_E);
	},{-1,0,0});

	#pragma omp parallel for
	for (int jE=0;jE<nE;jE++) {
		double frequency=energies[jE]/planck;
		size_t jR = 0.0;
		st.photon.ps.iterate([&](const SpaceIterator& itR) {
			double r = itR.val(DIM_R);
			double magf = st.magf.get(itR);
			double temp = st.tempElectrons.get(itR);
			double dens_e = st.denf_e.get(itR);
			double dens_i = st.denf_i.get(itR);

			double jv_th = jSync(energies[jE],temp,magf,dens_e)+jBremss(energies[jE],temp,dens_i,dens_e);
			//jv_th *= 4*pi;
			double jv_pl = luminositySynchrotron2(energies[jE],st.ntElectron,itR,magf)/frequency/(4*pi);
			double av_th = jv_th / bb(frequency,temp);
			SpaceCoord psc = {jE,jR,0};
			double av_pl = ssaAbsorptionCoeff(energies[jE],magf,st.ntElectron,psc);

			//double height = (r/schwRadius > 15) ? r*costhetaH(r) : height_fun(r);
			double height = r*costhetaH(r);
			double Sv = (jv_th+jv_pl)/(av_th+av_pl);
			//double Sv = jv_pl/(av_th+av_pl);
			double Inup = (frequency < 1.0e14) ? Sv * (1.0-exp(-2.0*height*(av_th+av_pl))) :
							2.0*height*(jv_th+jv_pl);
			//				2.0*height*jv_pl;
			double flux = 2.0*pi*r*r*(paso_r-1.0)*Inup;
			if (r/schwRadius <= 15)
				lumOutpp[jE][jR] = 4*pi*flux* (1.0-scattAA[jR][jR]);
			else
				lumOutBr[jE][jR] = 4*pi*flux* (1.0-scattAA[jR][jR]);
			lumOut[jE][jR] = lumOutpp[jE][jR]+lumOutBr[jE][jR];
			jR++;
		},{jE,-1,0});
	}
}

void reflectedSpectrum(Matrix lumOut, Matrix& lumOutRefl, Vector energies,
						double pasoE)
{
	size_t numInt = 40;
	for (size_t jE=nE/2;jE<nE;jE++) {
		double freq = energies[jE]/planck;
		double x = energies[jE]/(electronMass*cLight2);
		double xMax = 1.0e2;
		for (size_t jRcd=0;jRcd<nRcd;jRcd++) {
			double freq0,pasoFreq,aux1;
			int logicalInt = 1;
			if (x > 0.03 && x < xMax) {
				freq0 = energies[jE]/planck;
				pasoFreq = pow(xMax/x,1.0/(numInt+1));
				aux1 = 0.0;
			} else if (x <= 0.03) {
				freq0 = 0.03*electronMass*cLight2/planck;
				pasoFreq = pow(xMax/0.03,1.0/(numInt+1));
				aux1 = greenDeltaFunc(x);
			} else { logicalInt = 0; aux1 = 0.0; }

			double lum = 0.0;
			if (logicalInt == 1) {
				for (size_t jjFreq=0;jjFreq<numInt;jjFreq++) {
					double dfreq0 = freq0*(sqrt(pasoFreq)-pow(pasoFreq,-0.5));
					double lumInc = lumShellInc(freq0,jE,jRcd,lumOut,reachAD,energies,nR,redshift_RIAF_to_CD);
					double green = greenFuncRefl(freq,freq0);
					lum += green*lumInc*dfreq0*(freq/freq0);
					freq0 *= pasoFreq;
				}
			}
			lumOutRefl[jE][jRcd] =
					lum + aux1*lumShellInc(freq,jE,jRcd,lumOut,reachAD,energies,nR,redshift_RIAF_to_CD);

		}
	}
}

void coldDiskLuminosity(State& st, Matrix lumOut, Matrix& lumOutRefl,
						Matrix& lumOutCD, Vector energies)
{
	matrixInit(lumOutCD,nE,nRcd,0.0);
	matrixInit(lumOutRefl,nE,nRcd,0.0);

	//#pragma omp parallel for
	for (int jRcd=0;jRcd<nRcd;jRcd++) {
		double rCd = st.photon.ps[DIM_Rcd][jRcd];

		double lj1 = rCd/sqrt(paso_rCD);
		double lj2 = rCd*sqrt(paso_rCD);
		double area = 2.0*pi*(lj2*lj2-lj1*lj1);
		double aux1 = auxCD(rCd);
		double aux2 = 0.0;

		double pasoE = pow(st.photon.emax()/st.photon.emin(),1.0/(nE-1));
		//reflectedSpectrum(lumOut,lumOutRefl,energies,pasoE);

		for (size_t jjE=0;jjE<nE;jjE++) {
			double frequency = st.photon.ps[DIM_E][jjE]/planck;
			double dfreq = frequency*(sqrt(pasoE)-1.0/sqrt(pasoE));
			double lumInc = 0.0;
			for (size_t kR=0;kR<nR;kR++) {
				lumInc += (lumOut[jjE][kR]*reachAD[kR][jRcd] * pow(redshift_RIAF_to_CD[kR][jRcd],2));
			}
			aux2 += ((lumInc - lumOutRefl[jjE][jRcd])*dfreq);
		}
		double aux = aux1+aux2;
		double temp = pow(aux/area/ stefanBoltzmann,0.25);
		cout << "RadiusCD = " << rCd/schwRadius << " Temperature [K] = " << scientific << temp << endl;

		for(size_t jE=0;jE<nE;jE++) {
			double frequency = st.photon.ps[DIM_E][jE]/planck;
			lumOutCD[jE][jRcd] = area * pi * bb(frequency,1.7*temp)/pow(1.7,4);
		};
	}
}

void localCompton(const State& st, Matrix& lumOut, Matrix& lumOutIC, Vector energies)
{
	Matrix lumOutLocal;
	matrixInitCopy(lumOutLocal,nE,nR,lumOut);

	Vector comptonProbVec(nTempCompton*nNuPrimCompton*nNuCompton,0.0);
	if (calculateComptonRedMatrix) comptonMatrix();
	vectorRead("comptonProbMatrix.dat",comptonProbVec,comptonProbVec.size());

	// PARA CADA CELDA
	size_t jR=0;
	st.photon.ps.iterate([&](const SpaceIterator& itR) {

		double temp = st.tempElectrons.get(itR);
		double normtemp = boltzmann*temp/(electronMass*cLight2);
		if (normtemp >= tempMinCompton && normtemp < tempMaxCompton) {

			double eDens = st.denf_e.get(itR);
			double r = itR.val(DIM_R);
			double vol = volume(r);

			double tescap = 0.5*sqrt(pi)*height_fun(r)/cLight;
			Vector nOld(nE,0.0),nNew(nE,0.0),nWithoutCompton(nE,0.0);
			double tauu = eDens*thomson*height_fun(r);

			for (size_t jE=0;jE<nE;jE++) {
				nOld[jE] = lumOut[jE][jR]/vol / energies[jE] * tescap;
				nWithoutCompton[jE] = lumOut[jE][jR]/vol / energies[jE] * tescap;
			}

			double res = 0.0;
			size_t it=1;
			int cond = 1;
			do {
				cout << "jR = " << jR << "\t Iteration number = " << it << endl;
				cond = 0;

				for (size_t jE=0;jE<nE;jE++) {
					double frequency = energies[jE]/planck;
					if (frequency > nuMinCompton && frequency < nuMaxCompton) {
						double frequency = energies[jE]/planck;
						double rateScatt = rateThermal(normtemp,frequency)*eDens;
						double denom = 1.0/tescap + rateScatt;
						double integ = comptonNewLocal2(nOld,normtemp,jE,energies);
						nNew[jE] = (1.0/denom) * integ * eDens;
						if (nNew[jE] > nWithoutCompton[jE]) cond = 1;
					}
				}

				for (size_t jE=0;jE<nE;jE++) {
					nOld[jE] = nNew[jE];
					lumOut[jE][jR] += nNew[jE]*energies[jE]*vol / tescap;
					lumOutIC[jE][jR] = lumOut[jE][jR]-lumOutLocal[jE][jR];
				}
				cout << "Condition = " << cond << endl;
				++it;
			} while (cond == 1 && it < 40);
		}
		jR++;
	},{0,-1,0});

	show_message(msgEnd,Module_thermalCompton);
}

void thermalCompton2(State& st, Matrix& lumOut, Matrix& lumOutBr, Matrix& lumInICm,
						Matrix& lumOutIC, Matrix& lumOutIC_CD,
						Matrix& lumOutIC_Br, Vector energies, int processesFlags[])
{
	show_message(msgStart,Module_thermalCompton);

	Vector tempVec(nTempCompton,0.0);
	Vector nuPrimVec(nNuPrimCompton,0.0);
	Vector nuVec(nNuCompton,0.0);
	Vector comptonProbVec(nTempCompton*nNuPrimCompton*nNuCompton,0.0);

	if (comptonMethod == 0) {
		if (calculateComptonRedMatrix) comptonMatrix2();
		vectorRead("comptonProbMatrix2.dat",comptonProbVec,comptonProbVec.size());
		vectorRead("tempComptonVec.dat",tempVec,tempVec.size());
	} else {
		if (calculateComptonRedMatrix) comptonMatrix();
		vectorRead("comptonProbMatrix.dat",comptonProbVec,comptonProbVec.size());
	}

	vectorRead("nuPrimComptonVec.dat",nuPrimVec,nuPrimVec.size());
	vectorRead("nuComptonVec.dat",nuVec,nuVec.size());

	Matrix lumOutLocal,lumOutLocal_Br,lumOutCD,lumOutRefl,lumOutIC_CD_copy, lumOutIC_Br_copy;
	matrixInit(lumOutCD, nE, nRcd, 0.0);
	matrixInit(lumOutRefl, nE, nRcd, 0.0);
	matrixInit(lumOutIC, nE, nR, 0.0);
	matrixInit(lumOutIC_CD, nE, nR, 0.0);
	matrixInit(lumOutIC_CD_copy, nE, nR, 0.0);
	matrixInit(lumOutIC_Br_copy, nE, nR, 0.0);
	Vector lumInIC(nE, 0.0);
	Vector lumInIC_CD(nE, 0.0);
	Vector lumInIC_Br(nE, 0.0);
	matrixInitCopy(lumOutLocal, nE, nR, lumOut);
	Vector p(nR*nE*nE, 0.0);

	Vector a(nR, 1.0);
	if (comptonMethod == 1)
		cNew(st, p, a);
		//cNew(st, p, redshift_to_inf);

	if (processesFlags[3])
		coldDiskLuminosity(st, lumOut, lumOutRefl, lumOutCD, energies);

	double res;		// Residual.
	size_t it=1;	// Iterations.
	do {
		cout << "Iteration number = " << it << endl;
		res=0.0;

		// To compute the residuals.
		Vector lumOld(nE,0.0);
		for (size_t jE=0;jE<nE;jE++) {
			for (size_t jR=0;jR<nR;jR++) {
				lumOld[jE] += lumOut[jE][jR] + lumOutIC_CD[jE][jR];
			}
			if (processesFlags[3]) {
				for (size_t jRcd=0;jRcd<nRcd;jRcd++) {
					lumOld[jE] += lumOutCD[jE][jRcd] + lumOutRefl[jE][jRcd];
				}
			}
		}

		matrixInit(lumOutIC,nE,nR,0.0);
		matrixInit(lumOutIC_Br,nE,nR,0.0);
		matrixInit(lumOutIC_CD,nE,nR,0.0);

		// For each shell.
		size_t jR=0;
		st.photon.ps.iterate([&](const SpaceIterator& itR) {
			double normtemp = boltzmann*st.tempElectrons.get(itR)/(electronMass*cLight2);
			// Compute the scattered luminosity.
			fill(lumInIC.begin(),lumInIC.end(),0.0);
			fill(lumInIC_CD.begin(),lumInIC_CD.end(),0.0);
			fill(lumInIC_Br.begin(),lumInIC_Br.end(),0.0);

			for (size_t jjE=0;jjE<nE;jjE++) {
				for (size_t jjR=0;jjR<nR;jjR++) {
					Vector lumVec(nE,0.0);
					//lumInIC[jjE] += scattAA[jjR][jR] * lumOut[jjE][jjR] * pow(redshift[jjR][jR],2);
					for (size_t jjjE=0;jjjE<nE;jjjE++)
						lumVec[jjjE] = lumOut[jjjE][jjR];

					double localEnergy = energies[jjE] / redshift[jjR][jR];
					lumInIC[jjE] += scattAA[jjR][jR] * pow(redshift[jjR][jR],2)
										* lumInterp(lumVec,energies,jjE,nE,localEnergy);
				}
				for (size_t jjR=0;jjR<nR;jjR++) {
					Vector lumVec(nE,0.0);
					//lumInIC[jjE] += scattAA[jjR][jR] * lumOut[jjE][jjR] * pow(redshift[jjR][jR],2);
					for (size_t jjjE=0;jjjE<nE;jjjE++)
						lumVec[jjjE] = lumOutBr[jjjE][jjR] + lumOutIC_Br_copy[jjjE][jjR];

					double localEnergy = energies[jjE] / redshift[jjR][jR];
					lumInIC_Br[jjE] += scattAA[jjR][jR] * pow(redshift[jjR][jR],2)
										* lumInterp(lumVec,energies,jjE,nE,localEnergy);
				}
				if (processesFlags[3]) {
					for (size_t jRcd=0;jRcd<nRcd;jRcd++) {
						Vector lumVec(nE,0.0);
						for (size_t jjjE=0;jjjE<nE;jjjE++)
							lumVec[jjjE] = lumOutCD[jjjE][jRcd]+lumOutRefl[jjjE][jRcd];

						double localEnergy = energies[jjE] / redshift_CD_to_RIAF[jRcd][jR];
						lumInIC_CD[jjE] += scattDA[jRcd][jR] * pow(redshift_CD_to_RIAF[jRcd][jR],2) *
											lumInterp(lumVec,energies,jjE,nE,localEnergy);

					}
					for (size_t jjR=0;jjR<nR;jjR++) {
						Vector lumVec(nE,0.0);
						for (size_t jjjE=0;jjjE<nE;jjjE++)
							lumVec[jjjE] = lumOutIC_CD_copy[jjjE][jjR];

						double localEnergy = energies[jjE] / redshift[jjR][jR];
						lumInIC_CD[jjE] += scattAA[jjR][jR] * pow(redshift[jjR][jR],2)
										* lumInterp(lumVec,energies,jjE,nE,localEnergy);
					}
				}
				lumInICm[jjE][jR] = lumInIC[jjE] + lumInIC_CD[jjE];
			}
			if (normtemp > tempMinCompton && normtemp < tempMaxCompton) {

				for (size_t jE=0;jE<nE;jE++) {
					double frequency = energies[jE]/planck;
					if (comptonMethod == 3) {
						if (it == 1)
							comptonNewNewNewPruebaVector(jR,energies,jE,p,normtemp);
						if (frequency > nuMinCompton && frequency < nuMaxCompton)
							lumOutIC[jE][jR] = compton(p,lumInIC,jR,energies,jE);
							lumOutIC_CD[jE][jR] = compton(p,lumInIC_CD,jR,energies,jE);
							lumOutIC_Br[jE][jR] = compton(p,lumInIC_Br,jR,energies,jE);
					} else {
						if (frequency > nuMinCompton && frequency < nuMaxCompton)
							lumOutIC[jE][jR] = compton(p,lumInIC,jR,energies,jE);
							lumOutIC_CD[jE][jR] = compton(p,lumInIC_CD,jR,energies,jE);
							lumOutIC_Br[jE][jR] = compton(p,lumInIC_Br,jR,energies,jE);
					}
				}
			}
			jR++;
		},{0,-1,0});

		double pasoNuPrim = pow(energies[nE-1]/energies[0],1.0/(nE-1));
		double nPhNS = 0.0;
		double nPhBS = 0.0;
		double nPhAS = 0.0;
		for (size_t jE=0;jE<nE;jE++) {
			double dNu = energies[jE]/planck * (sqrt(pasoNuPrim)-1.0/sqrt(pasoNuPrim));
			double lumNew = 0.0;
			for (size_t jR=0;jR<nR;jR++) {
				nPhNS += lumOutLocal[jE][jR]/energies[jE] * dNu;
				nPhBS += lumInICm[jE][jR]/energies[jE] * dNu;
				nPhAS += lumOutIC[jE][jR]/energies[jE] * dNu;
				double lumAux = lumOutLocal[jE][jR]+lumOutIC[jE][jR];
				lumNew += (lumAux + lumOutIC_CD[jE][jR]);
				lumOut[jE][jR] = lumAux;
				lumOutIC_CD_copy[jE][jR] = lumOutIC_CD[jE][jR];
				lumOutIC_Br_copy[jE][jR] = lumOutIC_Br[jE][jR];
			}
			for (size_t jRcd=0;jRcd<nRcd;jRcd++) {
				lumNew += (lumOutCD[jE][jRcd]+lumOutRefl[jE][jRcd]);
			}
			if (lumNew > 0.0 && lumOld[jE] > 0.0)
				res += abs(log10(lumNew/lumOld[jE]));
		}
		cout << endl;
		cout << "Total photons non-scattered per unit time = "
						<< nPhNS << " s^-1" << endl;
		cout << "Total photons scattered per unit time, before scattering = "
						 << nPhBS << " s^-1" << endl;
		cout << "Total photons scattered per unit time, after scattering = "
						 << nPhAS << " s^-1" << endl;
		cout << endl;

		res /= nE;
		cout << "Residuo = " << res << endl;
		++it;
	} while (res > 1.0e-3 && it < 100);

	for (size_t jE=0;jE<nE;jE++)
		for (size_t jR=0;jR<nR;jR++)
			lumOut[jE][jR] += lumOutIC_CD[jE][jR];

	show_message(msgEnd,Module_thermalCompton);
}

/*
void thermalCompton(State& st, Matrix& lumOut, Matrix& lumInICm, Matrix& lumOutIC, Vector energies,
						int processesFlags[])
{
	show_message(msgStart,Module_thermalCompton);
	Vector tempVec(nTempCompton,0.0);
	Vector nuPrimVec(nNuPrimCompton,0.0);
	Vector nuVec(nNuCompton,0.0);
	Vector comptonProbVec(nTempCompton*nNuPrimCompton*nNuCompton,0.0);

	if (comptonMethod == 0) {
		if (calculateComptonRedMatrix) comptonMatrix2();
		vectorRead("comptonProbMatrix2.dat",comptonProbVec,comptonProbVec.size());
		vectorRead("tempComptonVec.dat",tempVec,tempVec.size());
	} else {
		if (calculateComptonRedMatrix) comptonMatrix();
		vectorRead("comptonProbMatrix.dat",comptonProbVec,comptonProbVec.size());
	}

	vectorRead("nuPrimComptonVec.dat",nuPrimVec,nuPrimVec.size());
	vectorRead("nuComptonVec.dat",nuVec,nuVec.size());

	Matrix lumOutCD,lumOutRefl,lumOutIC_local;
	matrixInit(lumOutCD,nE,nRcd,0.0);
	matrixInit(lumOutRefl,nE,nRcd,0.0);
	Vector lumInIC(nE,0.0);
	Vector lumBeforeCompton(nE,0.0);
	matrixInitCopy(lumOutIC_local,nE,nR,lumOut);
	Vector p(nR*nE*nE,0.0);

	for (size_t jE=0;jE<nE;jE++)
		for (size_t jR=0;jR<nR;jR++)
			lumBeforeCompton[jE] += lumOut[jE][jR];

	if (comptonMethod == 1) cNew(st,p,redshift_to_inf);

	int cond = 1;		// Residual.
	size_t it=1;	// Iterations.
	do {
		cout << "Iteration number = " << it << endl;
		cond = 0;
		if (processesFlags[3])
			coldDiskLuminosity(st,lumOut,lumOutRefl,lumOutCD,energies);

		Vector lumCompton(nE,0.0);

		// For each shell.
		size_t jR=0;
		st.photon.ps.iterate([&](const SpaceIterator& itR) {
			double normtemp = boltzmann*st.tempElectrons.get(itR)/(electronMass*cLight2);
			// Compute the scattered luminosity.
			fill(lumInIC.begin(),lumInIC.end(),0.0);
			for (size_t jjE=0;jjE<nE;jjE++) {

				for (size_t jjR=0;jjR<nR;jjR++)
					lumInIC[jjE] += scattAA[jjR][jR] * lumOutIC_local[jjE][jjR];

				if (processesFlags[3]) {
					for (size_t jRcd=0;jRcd<nRcd;jRcd++)
						lumInIC[jjE] += scattDA[jRcd][jR] *
											(lumOutCD[jjE][jRcd]+lumOutRefl[jjE][jRcd]);
				}
				lumInICm[jjE][jR] = lumInIC[jjE];
			}

			for (size_t jE=0;jE<nE;jE++)
				lumOutIC_local[jE][jR] = 0.0;

			if (normtemp > tempMinCompton && normtemp < tempMaxCompton) {
				double A = 1.0+4.0*normtemp*(1.0+4.0*normtemp);
				Vector unrEnergies_vec(nE);
				for (size_t jE=0;jE<nE;jE++)
					unrEnergies_vec[jE] = energies[jE]/redshift_to_inf[jR];

				for (size_t jE=0;jE<nE;jE++) {
					double frequency = energies[jE]/planck;
					double unrFrequency = unrEnergies_vec[jE]/planck;
					if (unrFrequency > nuMinCompton && unrFrequency < nuMaxCompton) {
						lumOutIC_local[jE][jR] = compton(p,lumInIC,jR,unrEnergies_vec,jE);
						lumCompton[jE] += lumOutIC_local[jE][jR];
					}
				}
			}
			jR++;
		},{0,-1,0});

		for (size_t jE=0;jE<nE;jE++)
			if (lumCompton[jE] > 0.01*lumBeforeCompton[jE]) cond = 1;

		double pasoNuPrim = pow(energies[nE-1]/energies[0],1.0/(nE-1));
		double nPhNS = 0.0;
		double nPhBS = 0.0;
		double nPhAS = 0.0;
		for (size_t jE=0;jE<nE;jE++) {
			double dNu = energies[jE]/planck * (sqrt(pasoNuPrim)-1.0/sqrt(pasoNuPrim));
			for (size_t jR=0;jR<nR;jR++) {
				nPhBS += lumInICm[jE][jR]/energies[jE] * dNu;
				nPhAS += lumOutIC_local[jE][jR]/energies[jE] * dNu;
				lumOut[jE][jR] += lumOutIC_local[jE][jR];
				lumOutIC[jE][jR] += lumOutIC_local[jE][jR];
			}
		}
		cout << endl;
		cout << "Total photons scattered per unit time, before scattering = "
					 << nPhBS << " s^-1" << endl;
		cout << "Total photons scattered per unit time, after scattering = "
					 << nPhAS << " s^-1" << endl;
		cout << endl;

		cout << "Condition = " << cond << endl;
		++it;
	} while (cond);
	show_message(msgEnd,Module_thermalCompton);
}
*/

void writeLuminosities(State& st, Vector energies, Matrix lumOutSy, Matrix lumOutBr,
						Matrix lumOutpp, Matrix lumInICm, Matrix lumOutIC, Matrix lumOutIC_CD,
						Matrix lumOutIC_Br,
						Matrix lumOut, Matrix lumOutCD, Matrix lumOutRefl, const string& filename)
{
	ofstream file1,file2;
	ofstream fileCell;
	file1.open(filename.c_str(),ios::out);
	file2.open("lumRadius.dat",ios::out);
	fileCell.open("lumCell.txt",ios::out);

	double lumSy,lumBr,lumICin,lumIC,lumIC_CD,lumIC_Br,lumpp,lumTot,lumCD,lumRefl;
	double lumThermalTot = 0.0;
	double lumThermalTotRIAF = 0.0;
	double pasoF = pow(energies[nE-1]/energies[0],1.0/(nE-1));
	for (size_t jE=0;jE<nE;jE++) {
		double E = energies[jE];
		double frequency = E/planck;
		double energyEV = energies[jE]/EV_TO_ERG;
		lumSy = lumBr = lumICin = lumIC = lumIC_CD = lumIC_Br = lumpp = lumTot = lumCD = lumRefl = 0.0;
		for (size_t jR=0;jR<nR;jR++) {
			Vector lumSyVec(nE,0.0), lumBrVec(nE,0.0), lumICinVec(nE,0.0), lumICVec(nE,0.0),
					lumIC_CDVec(nE,0.0), lumIC_BrVec(nE,0.0), lumppVec(nE,0.0), lumOutVec(nE,0.0);
			for (size_t jjE=0;jjE<nE;jjE++) {
				lumSyVec[jjE] = lumOutSy[jjE][jR];
				lumBrVec[jjE] = lumOutBr[jjE][jR];
				lumICinVec[jjE] = lumInICm[jjE][jR];
				lumICVec[jjE] = lumOutIC[jjE][jR];
				lumIC_CDVec[jjE] = lumOutIC_CD[jjE][jR];
				lumIC_BrVec[jjE] = lumOutIC_Br[jjE][jR];
				lumppVec[jjE] = lumOutpp[jjE][jR];
				lumOutVec[jjE] = lumOut[jjE][jR];
			}
			double localEnergy = energies[jE]/redshift_to_inf[jR];
			lumSy += lumInterp(lumSyVec,energies,jE,nE,localEnergy) * pow(redshift_to_inf[jR],3) * escapeAi[jR];
			lumBr += lumInterp(lumBrVec,energies,jE,nE,localEnergy) * pow(redshift_to_inf[jR],3) * escapeAi[jR];
			lumICin += lumInterp(lumICinVec,energies,jE,nE,localEnergy) * pow(redshift_to_inf[jR],3) * escapeAi[jR];
			lumIC += lumInterp(lumICVec,energies,jE,nE,localEnergy) * pow(redshift_to_inf[jR],3) * escapeAi[jR];
			lumIC_CD += lumInterp(lumIC_CDVec,energies,jE,nE,localEnergy) * pow(redshift_to_inf[jR],3) * escapeAi[jR];
			lumIC_Br += lumInterp(lumIC_BrVec,energies,jE,nE,localEnergy) * pow(redshift_to_inf[jR],3) * escapeAi[jR];

			lumpp += lumInterp(lumppVec,energies,jE,nE,localEnergy) * pow(redshift_to_inf[jR],3) * escapeAi[jR];
			lumTot += lumInterp(lumOutVec,energies,jE,nE,localEnergy) * pow(redshift_to_inf[jR],3) * escapeAi[jR];
		}
		for (size_t jRcd=0;jRcd<nRcd;jRcd++) {
			double localEnergy = energies[jE]/redshift_CD_to_inf[jRcd];
			Vector lumCDVec(nE,0.0), lumReflVec(nE,0.0);
			for (size_t jjE=0;jjE<nE;jjE++) {
				lumCDVec[jjE] = lumOutCD[jjE][jRcd];
				lumReflVec[jjE] = lumOutRefl[jjE][jRcd];
			}
			lumCD += lumInterp(lumCDVec,energies,jE,nE,localEnergy) * pow(redshift_CD_to_inf[jRcd],3) * escapeDi[jRcd];
			lumRefl += lumInterp(lumReflVec,energies,jE,nE,localEnergy) * pow(redshift_CD_to_inf[jRcd],3) * escapeDi[jRcd];
		}
		double dfreq = frequency*(pasoF-1.0);
		lumThermalTot += (lumTot + lumCD + lumRefl)*dfreq;
		lumThermalTotRIAF += lumTot*dfreq;
        file1
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << frequency
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << energyEV
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumSy*frequency
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumBr*frequency
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumIC*frequency
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumpp*frequency
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumCD*frequency
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumRefl*frequency
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumTot*frequency
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumICin*frequency
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumIC_CD*frequency
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumIC_Br*frequency
			<< endl;
	};
	cout << "Total thermal luminosity (power) = " << lumThermalTot << endl;

	double lumThermal = 0.0;
	for (size_t jR=0;jR<nR;jR++) {
		double r = st.denf_i.ps[DIM_R][jR]/schwRadius;
		Vector lumVec(nE,0.0);
		for (size_t jjE=0;jjE<nE;jjE++)
			lumVec[jjE] = lumOut[jjE][jR];

		for (size_t jE=0;jE<nE;jE++) {
			double E = energies[jE];
			double localEnergy = E / redshift_to_inf[jR];
			double frequency = E/planck;
			double dfreq = frequency*(pasoF-1.0);
			lumThermal += lumInterp(lumVec,energies,jE,nE,localEnergy)*dfreq*escapeAi[jR] * pow(redshift_to_inf[jR],3);
		}
		cout << "percentage of the lum produced inside r = " << r*sqrt(paso_r)
			 << " equal to " << lumThermal/lumThermalTotRIAF * 100 << " %" << endl;
	};

	double eVar = pow(energies[nE-1]/energies[0],1.0/(nE-1));
	size_t jR=0;
	st.photon.ps.iterate([&](const SpaceIterator& itR) {
		double r = itR.val(DIM_R)/schwRadius;
		double vol = volume(itR.val(DIM_R));
		lumSy = lumBr = lumICin = lumIC = lumpp = lumTot = 0.0;
		for (size_t jE=0;jE<nE;jE++)  {
			double frequency = energies[jE]/planck;
			double dfrequency = frequency * (eVar-1.0);
			lumSy += lumOutSy[jE][jR]*dfrequency;
			lumBr += lumOutBr[jE][jR]*dfrequency;
			lumICin += lumInICm[jE][jR]*dfrequency;
			lumIC += lumOutIC[jE][jR]*dfrequency;
			lumpp += lumOutpp[jE][jR]*dfrequency;
			lumTot += lumOut[jE][jR]*dfrequency;
			fileCell << log10(r) << "\t" << log10(energies[jE]/planck)
					 << "\t" << safeLog10(lumOut[jE][jR]*frequency) << endl;
		}

		file2
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << r
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << vol
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumSy
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumBr
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumIC
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumpp
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumTot
			<< setw(10) << setiosflags(ios::fixed) << scientific << setprecision(2) << lumICin
			<< endl;
		jR++;
	},{0,-1,0});

	file1.close();
	file2.close();
	fileCell.close();
}

void photonDensity(State& st, Vector energies, Matrix lumOut)
{
	ofstream file;
	file.open("photonDensity_gap.dat",ios::out);

	size_t nZ = nR;
	double zMin = schwRadius;
	double pasoZ = pow(1e6,1.0/nZ);
	double z = zMin;
	st.photon.ps.iterate([&](const SpaceIterator& iZ) {
		z *= pasoZ;
		double Uph = 0.0;
		size_t jE=0;
		double pasoE = pow(energies.back()/energies.front(),1.0/(energies.size()-1));
		st.photon.ps.iterate([&](const SpaceIterator& iZE) {
			double nPh = 0.0;
			double energy = iZE.val(DIM_E);
			double dE = energy * (pasoE - 1.0);
			size_t jR=0;
			st.photon.ps.iterate([&](const SpaceIterator& iZER) {
				double r = iZER.val(DIM_R);
				double dist2 = z*z+r*r;
				nPh += ( lumOut[jE][jR] / (4*pi*dist2*cLight*energy*planck) );
				jR++;
			},{iZE.coord[DIM_E],-1,0});
			Uph += nPh * energy * dE;
			file << z/schwRadius << "\t" << energy << "\t" << nPh << endl;
			st.photon.injection.set(iZE,nPh);
			jE++;
		},{-1,iZ.coord[DIM_R],0});
		double Ub = P2(magneticField(st.denf_e.ps[DIM_R][0])/pow(z/schwRadius,1))/(8*pi);
		cout << "z = " << z/schwRadius << " Rs,\t Uph = " << Uph << " erg cm^-3,\t Uph/Ub = " << Uph/Ub << endl;
	},{0,-1,0});
	file.close();
}

void photonDensityAux(State& st, Vector energies, Matrix lumOut)
{
	Vector z(50,GlobalConfig.get<double>("zGap")*schwRadius);
	double zMax = z[0]*1000;
	double pasoZ = pow(zMax/z[0],1.0/50);
	for (size_t i=1;i<50;i++) z[i] = z[i-1]*pasoZ;

	ofstream file;
	file.open("photonDensity_z.dat",ios::out);

	for (size_t i=1;i<50;i++) {
		double Uph = 0.0;
		size_t jE=0;
		double pasoE = pow(energies.back()/energies.front(),1.0/(energies.size()-1));
		st.photon.ps.iterate([&](const SpaceIterator& itE) {
			double nPh = 0.0;
			double energy = itE.val(DIM_E);
			double dE = energy * (pasoE - 1.0);
			size_t jR=0;
			st.photon.ps.iterate([&](const SpaceIterator& itER) {
				double r = itER.val(DIM_R);
				double dist2 = z[i]*z[i]+r*r;
				nPh += ( lumOut[jE][jR] / (4*pi*dist2*cLight*energy*planck) );
				jR++;
			},{itE.coord[DIM_E],-1,0});
			Uph += nPh * dE;
			jE++;
		},{-1,0,0});
		file << z[i]/schwRadius << "\t" << Uph << endl;
	}
	file.close();
}

void targetField(State& st, Matrix lumOut, Matrix lumCD, Matrix lumRefl, Vector energies)
{
	size_t jE=0;
	st.photon.ps.iterate([&](const SpaceIterator& itE) {
		size_t jR=0;
		double E = itE.val(DIM_E);
		st.photon.ps.iterate([&](const SpaceIterator& itER) {
			double r = itER.val(DIM_R);
			double rB2 = r*sqrt(paso_r);
			double rB1 = rB2/paso_r;
			double vol = volume(r);
			double lumReachingShell = 0.0;
			double height = height_fun(r);
			double tau_es_1 = st.denf_e.get(itER)*thomson*height;
			double tau_es_2 = tau_es_1 * (rB2-rB1)/height;
			double tescape = height/cLight * (1.0 + tau_es_1);
			double tcross = (rB2-rB1)/cLight * (1.0 + tau_es_2);

			for (size_t jjR=0;jjR<nR;jjR++) {
				Vector lumVec(nE,0.0);
				for (size_t jjE=0;jjE<nE;jjE++)
					lumVec[jjE] = lumOut[jE][jjR];

				double localEnergy = E / redshift[jjR][jR];
				double lumLocal = lumInterp(lumVec,energies,jE,nE,localEnergy);
				lumReachingShell += ( (jjR == jR) ? lumOut[jE][jR]*tescape :
									reachAA[jjR][jR]*pow(redshift[jjR][jR],2)*lumLocal*tcross );
			}
			for (size_t jjRcd=0;jjRcd<nRcd;jjRcd++) {
				Vector lumVec(nE,0.0);
				for (size_t jjE=0;jjE<nE;jjE++)
					lumVec[jjE] = lumCD[jE][jjRcd] + lumRefl[jE][jjRcd];

				double localEnergy = E / redshift_CD_to_RIAF[jjRcd][jR];
				lumReachingShell += reachDA[jjRcd][jR]*pow(redshift_CD_to_RIAF[jjRcd][jR],2) *
									lumInterp(lumVec,energies,jE,nE,localEnergy)*tcross;
			}
			st.photon.distribution.set(itER,lumReachingShell/(vol*planck*E)); //erg^⁻1 cm^-3 */
			//for (size_t jjR=0;jjR<nR;jjR++)
			//	lumReachingShell += reachAA[jjR][jR]*pow(redshift[jjR][jR],2)*lumOut[jE][jjR];
			//for (size_t jjRcd=0;jjRcd<nRcd;jjRcd++)
			//	lumReachingShell += reachDA[jjRcd][jR]*pow(redshift_CD_to_RIAF[jjRcd][jR],2)*
			//							(lumCD[jE][jjRcd]+lumRefl[jE][jjRcd]);
			//st.photon.distribution.set(itER, lumReachingShell/(4.0*pi*r*height*planck*E*cLight)); //erg^⁻1 cm^-3
			jR++;
		},{itE.coord[DIM_E],-1,0});
		jE++;
	},{-1,0,0});
}

void absorptionLumThermal(State& st, Matrix lumOut, Matrix lumCD, Matrix lumRefl, Matrix& lumOut_gg,
							Vector energies)
{
	int cond = 1;
	Matrix lumOut_gg_aux;
	matrixInitCopy(lumOut_gg,nE,nR,lumOut);
	matrixInitCopy(lumOut_gg_aux,nE,nR,lumOut);
	int it = 0;
	do {
		cond = 0;
		targetField(st,lumOut_gg,lumCD,lumRefl,energies);
		st.photon.ps.iterate([&](const SpaceIterator& iR) {
			double height = height_fun(iR.val(DIM_R));
			for (size_t jE=0;jE<nE;jE++) {
				double E = st.photon.ps[DIM_E][jE];
				double kappa_gg = integSimpsonLog(st.photon.emin(),st.photon.emax(),
						[&E,&iR,&st](double Eph)
						{
							if (Eph*E > P2(electronRestEnergy))
								return st.photon.distribution.interpolate({{0,Eph}},&iR.coord)*
										ggCrossSection2(E,Eph);
							else
								return 0.0;
						},50);
				double tau_gg = 0.5*sqrt(pi)*kappa_gg*height;
				double factor_gg = (tau_gg > 1.0e-5) ?
									(1.0-exp(-2*sqrt(3.0)*tau_gg))/(2.0*sqrt(3.0)*tau_gg) : 1.0;
				double lum = lumOut[jE][iR.coord[DIM_R]] * factor_gg;
				lumOut_gg[jE][iR.coord[DIM_R]] = lum;
				if (lum > 0.0 && lumOut[jE][iR.coord[DIM_R]] > 0.0)
					if (abs(safeLog10(lum/lumOut_gg_aux[jE][iR.coord[DIM_R]])) > 0.01) cond = 1;
				lumOut_gg_aux[jE][iR.coord[DIM_R]] = lum;
			}
		},{0,-1,0});
		it++;
		cout << "Iteration number " << it << "." << endl;
	} while (cond && it <= 20);
	cout << "Exit in " << it << " iterarations." << endl;
}
/*
void calculateElectronTemp(State& st, Matrix lumOut, Matrix lumInICm, Vector energies, double& res)
{
	ofstream fileNewTemp_e, fileCooling;
	fileNewTemp_e.open("newTempElectrons.txt");
	fileCooling.open("coolingElectrons.txt");
	double eVar = pow(energies[nE-1]/energies[0],1.0/(nE-1));
	size_t newDim = 10000;
	Vector logTe2(newDim,logTe.back());
	double paso_r_new = pow(st.denf_e.ps[DIM_R][nR-1]/st.denf_e.ps[DIM_R][1],1.0/(newDim-1));
	double r = st.denf_e.ps[DIM_R][nR-1];
	double dlogr = log(paso_r_new);
	size_t kR = 1;
	while (r > st.denf_e.ps[DIM_R][1]) {
		double Qmin = Qmin_func(r,lumOut,lumInICm,energies,st);
		double v = radialVel(r);
		double Te = exp(logTe2[kR-1]);
		double Ti = ionTemp(r);
		double pe = massDensityADAF(r) * boltzmann * Te / (atomicMassUnit*eMeanMolecularWeight);
		double Qie = qie_beta(r,Ti,Te);
		double Qp = Qplus(r,Ti,Te);
		double Qs = r/(pe*v) * (delta*Qp + Qie - Qmin);//*min(P3(Te/electronTemp(r)),1.0));
		double normtemp = boltzmann*Te / electronRestEnergy;
		double a_aux = 3.0-6.0/(4.0+5.0*normtemp) + normtemp * 30.0/P2(4.0+5.0*normtemp);
		double dlogrhodlogr = dlogrho_dlogr(r);
		double func = (Qs+dlogrhodlogr) / a_aux;

		while (abs(func*dlogr) > 0.01) {
			paso_r_new = pow(paso_r_new,1.0/10);
			dlogr = log(paso_r_new);
		}
		logTe2[kR] = min(log(min(electronTempOriginal(r),electronTemp(r))),
						logTe2[kR-1] - func * dlogr);
		//logTe2[kR] = logTe2[kR-1] - func *dlogr;
		fileNewTemp_e << log10(r/schwRadius) << "\t" << log10(exp(logTe2[kR])) << endl;
		fileCooling << log10(r/schwRadius) << "\t" << Qp << "\t" << Qie << "\t" << Qmin << endl;

		//cout << kR << endl;
		kR++;
		r /= paso_r_new;
		paso_r_new = pow(paso_r_new,10);
		dlogr = log(paso_r_new);
	}
	fileNewTemp_e.close();
	fileCooling.close();
}*/

void thermalRadiation(State& st, const string& filename)
{
	show_message(msgStart,Module_thermalLuminosities);

	Matrix lumOutSy,lumOutBr,lumOutpp,lumInICm,lumOutIC,lumOutIC_CD,lumOutIC_Br,
			lumOut,lumOutCD,lumOutRefl,lumOut_gg;
	Matrix lumSy,lumBr,lumIC,lum;
	matrixInit(lumOutSy,nE,nR,0.0);
	matrixInit(lumOutBr,nE,nR,0.0);
	matrixInit(lumOutpp,nE,nR,0.0);
	matrixInit(lumInICm,nE,nR,0.0);
	matrixInit(lumOutIC,nE,nR,0.0);
	matrixInit(lumOutIC_CD,nE,nR,0.0);
	matrixInit(lumOutIC_Br,nE,nR,0.0);
	matrixInit(lumOutCD,nE,nR,0.0);
	matrixInit(lumOutRefl,nE,nR,0.0);
	matrixInit(lumOut,nE,nR,0.0);
	matrixInit(lumOut_gg,nE,nR,0.0);

	Vector energies(nE,0.0);

	int processesFlags[numProcesses];	readThermalProcesses(processesFlags);
	if (processesFlags[0] || processesFlags[1] || processesFlags[2] || processesFlags[3]) {
		double res = 0.0;
		do {
			if (processesFlags[0] || processesFlags[1] || processesFlags[2])
				if (height_method == 0)
					localProcesses(st,lumOutSy,lumOutBr,lumOutpp,energies,processesFlags,lumOut);
				else {
					localProcesses2(st,lumOutSy,lumOutBr,energies,processesFlags,lumOut);
				}
			if (processesFlags[4]) {
				if (comptonMethod == 2)
					localCompton(st,lumOut,lumOutIC,energies);
				else
					thermalCompton2(st,lumOut,lumOutBr,lumInICm,lumOutIC,lumOutIC_CD,lumOutIC_Br,energies,processesFlags);
			}
			if (processesFlags[3])
				coldDiskLuminosity(st,lumOut,lumOutRefl,lumOutCD,energies);
		} while (res > 1.0e-3);
		//photonDensity(st,energies,lumOut);
		//photonDensityAux(st,energies,lumOut);
		absorptionLumThermal(st,lumOut,lumOutCD,lumOutRefl,lumOut_gg,energies);
		writeLuminosities(st,energies,lumOutSy,lumOutBr,lumOutpp,lumInICm,
							lumOutIC,lumOutIC_CD,lumOutIC_Br,lumOut_gg,lumOutCD,lumOutRefl,filename);
	}
	writeEandRParamSpace("photonDensity", st.photon.distribution, 0, 0);
	writeRParamSpace("photonDensity_R", st.photon.distribution, 0, 0);
	show_message(msgEnd,Module_thermalLuminosities);
}


// Copyright (c) 2020 Marcus Valtonen Örnhag
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "get_valtonenornhag_arxiv_2020b.hpp"
#include <Eigen/Geometry>
#include <cmath>  // max
#include <vector>
#include "solver_valtonenornhag_arxiv_2020b_fHf.hpp"
#include "normalize2dpts.hpp"
#include "posedata.hpp"

namespace HomLib {
namespace ValtonenOrnhagArxiv2020B {
    inline Eigen::Vector4d construct_hvector(double w, const Eigen::VectorXd input);

    std::vector<HomLib::PoseData> get_fHf(
        const Eigen::MatrixXd &p1,
        const Eigen::MatrixXd &p2,
        const Eigen::Matrix3d &R1,
        const Eigen::Matrix3d &R2
    ) {
        // This is a 2-point method
        int nbr_pts = 2;

        // We expect inhomogenous input data, i.e. p1 and p2 are 2x3 matrices
        assert(p1.rows() == 2);
        assert(p2.rows() == 2);
        assert(p1.cols() == nbr_pts);
        assert(p2.cols() == nbr_pts);
        int nbr_coeffs = 26;

        // Save copies of the inverse rotation
        Eigen::Matrix3d R1T = R1.transpose();
        Eigen::Matrix3d R2T = R2.transpose();

        // Compute normalization matrix
        double scale1 = normalize2dpts(p1);
        double scale2 = normalize2dpts(p2);
        double scale = std::max(scale1, scale2);
        Eigen::Vector3d s;
        s << scale, scale, 1.0;
        Eigen::DiagonalMatrix<double, 3> S = s.asDiagonal();

        // Normalize data
        Eigen::MatrixXd x1(3, 2);
        Eigen::MatrixXd x2(3, 2);
        x1 = p1.colwise().homogeneous();
        x2 = p2.colwise().homogeneous();

        x1 = S * x1;
        x2 = S * x2;

        Eigen::Matrix2d x1t;
        x1t << x1.colwise().hnormalized();
        Eigen::Matrix2d x2t;
        x2t << x2.colwise().hnormalized();

        // Wrap input data to expected format
        Eigen::VectorXd input(nbr_coeffs);
        input << x1t.col(0),
                 x2t.col(0),
                 x1t.col(1),
                 x2t.col(1),
                 Eigen::Map<Eigen::VectorXd>(R1T.data(), 9),
                 Eigen::Map<Eigen::VectorXd>(R2T.data(), 9);

        // Extract w
        Eigen::VectorXcd w = HomLib::ValtonenOrnhagArxiv2020B::solver_fHf(input);

        // Pre-processing: Remove complex-valued solutions
        double thresh = 1e-5;
        Eigen::ArrayXd real_w = w.imag().array().abs();

        // This is a 2 pt solver
        std::vector<HomLib::PoseData> posedata;
        HomLib::PoseData tmp_pose;
        double w_tmp;
        Eigen::Vector4d hvec;
        Eigen::Matrix3d K, Ki, Htmp;

        double tol = 1e-13;

        for (int i = 0; i < real_w.size(); i++) {
            if (real_w(i) <= thresh) {
                // Compute algebraic error, and compare to other solutions.
                w_tmp = w(i).real();

                // Compute h vector
                hvec = construct_hvector(w_tmp, input);

                // Two spurious solutions were added, corresponding to last element
                // equal to zero.
                if (std::abs(hvec(3)) > tol) {
                    Htmp = Eigen::Matrix3d::Identity(3, 3);
                    Htmp.col(1) = hvec.hnormalized();

                    K = Eigen::Vector3d(w_tmp, w_tmp, 1).asDiagonal();
                    Ki = Eigen::Vector3d(1, 1, w_tmp).asDiagonal();
                    Htmp = S.inverse() * K * R2 * Htmp * R1T * Ki * S;

                    // Package output
                    tmp_pose.homography = Htmp;
                    tmp_pose.focal_length = w_tmp / scale;
                    posedata.push_back(tmp_pose);
                }
            }
        }

        return posedata;
    }

    inline Eigen::Vector4d construct_hvector(double w, const Eigen::VectorXd input) {
        Eigen::VectorXd d(27);
        d << w, input;

        double s1 = d[0]*d[25];
        double s2 = d[0]*d[16];
        double t1 = s1 + d[7]*d[19] + d[8]*d[22];
        double t2 = d[3]*d[19] + s1 + d[4]*d[22];
        double t3 = d[5]*d[10] + s2 + d[6]*d[13];
        double t4 = d[1]*d[10] + d[2]*d[13] + s2;
        double u1 = d[0]*d[24];
        double u2 = d[0]*d[15];
        double u3 = d[0]*d[26];

        Eigen::Matrix<double, 4, 4> M;
        M << 0, -t4*(d[3]*d[20] + u3 + d[4]*d[23]), t4*t2, (d[1]*d[11] + d[2]*d[14] + d[0]*d[17])*t2,  // NOLINT
             -t4*t2,  t4*(d[3]*d[18] + u1 + d[4]*d[21]), 0, -(d[1]*d[9] + d[2]*d[12] + u2)*t2,  // NOLINT
             0, -t3*(u3 + d[7]*d[20] + d[8]*d[23]), t3*t1,  (d[5]*d[11] + d[0]*d[17] + d[6]*d[14])*t1,  // NOLINT
             -t3*t1,  t3*(u1 + d[7]*d[18] + d[8]*d[21]), 0, -(d[5]*d[9] + u2 + d[6]*d[12])*t1;  // NOLINT

        // Perform SVD
        Eigen::JacobiSVD<Eigen::Matrix<double, 4, 4>> svd(M, Eigen::ComputeFullV);

        // Extract hvector
        Eigen::Vector4d h = svd.matrixV().col(3);

        return h;
    }
}  // namespace ValtonenOrnhagArxiv2020B
}  // namespace HomLib


//
//  expr/detail/expr.hpp
//  solver
//
//  Created by Alexandre Martin on 28/04/2016.
//  Copyright © 2016 scalexm. All rights reserved.
//

#ifndef DETAIL_EXPR_HPP
#define DETAIL_EXPR_HPP

#include <ostream>
#include <boost/variant.hpp>

namespace expr { namespace detail {
    /*
        tags for the template struct binary: each tag identifies a binary logical operator
    */
    struct and_tag {
        constexpr static const char * text = "/\\";
    };

    struct or_tag {
        constexpr static const char * text = "\\/";
    };

    struct xor_tag {
        constexpr static const char * text = "X";
    };

    struct impl_tag {
        constexpr static const char * text = "=>";
    };

    struct equiv_tag {
        constexpr static const char * text = "<=>";
    };

    struct none_ { };
    template<class Atom> struct not_;
    template<class Tag, class Atom> struct binary_;

    template<class Atom>
    using and_ = binary_<detail::and_tag, Atom>;

    template<class Atom>
    using or_ = binary_<detail::or_tag, Atom>;

    template<class Atom>
    using xor_ = binary_<detail::xor_tag, Atom>;

    template<class Atom>
    using impl_ = binary_<detail::impl_tag, Atom>;

    template<class Atom>
    using equiv_ = binary_<detail::equiv_tag, Atom>;

    template<class Atom>
    using expr_ = boost::variant<
        none_, // none in first position for default ctor
        Atom,
        boost::recursive_wrapper<not_<Atom>>,
        boost::recursive_wrapper<and_<Atom>>,
        boost::recursive_wrapper<or_<Atom>>,
        boost::recursive_wrapper<xor_<Atom>>,
        boost::recursive_wrapper<impl_<Atom>>,
        boost::recursive_wrapper<equiv_<Atom>>
    >;

    template<class Tag, class Atom>
    struct binary_ {
        expr_<Atom> op_left, op_right;
    };

    template<class Atom>
    struct not_ {
        expr_<Atom> op;
    };

    inline bool operator ==(const none_ &, const none_ &) {
        return true;
    }

    template<class Atom>
    inline bool operator ==(const not_<Atom> & a, const not_<Atom> & b) {
        return a.op == b.op;
    }

    template<class Tag, class Atom>
    inline bool operator ==(const binary_<Tag, Atom> & a, const binary_<Tag, Atom> & b) {
        return a.op_left == b.op_left && a.op_right == b.op_right;
    }

    inline std::ostream & operator <<(std::ostream & stream, const none_ &) {
        return stream << "[none]";
    }

    template<class Atom>
    inline std::ostream & operator <<(std::ostream & stream, const not_<Atom> & exp) {
        return stream << "~(" << exp.op << ")";
    }

    template<class Tag, class Atom>
    inline std::ostream & operator <<(std::ostream & stream, const binary_<Tag, Atom> & exp) {
        return stream << "(" << exp.op_left << " " << Tag::text << " " << exp.op_right << ")";
    }
} }

#endif


/*
 * This file is part of the statismo library.
 *
 * Author: Marcel Luethi (marcel.luethi@unibas.ch)
 *
 * Copyright (c) 2011 University of Basel
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifndef __PCAModelBuilder_TXX
#define __PCAModelBuilder_TXX

#include "PCAModelBuilder.h"

#include <iostream>

#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

#include "CommonTypes.h"
#include "Exceptions.h"

namespace statismo {

template <typename T>
PCAModelBuilder<T>::PCAModelBuilder()
    : Superclass() {
}

template <typename T>
typename PCAModelBuilder<T>::StatisticalModelType*
PCAModelBuilder<T>::BuildNewModel(const DataItemListType& sampleDataList, double noiseVariance, bool computeScores, EigenValueMethod method) const {

    unsigned n = sampleDataList.size();
    if (n <= 0) {
        throw StatisticalModelException("Provided empty sample set. Cannot build the sample matrix");
    }

    unsigned p = sampleDataList.front()->GetSampleVector().rows();
    const Representer<T>* representer = sampleDataList.front()->GetRepresenter();

    // Compute the mean vector mu
    VectorType mu = VectorType::Zero(p);

    for (typename DataItemListType::const_iterator it = sampleDataList.begin();
            it != sampleDataList.end();  ++it) {
        assert((*it)->GetSampleVector().rows() == p); // all samples must have same number of rows
        assert((*it)->GetRepresenter() == representer); // all samples have the same representer
        mu += (*it)->GetSampleVector();
    }
    mu /= n;

    // Build the mean free sample matrix X0
    MatrixType X0(n, p);
    unsigned i = 0;
    for (typename DataItemListType::const_iterator it = sampleDataList.begin();
            it != sampleDataList.end(); ++it) {
        X0.row(i++) = (*it)->GetSampleVector() - mu;
    }

    // build the model
    StatisticalModelType* model = BuildNewModelInternal(representer, X0, mu, noiseVariance, method);

    // compute the scores if requested
    MatrixType scores;
    if (computeScores) {
        scores = this->ComputeScores(sampleDataList, model);
    }

    typename BuilderInfo::ParameterInfoList bi;
    bi.push_back(BuilderInfo::KeyValuePair("NoiseVariance ", Utils::toString(noiseVariance)));

    typename BuilderInfo::DataInfoList dataInfo;
    i = 0;
    for (typename DataItemListType::const_iterator it = sampleDataList.begin();
            it != sampleDataList.end();
            ++it, i++) {
        std::ostringstream os;
        os << "URI_" << i;
        dataInfo.push_back(BuilderInfo::KeyValuePair(os.str().c_str(),(*it)->GetDatasetURI()));
    }

    // finally add meta data to the model info
    BuilderInfo builderInfo("PCAModelBuilder", dataInfo, bi);

    ModelInfo::BuilderInfoList biList;
    biList.push_back(builderInfo);

    ModelInfo info(scores, biList);
    model->SetModelInfo(info);

    return model;
}

template <typename T>
typename PCAModelBuilder<T>::StatisticalModelType*
PCAModelBuilder<T>::BuildNewModelInternal(const Representer<T>* representer, const MatrixType& X0, const VectorType& mu,
        double noiseVariance, EigenValueMethod method) const {

    unsigned n = X0.rows();
    unsigned p = X0.cols();

    switch(method) {
    case JacobiSVD:

        typedef Eigen::JacobiSVD<MatrixType> SVDType;
        typedef Eigen::JacobiSVD<MatrixTypeDoublePrecision> SVDDoublePrecisionType;

        // We destinguish the case where we have more variables than samples and
        // the case where we have more samples than variable.
        // In the first case we compute the (smaller) inner product matrix instead of the full covariance matrix.
        // It is known that this has the same non-zero singular values as the covariance matrix.
        // Furthermore, it is possible to compute the corresponding eigenvectors of the covariance matrix from the
        // decomposition.

        if (n < p) {
            // we compute the eigenvectors of the covariance matrix by computing an SVD of the
            // n x n inner product matrix 1/(n-1) X0X0^T
            MatrixType Cov = X0 * X0.transpose() * 1.0/(n-1);
            SVDDoublePrecisionType SVD(Cov.cast<double>(), Eigen::ComputeThinV);
            VectorType singularValues = SVD.singularValues().cast<ScalarType>();
            MatrixType V = SVD.matrixV().cast<ScalarType>();

            unsigned numComponentsAboveTolerance = ((singularValues.array() - noiseVariance - Superclass::TOLERANCE) > 0).count();

            // there can be at most n-1 nonzero singular values in this case. Everything else must be due to numerical inaccuracies
            unsigned numComponentsToKeep = std::min(numComponentsAboveTolerance, n - 1);
            // compute the pseudo inverse of the square root of the singular values
            // which is then needed to recompute the PCA basis
            VectorType singSqrt = singularValues.array().sqrt();
            VectorType singSqrtInv = VectorType::Zero(singSqrt.rows());
            for (unsigned i = 0; i < numComponentsToKeep; i++) {
                assert(singSqrt(i) > Superclass::TOLERANCE);
                singSqrtInv(i) = 1.0 / singSqrt(i);
            }

            if (numComponentsToKeep == 0) {
                throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
            }

            // we recover the eigenvectors U of the full covariance matrix from the eigenvectors V of the inner product matrix.
            // We use the fact that if we decompose X as X=UDV^T, then we get X^TX = UD^2U^T and XX^T = VD^2V^T (exploiting the orthogonormality
            // of the matrix U and V from the SVD). The additional factor sqrt(n-1) is to compensate for the 1/sqrt(n-1) in the formula
            // for the covariance matrix.

            MatrixType pcaBasis = X0.transpose() * V * singSqrtInv.asDiagonal();
            pcaBasis /= sqrt(n - 1.0);
            pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

            VectorType sampleVarianceVector = singularValues.topRows(numComponentsToKeep);
            VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);

            StatisticalModelType* model = StatisticalModelType::Create(representer, mu, pcaBasis, pcaVariance, noiseVariance);

            return model;
        } else {
            // we compute an SVD of the full p x p  covariance matrix 1/(n-1) X0^TX0 directly
            SVDType SVD(X0.transpose() * X0, Eigen::ComputeThinU);
            VectorType singularValues = SVD.singularValues();
            singularValues /= (n - 1.0);
            unsigned numComponentsToKeep = ((singularValues.array() - noiseVariance - Superclass::TOLERANCE) > 0).count();
            MatrixType pcaBasis = SVD.matrixU();

            pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

            if (numComponentsToKeep == 0) {
                throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
            }

            VectorType sampleVarianceVector = singularValues.topRows(numComponentsToKeep);
            VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);
            StatisticalModelType* model = StatisticalModelType::Create(representer, mu, pcaBasis, pcaVariance, noiseVariance);
            return model;
        }
        break;

    case SelfAdjointEigenSolver: {
        // we compute the eigenvalues/eigenvectors of the full p x p  covariance matrix 1/(n-1) X0^TX0 directly

        typedef Eigen::SelfAdjointEigenSolver<MatrixType> SelfAdjointEigenSolver;
        SelfAdjointEigenSolver es;
        es.compute(X0.transpose() * X0);
        VectorType eigenValues = es.eigenvalues().reverse(); // SelfAdjointEigenSolver orders the eigenvalues in increasing order
        eigenValues /= (n -1.0);

        unsigned numComponentsToKeep = ((eigenValues.array() - noiseVariance - Superclass::TOLERANCE) > 0).count();
        MatrixType pcaBasis = es.eigenvectors().rowwise().reverse();
        pcaBasis.conservativeResize(Eigen::NoChange, numComponentsToKeep);

        if (numComponentsToKeep == 0) {
            throw StatisticalModelException("All the eigenvalues are below the given tolerance. Model cannot be built.");
        }

        VectorType sampleVarianceVector = eigenValues.topRows(numComponentsToKeep);
        VectorType pcaVariance = (sampleVarianceVector - VectorType::Ones(numComponentsToKeep) * noiseVariance);
        StatisticalModelType* model = StatisticalModelType::Create(representer, mu, pcaBasis, pcaVariance, noiseVariance);
        return model;
    }
    break;

    default:
        throw StatisticalModelException("Unrecognized decomposition/eigenvalue solver method.");
        return 0;
        break;
    }
}

} // namespace statismo

#endif