#include <iostream>
#include <list>
#include <numeric>
#include <unordered_map>

#include <boost/optional.hpp>

#include "node.h"
#include "graph.h"

// Route between two nodes
namespace rbn
{
    bool routeExists(g::IntGraph const& graph, g::IntVertex::Ptr const& start, g::IntVertex::Ptr const& end)
    {
        if (start == end)
            return true;

        // Just for sure mark all nodes as unvisited
        for (auto && n : graph.verticies())
            n.second->setState(g::IntVertex::Unvisited);

        std::queue<g::IntVertex::Ptr> verteciesQueue;

        start->setState(g::IntVertex::Visiting);
        verteciesQueue.push(start);

        g::IntVertex::Ptr visitingVertex;
        while (!verteciesQueue.empty()) {
            visitingVertex = verteciesQueue.front();
            verteciesQueue.pop();

            if (visitingVertex) {
                for (auto && v : visitingVertex->linkedVertices()) {
                    if (v->state() == g::IntVertex::Unvisited) {
                        if (v == end)
                            return true;
                        else {
                            v->setState(g::IntVertex::Visiting);
                            verteciesQueue.push(v);
                        }
                    }
                }

                visitingVertex->setState(g::IntVertex::Visited);
            }
        }

        return false;
    }
}

// Minimal tree (create minimal tree from sorted array with unique elements)
namespace bst
{
    using namespace Tree;

    namespace details
    {
        IntNodePtr createMinimalBSTImpl(std::vector<int> const& array, int from, int to)
        {
            if (to < from)
                return nullptr;

            int mid = (from + to) / 2;
            auto node = std::make_shared<IntNode>(array.at(mid));
            node->setLeftChild(createMinimalBSTImpl(array, from, mid - 1));
            node->setRightChild(createMinimalBSTImpl(array, mid + 1, to));
            return node;
        }
    }

    IntNodePtr createMinimalBST(std::vector<int> const& array)
    {
        return details::createMinimalBSTImpl(array, 0, array.size() - 1);
    }
}

// List of depth. For binary tree, algorithm to create a linked list of all nodes on each depth
// using preorder traversal algorithm
namespace lod
{
    using namespace Tree;
    using NodesList = std::list<IntNodePtr>;
    using NodesListsArray = std::vector<NodesList>;

    namespace details
    {
        void createLevelLinkedListImpl(IntNodePtr const& root, NodesListsArray & lists, std::size_t level)
        {
            if (!root)
                return;

            // Check if level not in the list
            if (lists.size() == level) {
                lists.push_back(NodesList());
                lists.back().push_back(root);
            } else
                lists[level].push_back(root);

            createLevelLinkedListImpl(root->mLeftChild, lists, level + 1);
            createLevelLinkedListImpl(root->mRightChild, lists, level + 1);
        }
    }

    NodesListsArray createLevelLinkedList(IntNodePtr const& root)
    {
        NodesListsArray lists;
        details::createLevelLinkedListImpl(root, lists, 0);
        return lists;
    }
}

// Check if tree is balanced (difference between subtrees no more than one)
namespace bt
{
    using namespace Tree;

    namespace details
    {
        static const int ERROR_TAG = std::numeric_limits<int>::min();

        int checkHeight(IntNodePtr const& root)
        {
            if (!root)
                return -1;

            int leftHeight = checkHeight(root->mLeftChild);
            if (leftHeight == ERROR_TAG)
                return ERROR_TAG; // Propagate error to the top

            int rightHeight = checkHeight(root->mRightChild);
            if (rightHeight == ERROR_TAG)
                return ERROR_TAG; // Propagate error to the top

            if (std::abs(leftHeight - rightHeight) > 1)
                return ERROR_TAG; // Pass error back
            else
                return std::max(leftHeight, rightHeight) + 1;
        }
    }

    bool isBalanced(IntNodePtr const& root)
    {
        return details::checkHeight(root) != details::ERROR_TAG;
    }
}

// Check if tree is a valid BST
namespace vbst
{
    using namespace Tree;

    namespace details
    {
        using Int = boost::optional<int>;
        bool checkBSTImpl(IntNodePtr const& n, Int min = Int(), Int max = Int())
        {
            if (!n)
                return true;

            if ((min && n->mKey <= min) || (max && n->mKey > max))
                return false;

            if (!checkBSTImpl(n->mLeftChild, min, n->mKey) ||
                !checkBSTImpl(n->mRightChild, n->mKey, max))
                return false;

            return true;
        }
    }

    bool checkBST(IntNodePtr const& root)
    {
        return details::checkBSTImpl(root);
    }
}

// Successor (find "next" node)
namespace sr
{
    using namespace Tree;

    namespace details
    {
        IntNodePtr leftMostChild(IntNodePtr node)
        {
            if (!node)
                return nullptr;

            while (node->mLeftChild)
                node = node->mLeftChild;

            return node;
        }
    }

    IntNodePtr successor(IntNodePtr const& node)
    {
        if (!node)
            return nullptr;

        // If there are right child just return leftmost child shich will be the next node
        if (node->mRightChild)
            return details::leftMostChild(node->mRightChild);
        else {
            auto current = node;
            auto parent  = node->parent();

            // Go up until we're on left node instead of right
            while (parent && parent->mLeftChild != current) {
                current = parent;
                parent  = parent->parent();
            }

            return parent;
        }
    }
}

// Topological sort.
// Given list of projects and list of dependencies (pairs second depends on first). Find bild order
// or error is there is no valid build order.
namespace ts
{
    using namespace g;
    using ProjectsVector     = std::vector<std::string>;
    using DependenciesVector = std::vector<std::pair<std::string, std::string>>;

    namespace details
    {
        using Project = Vertex<std::string>;
        using OrderedProjects = std::list<Project::Ptr>;

        bool doDFS(Project::Ptr const& project, OrderedProjects & projects)
        {
            // Cycle detected
            if (project->state() == Project::Visiting)
                return false;

            if (project->state() == Project::Unvisited) {
                project->setState(Project::Visiting);

                for (auto && child : project->linkedVertices())
                    if (!doDFS(child, projects))
                        return false; // Propagate cycle

                project->setState(Project::Visited);
                projects.push_front(project);
            }

            return true;
        }

        decltype(auto) fillGraph(ProjectsVector const& projects,
                                 DependenciesVector const& dependencies)
        {
            auto graph = std::make_unique<StrGraph>();

            for (auto && project : projects)
                graph->addVertex(project);

            // Second depends on first
            for (auto && edge : dependencies)
                graph->addEdge(edge.second, edge.first);

            return graph;
        }
    }

    ProjectsVector orderedProjects(ProjectsVector     const& projects,
                                   DependenciesVector const& dependencies)
    {
        auto graph = details::fillGraph(projects, dependencies);

        details::OrderedProjects op;
        for (auto && project : graph->verticies())
            if (project.second->state() == details::Project::Unvisited)
                if (!details::doDFS(project.second, op))
                    return ProjectsVector();

        ProjectsVector result(op.size());
        std::transform(op.begin(), op.end(), result.begin(), [](auto && p) { return p->data(); });
        return result;
    }
}

// Find first common ancestor for two nodes
namespace fca
{
    using namespace Tree;

    namespace details
    {
        IntNodePtr doUpBy(IntNodePtr node, int depth)
        {
            while (depth > 0 && node) {
                node = node->parent();
                --depth;
            }

            return node;
        }
    }

    IntNodePtr commonAncestor(IntNodePtr const& f, IntNodePtr const& s)
    {
        assert(f && s);
        int delta = f->depth() - s->depth();
        IntNodePtr shallower = delta > 0 ? s : f;
        IntNodePtr deeper    = delta > 0 ? f : s;

        // Up deeper node
        deeper = details::doUpBy(deeper, std::abs(delta));

        // Search for intersection
        while (deeper != shallower && deeper && shallower) {
            deeper = deeper->parent();
            shallower = shallower->parent();
        }

        return !shallower || !deeper ? nullptr : shallower;
    }
}

// Given a BST which created by traversing array left to right,
// generate all arrays may lead to this tree
namespace bstsq
{
    using List = std::list<int>;
    using VectorLists = std::vector<List>;

    using namespace Tree;

    namespace details
    {
        void mergeLists(List & first, List & second, VectorLists & results, List & prefix);

        void addHeadToPrefixAndMerge(List & first, List & second, VectorLists & results,
                                     List & prefix, List & listForTakingHead)
        {
            // Take head
            int head = listForTakingHead.front();
            listForTakingHead.pop_front();
            prefix.push_back(head);

            mergeLists(first, second, results, prefix);

            // Restore head
            prefix.pop_back();
            listForTakingHead.push_front(head);
        }

        // Merge lists in all possible ways
        void mergeLists(List & first, List & second, VectorLists & results, List & prefix)
        {
            // If one list is empty, add remainder to the prefix and store result
            if (first.empty() || second.empty()) {
                List result(prefix);
                std::copy(first.begin(), first.end(), std::back_inserter(result));
                std::copy(second.begin(), second.end(), std::back_inserter(result));
                results.push_back(result);
                return;
            }

            // Recurse go through first and second lists
            addHeadToPrefixAndMerge(first, second, results, prefix, first /*listForTakingHead*/);
            addHeadToPrefixAndMerge(first, second, results, prefix, second /*listForTakingHead*/);
        }
    }

    VectorLists allSequences(IntNodePtr const& root)
    {
        VectorLists result;

        if (!root) {
            result.push_back(List());
            return result;
        }

        List prefix;
        prefix.push_back(root->mKey);

        // Recurse on left and right subtrees
        auto leftSeq  = allSequences(root->mLeftChild);
        auto rightSeq = allSequences(root->mRightChild);

        // Merge all list from the left and right sides
        for (auto && left : leftSeq) {
            for (auto && right : rightSeq) {
                VectorLists merged;
                details::mergeLists(left, right, merged, prefix);
                std::copy(merged.begin(), merged.end(), std::back_inserter(result));
            }
        }

        return result;
    }
}

// There are two large binary trees (T1 and T2), check if T2 is sutree of T1. T1 is bigger.
namespace ct
{
    using namespace Tree;

    namespace details
    {
        bool matchTree(IntNodePtr const& n1, IntNodePtr const& n2)
        {
            if (!n1 && !n2)
                return true; // nothing left
            else if (!n1 || !n2)
                return false; // one of them is empty, don't match
            else if (n1->mKey != n2->mKey)
                return false; // different data
            else // Recurse compare subtrees
                return matchTree(n1->mLeftChild, n2->mLeftChild) &&
                       matchTree(n1->mRightChild, n2->mRightChild);
        }

        bool subTree(IntNodePtr const& n1, IntNodePtr const& n2)
        {
            if (!n1)
                return false; // big tree is empty
            else if (n1->mKey == n2->mKey && matchTree(n1, n2))
                return true;

            return subTree(n1->mLeftChild, n2) || subTree(n1->mRightChild, n2);
        }
    }

    bool containsTree(IntNodePtr const& t1, IntNodePtr const& t2)
    {
      // The empty tree is always a subtree :)
      return !t2 || details::subTree(t1, t2);
    }
}

// Given binary tree and sum. Count the number of pathes to get given sum. Paths should traveling
// only from parent nodes to child.
namespace sp
{
    using namespace Tree;

    namespace details
    {
        using PathCount = std::unordered_map<int, int>;

        void incVal(PathCount & pathCount, int key, int delta)
        {
            int newCount = pathCount[key] + delta;
            if (newCount == 0)
                pathCount.erase(key); // Reduce space
            else
                pathCount[key] = delta;
        }

        int countPathsWithSumImpl(IntNodePtr const& node, int targetSum, int runnigSum,
                                  PathCount & pathCount)
        {
            if (!node)
                return 0; // Base case

            // Count paths with with sum ending at the current node
            runnigSum += node->mKey;
            int sum = runnigSum - targetSum;
            int totalPaths = pathCount[sum];

            // One additional path starts at root
            if (runnigSum == targetSum)
                ++totalPaths;

            // Increment pathCount, recurse, then decrement
            incVal(pathCount, runnigSum, 1);
            totalPaths += countPathsWithSumImpl(node->mLeftChild, targetSum, runnigSum, pathCount);
            totalPaths += countPathsWithSumImpl(node->mRightChild, targetSum, runnigSum, pathCount);
            incVal(pathCount, runnigSum, -1);

            return totalPaths;
        }
    }

    int countPathWithSum(IntNodePtr const& node, int sum)
    {
        details::PathCount pathCount;
        return details::countPathsWithSumImpl(node, sum, 0, pathCount);
    }
}

int main(int /*argc*/, char */*argv*/[])
{
    // 1
//    g::IntGraph graph;
//    auto first = graph.addVertex(1);
//    auto second = graph.addVertex(2);
//    auto third = graph.addVertex(3);
//    auto fourth = graph.addVertex(4);

//    graph.addEdge(1, 2);
//    graph.addEdge(1, 3);
//    graph.addEdge(2, 3);
//    graph.addEdge(3, 4);

//    std::cout << std::boolalpha << rbn::routeExists(graph, fourth, first) << std::endl;

//    graph.dump("/home/vt4a2h/Projects/alg/graph.dot");
//    system("cd /home/vt4a2h/Projects/alg/ && dot graph.dot -Tsvg > graph.svg ");

    // 2
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);
//        tree->dump("/home/vt4a2h/Projects/alg/graph.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot graph.dot -Tsvg > graph.svg ");
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 3
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);

//        auto lists = lod::createLevelLinkedList(tree);
//        for (auto && list : lists) {
//            for (auto && n : list) {
//                std::cout << n->mKey << "\t";
//            }
//            std::cout << std::endl;
//        }
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 4
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);

//        // Must be balanced
//        std::cout << "Is balanced: " << std::boolalpha << bt::isBalanced(tree) << std::endl;

//        // Disbalance somehow
//        Tree::IntNodePtr min = tree->mLeftChild;
//        while (min->mLeftChild)
//            min = min->mLeftChild;

//        std::cout << "Min element: " << min->mKey << std::endl;

//        min->makeLeftChild(-1)->makeLeftChild(-2);

//        std::cout << "Is balanced: " << std::boolalpha << bt::isBalanced(tree) << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 5
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);

//        std::cout << "Is BST: " << std::boolalpha << vbst::checkBST(tree) << std::endl;

//        Tree::IntNodePtr min = tree->mLeftChild;
//        while (min->mLeftChild)
//            min = min->mLeftChild;
//        min->makeLeftChild(100)->makeLeftChild(200);

//        std::cout << "Is BST: " << std::boolalpha << bt::isBalanced(tree) << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 6
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);

//        Tree::IntNodePtr min = tree->mLeftChild;
//        while (min->mLeftChild)
//            min = min->mLeftChild;

//        // 1 -> 2
//        std::cout << min->mKey << " -> " << sr::successor(min)->mKey << std::endl;

//        // 2 -> 3
//        auto two = min->parent();
//        std::cout << two->mKey << " -> " << sr::successor(two)->mKey << std::endl;

//        // 3 -> 4
//        auto three = sr::successor(min)->mRightChild;
//        std::cout << three->mKey << " -> " << sr::successor(three)->mKey << std::endl;

//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 7
//    ts::ProjectsVector pv {"a", "b", "c", "d", "e", "f"};
//    ts::DependenciesVector dv {{"a", "d"}, {"f", "b"}, {"b", "d"}, {"f", "a"}, {"d", "c"}};
//    try {
//        auto orderedProjects = ts::orderedProjects(pv, dv);
//        for (auto && p : orderedProjects)
//            std::cout << p << "\t";
//        std::cout << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 8
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
//        auto tree = bst::createMinimalBST(v);

//        Tree::IntNodePtr min = tree->mLeftChild;
//        while (min->mLeftChild)
//            min = min->mLeftChild;

//        Tree::IntNodePtr someNode = min->parent()->parent()->mRightChild->mRightChild;

//        min = min->makeRightChild(200)->makeLeftChild(300);

//        tree->dump("/home/vt4a2h/Projects/alg/fca_graph.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot fca_graph.dot -Tsvg > fca_graph.svg ");

//        if (auto ca = fca::commonAncestor(min, someNode))
//            std::cout << "First common ancestor: " << ca->mKey << std::endl;
//        else
//            std::cout << "No common ancestor here." << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 9
    // TODO: revise
//    try {
//        std::vector<int> v {1, 2, 3, 4, 5};
//        auto tree = bst::createMinimalBST(v);

//        auto allSequences = bstsq::allSequences(tree);
//        for (auto && s : allSequences) {
//            for (auto && e : s) {
//                std::cout << e << "\t";
//            }
//            std::cout << std::endl;
//        }

//        tree->dump("/home/vt4a2h/Projects/alg/bstsq_graph.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot bstsq_graph.dot -Tsvg > bstsq_graph.svg ");
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 10
//    try {
//        std::vector<int> v1 {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
//        auto tree1 = bst::createMinimalBST(v1);

//        auto tree2 = std::make_shared<Tree::IntNode>(2);
//        tree2->makeLeftChild(1);
//        tree2->makeRightChild(3)->makeRightChild(4);

//        tree1->dump("/home/vt4a2h/Projects/alg/tree1.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot tree1.dot -Tsvg > tree1.svg ");

//        tree2->dump("/home/vt4a2h/Projects/alg/tree2.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot tree2.dot -Tsvg > tree2.svg ");

//        std::cout << std::boolalpha << ct::containsTree(tree1, tree2) << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 11
    // Implement method for getting random node from BST (nodes should be equally likelly to be chosen)
//    try {
//        srand(time(NULL));

//        auto tree = std::make_shared<Tree::IntNode>(4);
//        auto two = tree->insertInOrder(2);
//        two->insertInOrder(1);
//        two->insertInOrder(3);

//        auto six = tree->insertInOrder(6);
//        six->insertInOrder(5);
//        six->insertInOrder(7);

//        tree->dump("/home/vt4a2h/Projects/alg/ub.dot");
//        system("cd /home/vt4a2h/Projects/alg/ && dot ub.dot -Tsvg > ub.svg ");

//        for (int i = 1; i <= 7; ++i)
//            std::cout << tree->randomNode()->mKey << "\t";
//        std::cout << std::endl;
//    } catch (std::exception const& e) {
//        std::cout << e.what() << std::endl;
//    }

    // 12 // TODO: revise
    try {
        std::vector<int> v {1, 2, 3, 4, 5};
        auto tree = bst::createMinimalBST(v);

        std::cout << sp::countPathWithSum(tree, 3) << std::endl;
    } catch (std::exception const& e) {
        std::cout << e.what() << std::endl;
    }

    return 0;
}


#include <iostream>
#include <fstream>
#include <iterator>
#include <mintrace.h>
// Following header files are used in the step-3 in the examples of deal.II;
// Here just using these to get a large scale matrix as the test data.
#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/compressed_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
// Finally, this is for output to a file and to the console:
#include <deal.II/numerics/data_out.h>
#include <fstream>
#include <iostream>

using namespace dealii;

// Computing matrix A times vector b, and using the A[0] to store the result and return it.
std::vector<double> multiply(std::vector<std::vector<double>> A, std::vector<double> b)
{
  int m=A.size();
  int n=b.size();
  for (int i=0;i<m;i++)
    {
      double temp=0;
      for(int j=0;j<n;j++)
	{
	  temp+=A[i][j]*b[j];
	}
      A[0][i]=temp;
      //std::cout<<temp;
      //std::cout<<"The result of "<<i+1<<"th iteration\n";

    }

  return A[0];
}

// Computing u'*v;
double multiply(std::vector<double> u, std::vector<double> v)
{
  if(u.size()!=v.size())
    {
      std::cout<<"Multiply() ERROR: u'*v error! Please Check the size of u and v!";
    }
  else
    {
      double sum=0;
      for(int i=0;i<u.size();i++)
	{
	  sum+=u[i]*v[i];
	}

      return sum;
    }
}

// Design for return transpose matrix of A;
std::vector<std::vector<double>> transpose(std::vector<std::vector<double>> A)
{
  int n=A.size();
  int m=A[0].size();
    if (n < m)
      {
        for (int i = 0; i < n; i++)
	  {
            for (int j = 0; j < i; j++)
	      {
                double temp = A[j][i];
                A[j][i] = A[i][j];
                A[i][j] = temp;
	      }
	  }
        for (int i = n; i < m; i++)
	  {
            std::vector<double> temp;
            for (int j = 0; j < n; j++)
	      {
                temp.push_back(A[j][i]);
	      }
            A.push_back(temp);
	  }
        for (int i = 0; i < n; i++)
	  {
            for (int j = n; j < m; j++)
	      {
                A[i].pop_back();
	      }
	  }
      }
    else if(m<n)
      {
        for (int i = 0; i < m; i++)
	  {
            for (int j = 0; j < i; j++)
	      {
                double temp = A[j][i];
                A[j][i] = A[i][j];
		A[i][j] = temp;
	      }
	  }
        for (int i = 0; i < m; i++)
	  {
            for (int j = m; j < n; j++)
	      {
                A[i].push_back(A[j][i]);
	      }
	  }

        for (int i = m; i < n; i++)
	  {
            A.pop_back();
	  }

      }
    else
      {
	for(int i=0;i<n;i++)
	  {
	    for(int j=0;j<i;j++)
	      {
		double temp=A[j][i];
		A[j][i]=A[i][j];
		A[i][j]=temp;
	      }
	  }
      }

    return A;

}

//This function computes the matrix A times matrix B, and its result will be stored in A.
// It can be improved if reducing the use of the temp matrix to store the original values which
// is used in computing.
void multiply(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> B)
{
    int n = A.size();
    int m = B.size();
    int col = B[0].size();
    std::vector<double> temp(m);
    std::vector<std::vector<double>> A0(n, temp);

    if (col < n)
      {
        for (int i = 0; i < n; i++)
	  {
            for (int j = 0; j < col; j++)
	      {
                double tempValue = 0;
                for (int t = 0; t < m; t++)
		  {
                    tempValue += A[i][t] * B[t][j];
		  }
                A0[i][j] = tempValue;
	      }
            for (int j = col; j < m; j++)
	      {
                A0[i].pop_back();
	      }
	  }

        A = A0;
      }
    else
      {
        for (int i = 0; i <n; i++)
	  {
            for (int j = 0; j < m; j++)
	      {
                double tempValue = 0;
                for (int t = 0; t < m; t++)
		  {
                    tempValue += A[i][t] * B[t][j];
		  }
                A0[i][j] = tempValue;
	      }
            for (int j = m; j < col; j++)
	      {
                double tempValue = 0;
                for (int t = 0; t < m; t++)
		  {
                    tempValue += A[i][t] * B[t][j];
		  }
                A0[i].push_back(tempValue);
	      }
	  }
        A = A0;
      }
}

// this function will turn the matrix Q into the nxn identity matrix;
void identitymatrix(std::vector<std::vector<double>> &Q, int n)
{
  Q.clear();
  std::vector<double> temp(n,0);
  for (int i=0;i<n;i++)
    {
      Q.push_back(temp);
      Q[i][i]=1;
    }
}

// This function computes the infi-norm of vector x;
double infi_Norm(std::vector<double> x)
{
  double norm=0;
  for (int i=0;i<x.size();i++)
    {
      if (norm<abs(x[i]))
	{
	  norm=abs(x[i]);
	}
    }
  return norm;
}

// This function computes the inner-product corresponding with M of <u,v>
void M_inner(SparseMatrix<double> M, std::vector<double>u, std::vector<double>v){}

class Step3
{
public:
  Step3 ();

  void run ();
  void make_grid ();
  void setup_system ();
  void assemble_system ();

  //void multiply(std::vector<double>& x);
  std::vector<double> multiply(std::vector<double> x0);

  //void solve ();
  //
  void output_results () const;

  Triangulation<2>     triangulation;
  FE_Q<2>              fe;
  DoFHandler<2>        dof_handler;
  SparsityPattern      sparsity_pattern;
  SparseMatrix<double> system_matrix;

  Vector<double>       solution;
  Vector<double>       system_rhs;

  //std::vector<double> x;

};

//
//
//void Step3::multiply(std::vector<double>& x)
std::vector<double> multiply(SparseMatrix<double>A, std::vector<double> x0)
{
  std::vector<double> x(x0.size(),0);

  if (A.n()!=x0.size())
    {
      std::cout<<"Function multiply() ERROR: The sizes of matrix and vector are not same! Please check it!\n";
    }
  else
    {
      // std::cout<<"TEST POINT 2 \n";
      for(int k=0;k<A.m();k++)
	{
	  //std::cout<<"this is the "<<k<<"th iterations \n";
	  SparseMatrix<double>::const_iterator i=A.begin(k);

	  while(i!=A.end(k))
	    {
	      x[k]+=i->value()*x0[i->column()];
	      ++i;
	    }
	}
      return x;
    }
}

Step3::Step3 ()
  :
  fe (1),
  dof_handler (triangulation)
{}

void Step3::make_grid ()
{
  // First create the grid and refine all cells five times. Since the initial
  // grid (which is the square [-1,1]x[-1,1]) consists of only one cell, the
  // final grid has 32 times 32 cells, for a total of 1024.
  GridGenerator::hyper_cube (triangulation, -1, 1);
  triangulation.refine_global (5);
  // Unsure that 1024 is the correct number?  Let's see: n_active_cells
  // returns the number of active cells:
  std::cout << "Number of active cells: "
            << triangulation.n_active_cells()
            << std::endl;

  std::cout << "Total number of cells: "
            << triangulation.n_cells()
            << std::endl;
  // Note the distinction between n_active_cells() and n_cells().
}

void Step3::setup_system ()
{
  dof_handler.distribute_dofs (fe);
  std::cout << "Number of degrees of freedom: "
            << dof_handler.n_dofs()
            << std::endl;

  CompressedSparsityPattern c_sparsity(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern (dof_handler, c_sparsity);
  sparsity_pattern.copy_from(c_sparsity);

  system_matrix.reinit (sparsity_pattern);

  solution.reinit (dof_handler.n_dofs());
  system_rhs.reinit (dof_handler.n_dofs());
}

void Step3::assemble_system ()
{

  QGauss<2>  quadrature_formula(2);

  FEValues<2> fe_values (fe, quadrature_formula,
                         update_values | update_gradients | update_JxW_values);

  const unsigned int   dofs_per_cell = fe.dofs_per_cell;
  const unsigned int   n_q_points    = quadrature_formula.size();

  FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
  Vector<double>       cell_rhs (dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices (dofs_per_cell);

  DoFHandler<2>::active_cell_iterator
  cell = dof_handler.begin_active(),
  endc = dof_handler.end();
  for (; cell!=endc; ++cell)
    {
      fe_values.reinit (cell);

      cell_matrix = 0;
      cell_rhs = 0;

      for (unsigned int i=0; i<dofs_per_cell; ++i)
        for (unsigned int j=0; j<dofs_per_cell; ++j)
          for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
            cell_matrix(i,j) += (fe_values.shape_grad (i, q_point) *
                                 fe_values.shape_grad (j, q_point) *
                                 fe_values.JxW (q_point));

      for (unsigned int i=0; i<dofs_per_cell; ++i)
        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          cell_rhs(i) += (fe_values.shape_value (i, q_point) *
                          1 *
                          fe_values.JxW (q_point));

      cell->get_dof_indices (local_dof_indices);
      ///////////////////////
      // Following commands compute the interior of the matrix and store them into the matrix.

      for (unsigned int i=0; i<dofs_per_cell; ++i)
        for (unsigned int j=0; j<dofs_per_cell; ++j)
          system_matrix.add (local_dof_indices[i],
                             local_dof_indices[j],
                             cell_matrix(i,j));

      // And again, we do the same thing for the right hand side vector.
      for (unsigned int i=0; i<dofs_per_cell; ++i)
        system_rhs(local_dof_indices[i]) += cell_rhs(i);
    }

  std::map<types::global_dof_index,double> boundary_values;
  VectorTools::interpolate_boundary_values (dof_handler,
                                            0,
                                            ZeroFunction<2>(),
                                            boundary_values);

  MatrixTools::apply_boundary_values (boundary_values,
                                      system_matrix,
                                      solution,
                                      system_rhs);
}

//void Step3::multiply(std::vector<double>& x)
std::vector<double> Step3::multiply(std::vector<double> x0)
{
  std::vector<double> x(x0.size(),0);

  if (system_matrix.n()!=x0.size())
    {
      std::cout<<"Function multiply() ERROR: The sizes of matrix and vector are not same! Please check it!\n";
    }
  else
    {
      // std::cout<<"TEST POINT 2 \n";
      for(int k=0;k<system_matrix.m();k++)
	{
	  //std::cout<<"this is the "<<k<<"th iterations \n";
	  SparseMatrix<double>::const_iterator i=system_matrix.begin(k);

	  while(i!=system_matrix.end(k))
	    {
	      x[k]+=i->value()*x0[i->column()];
	      ++i;
	    }
	}
      return x;
    }
}

//void Householder(std::vector<std::vector<double>> *A, std::vector<std::vector<double>> *P)
//void Householder(std::vector<std::vector<double>> *A, std::vector<std::vector<double>> *P)

// This function computes the householder process of the symmetric definite matrix, it return
// two matrices. The tridiagonal matrix coordinating with A and the hermitian unitary matrix P.
void Householder(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> &P)
{
  int n=A.size();
  std::vector<std::vector<double>> Pk;

  for(int k=0;k<n-2;k++)
    {
      identitymatrix(Pk, n);
      double q=0;
      double alpha=0;
      // Computing the value of q in the Householder function;
      for(int i=k+1;i<n;i++)
	{
	  q+=A[i][k]*(A[i][k]);
	}

      // Computing the value of alpha
      if(A[k+1][k]==0)
	{
	  alpha=-sqrt(q);
	}
      else
	{
	  alpha=-sqrt(q)*A[k+1][k]/abs(A[k+1][k]);
	  // std::cout<<"the abs A[k+1][k] is "<<A[k+1][k]<<std::endl;
	}

      double RSQ=alpha*alpha-alpha*A[k+1][k]; // RSQ = 2r^2;

      //std::cout<<"Check Point 2, This is RSQ:::"<<RSQ<<"\n";

      std::vector<double> v(n-k,0), u(n-k,0), z(n-k,0);
      v[0]=0;
      v[1]=A[k+1][k]-alpha;

      //std::cout<<"Check Point 3::: "<<v[1]<<" \n";
      // w=(1/sqrt(2*RSQ)*v=1/2r*v);
      for(int j=2;j<v.size();j++)
	{
	  v[j]=A[k+j][k];
	}

      // std::cout<<"CheckPPPPPoint555 \n";

      for(int j=0;j<n-k;j++)
	{
	  for(int i=1;i<n-k;i++)
	    {
	      u[j]+=A[k+j][k+i]*v[i];
	    }
	  u[j]/=RSQ;
	}

      //std::cout<<"Check POOOIONIOHIHO\n";

      double PROD=0;
      for(int i=1;i<n-k;i++)
	{
	  PROD+=v[i]*u[i];
	}

      //std::cout<<"CheckPoint 4!!!!!!!!!\n";

      for(int j=0;j<n-k;j++)
	{
	  z[j]=u[j]-PROD/(2*RSQ)*v[j];
	}

      for(int l=k+1;l<n-1;l++)
	{
	  for(int j=l+1;j<n;j++)
	    {
	      A[j][l]=A[j][l]-v[l-k]*z[j-k]-v[j-k]*z[l-k];
	      A[l][j]=A[j][l];
	    }
	  A[l][l]=A[l][l]-2*v[l-k]*z[l-k];
	}

      A[n-1][n-1]=A[n-1][n-1]-2*v[n-k-1]*z[n-k-1];

      for(int j=k+2;j<n;j++)
	{
	  A[k][j]=0;
	  A[j][k]=0;
	}

      A[k+1][k]=A[k+1][k]-v[1]*z[0];
      A[k][k+1]=A[k+1][k];

      for (int j=k+1;j<n;j++)
	{
	  std::vector<double> temp(j-k,0);
	  for(int i=k+1;i<=j;i++)
	    {
	      for(int t=k+1;t<n;t++)
		{
		  temp[i-k-1]+=v[i-k]*v[t-k]*Pk[t][j]/(2*RSQ);
		}
	    }

	  //temp=multiply(W,Pj);// to get the jth column of the W*P;
	  for (int i=k+1;i<=j;i++)
	    {
	      Pk[i][j]-=2*temp[i-k-1];
	      Pk[j][i]=Pk[i][j];
	    }
	}
      multiply(P,Pk);

    }

}

void Step3::run ()
{
  make_grid ();
  setup_system();
  assemble_system ();
  std::cout<<system_matrix.m()<<"\n";
  std::cout<<system_matrix.n()<<"\n";

  //std::cout<<system_matrix.begin()->value()<<"\n";
  //std::cout<<system_matrix.begin()->column()<<"\n";

  std::vector<double> x(system_matrix.n(),1);
  //
  // Due to the system_matrix is a member of the class step3, but x is not a member of the class,
  // So we can not directly define the multiply function out of the class and using the
  // system_matrix directly.
  // we need to get a variable A equals to the system_matrix but A is not a member of the class;

  std::cout << "CheckPoint 1 \n";

  // multiply(x);
  x=multiply(x);
  //std::cout<<"This is the value of the vector x"<<x[0]<<std::endl;
  std::ofstream out ("sparse_matrix");
  system_matrix.print(out);

  std::ofstream output_file ("solution");
  for (const auto &e : x) output_file << e << "\n";

}

///////
// This function computes the QR factorization of the tridiagonal matrix A.
// Input: tridiagonal matrix A, identity matrix Q
// Output: uptriangle matrix A_, the orthonormal matrix Q; A=Q*A_;
//
// tips: Maybe I can improve the function by replace the matrix A by two vectors an bn-1;
// an contains the diagonal entries of A; bn-1 contains the sub-diagonal entries of A;

void QR(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> &Q)
{
  int n=A.size();

  // obtain the diagonal and subdiagonal entries of matrix A;
  /*
  std::vector<double>an(n),bn(n-1);
  for (int i=0;i<A.size()-1;i++)
    {
      an[i]=A[i][i];
      bn[i]=A[i+1][i];
    }
  an[n-1]=A[n-1][n-1];
  */

  for (int i=0;i<A.size()-1;i++)
    {
      double theta=0;
      double tempa1=A[i][i],tempa2=A[i+1][i+1],tempb=A[i+1][i],tempc=A[i][i+1];
      theta=atan(A[i+1][i]/A[i][i]);
      double c=cos(theta), s=sin(theta);

      A[i][i]=c*tempa1+s*tempb;
      A[i][i+1]=c*tempc+s*tempa2;
      A[i+1][i+1]=-s*tempc+c*tempa2;
      A[i+1][i]=-s*tempa1+c*tempb;
      if (i<A.size()-2)
	{
	  A[i][i+2]=s*A[i+1][i+2];
	  A[i+1][i+2]=c*A[i+1][i+2];
	}

      // update the orthogonal matrix Q by Q_k=Q_k-1*Q;

      for(int k=0;k<n;k++)
	{
	  double tmp1=Q[k][i],tmp2=Q[k][i+1];
	  Q[k][i]=Q[k][i]*c+Q[k][i+1]*s;
	  Q[k][i+1]=tmp1*(-s)+tmp2*c;
	}

    }

}

// This function computes the eigenvalue of the matrix A by QR methods, and it will also return the
// matrix Q during the process;
void QRSolver(std::vector<std::vector<double>> &A, std::vector<std::vector<double>> &Q, double tol=0.001)
{
  int n=A.size();
  // using householder transform matrix A into the tridiagonal matrix
  // store it in A and store the Householder unitary matrix in Q; A=QA_Q;
  //Householder(A,Q);

  // Qk as the initial matrix for every QR factorization, multiplying it together to get the
  // final unitary matrix;
  std::vector<std::vector<double>> Qk;

  // Get the subdiagonal entries of the matrix A and verify if its norm smaller than the tolerance;
  // If it is small enough that means we diagonalize the matrix A and the diagonal entries are
  // eigenvalues of A.
  std::vector<double> b(n-1,1);
  for(int i=0;i<n-1;i++)
    {
      b.push_back(A[i+1][i]);
    }

  int num=0;
  while (infi_Norm(b)>tol&&num<1000)
    {
      num++;

      b.clear();

      Householder(A,Q);

      identitymatrix(Qk,n);

      QR(A,Qk);

      // Compute the num-th iteration, get the Qk in this step;
      multiply(Q,Qk);
      // compute R*Q beacuse I store the R(computed above) into A, So I directly use multiply
      // function to get A*Q into A=RQ.
      multiply(A,Qk);

      //update the entries of b, i.e. the subdiagonal entries of matrix A=RQ;
        for(int i=0;i<n-1;i++)
	  {
	    b.push_back(A[i+1][i]);
	  }
    }
}

// The function using Conjugate Gradient method to get the solution of the problem Ax=b;
// It is modified CG function that is designed for solve PAP delta = PAX which needs A, M, X, b;
// ATTENTION: It can be revised as a class for A and M, It will be more convenient.
std::vector<double> CG(SparseMatrix<double>A, SparseMatrix<double> M, std::vector<std::vector<double>> X, std::vector<double> b, std::vector<double> &x)
{
  return x;
}

// Computing the matrix P during the minimal trace process;
std::vector<std::vector<double>> matrixP(SparseMatrix<double> M, std::vector<std::vector<double>>X )
{
  std::vector<std::vector<double>> P;
  return P;
}

//
// This function computes the matrix M-orthogonal modified Gram-Schmidt procedure;
std::vector<double> M_orth(SparseMatrix<double> M, std::vector<double> x)
{
  return x;
}

// This function computes a random matrix V as the initial matrix of the iteration;
// n implies the column number of V;
void rand_V(SparseMatrix<double>M, std::vector<std::vector<double>> &V, int n)
{
  std::vector<std::vector<double>> A(V);
  V=A;
}

// Compute matrix A - B;
std::vector<std::vector<double>> minus(std::vector<std::vector<double>> A, std::vector<std::vector<double>> B)
{
  return A;
}

// This function computes the smallest p eigenvalues of matrix A corresponding to matrix M;
// Notice: The A and M are square matrices here.
std::vector<double> min_trace(SparseMatrix<double> A, SparseMatrix<double>M, int p, double tol=0.001, int max_iter=1)
{
  int n=A.m();
  std::vector<std::vector<double>>V(p);
  std::vector<std::vector<double>>W(n);
  std::vector<std::vector<double>> MXTheta(p), U, X, Rk, delta;
  //SparseMatrix<double> P;

  // Construct the initial matrix V1 which is a n x p matrix and it is orthogonal by M, i.e.
  // V1'*M*V1=I_p;
  // I store the V(i,j)=V[j][i];
  rand_V(M,V,p);
  for (int k=0;k<max_iter;k++)
    {
      // Computing the matrix W and H;
      // Here I use the property of the symmetric A: H=V'*A*V=V'*W=W'*V=H'=V'*A'*V=H;

      // store the A *V[i] into W[i];
      for(int i=0;i<p;i++)
	{
	  std::vector<double> tempV=V[i];
	  // W[i]=multiply(A,V[i]);
	  W[i]=multiply(A,tempV);
	}

      // copy the W into H;
      std::vector<std::vector<double>> H(W);
      // Computing H'*V; in fact H[i][j]=H(j,i); So above the H=H';
      // Get the actual Hk;
      multiply(H,transpose(V));

      // Compute the spectral decomposition of Hk;
      identitymatrix(U,p);
      QRSolver(H,U); // H = theta_k, U=U_k;

      // transpose V into V[i][j]=V(i,j);
      X=transpose(V);
      // Compute Ritz vectors X_k=V_k*U_k;
      multiply(X,U);

      // Compute MXTheta = M * X_k * Theta_k
      // using XTheta to represent the matrix otherwise X will be changed by multiply;
      std::vector<std::vector<double>> XTheta(X);
      // Compute XTheta = X_k * Theta_k = XTheta*H;
      multiply(XTheta,H);
      for(int i=0;i<p;i++)
	{
	  MXTheta[i]=multiply(M, XTheta[i]);
	}
      // Compute residual Rk in fact Rk[i][j] = Rk(j,i) ;
      // This step might be wrong;
      //Rk=minus(multiply(transpose(U),W), MXTheta);
      /*
      if(infi_Norm(Rk)<tol)
	{
	  break;
	}
      */

      // Compute matrix P;
      //  P=matrixP(M, X);

      // Solve the SPD eigenvalue problem by modified PCG or CG;
      // PAP delta_k = PA X_k;
      // divide it into p eigenvalue problem:
      // PAP d_i = PA x_i; d_i = delta_k*ei, x_i = X_k*e_i;

      for(int i=0;i<p;i++)
	{
	  std::vector<double> x(n,0);
	  SparseMatrix<double> PAP;
	  std::vector<double> PAXi;
	  delta[i]=CG(A, M, X, PAXi, x);
	}

      // update the V = V_k+1;
      std::vector<std::vector<double>> XT;
      XT=transpose(X);
      for(int i=0;i<p;i++)
	{
	  XT=minus(XT, delta);
	  V[i]=M_orth(M,XT[i]);
	}
      //V=transpose(V);
    }

  // Compute the diagonal Matrix V'*A*V whose diagonal entries are eigenvalues of the matrix A
  // corresponding to matrix M;

  std::vector<double> eigenvalue(p);
  for(int i=0;i<p;i++)
    {
      std::vector<double> tempV, tempV2;
      tempV=multiply(A,V[i]);
      tempV2=V[i];
      eigenvalue[i]=multiply(tempV,tempV2);
    }

  return eigenvalue;

}

int main()
{
  Step3 laplace_problem;
  /////laplace_problem.run ();

  //std::stringstream result;
  std::vector<double> b(4,0);
  std::vector<std::vector<double>> A(4, b), P(4,b);
  P[0][0]=1;
  P[1][1]=1;
  P[2][2]=1;
  P[3][3]=1;
  std::vector<std::vector<double>> Q(P);

  A[0][0]=4;
  A[0][1]=1;
  A[0][2]=-2;
  A[0][3]=2;
  A[1][0]=1;
  A[1][1]=2;
  A[1][2]=0;
  A[1][3]=1;
  A[2][0]=-2;
  A[2][1]=0;
  A[2][2]=3;
  A[2][3]=-2;
  A[3][0]=2;
  A[3][1]=1;
  A[3][2]=-2;
  A[3][3]=-1;

  //std::vector<double> c={1,2,3,4};
  //transpose(A);

  std::cout<<"The matrix A is :\n";
  std::cout<<"\n";

  for(int i=0;i<A.size();i++)
    {
      for(int j=0; j<A.size();j++)
	{
	  std::cout<<A[i][j]<<" ";
	}
      std::cout<<"\n";
    }
  std::cout<<"\n";

  // multiply(A,A);

  // c=multiply(A,c);

  /*
  Householder(A,P);

  std::cout<<"The matrix A is :\n";
  std::cout<<"\n";

  for(int i=0;i<A.size();i++)
    {
      for(int j=0; j<A.size();j++)
	{
	  std::cout<<A[i][j]<<" ";
	}
      std::cout<<"\n";
    }
  std::cout<<"\n";

  QR(A,Q);

  */

  ////
  //  ATTENTION:!!!!! the original matrix A is symmetric, but after A= QR to get A_=RQ, A_ might be  // nonsymmetric!!!!!! the function is useless!!!!!!! Please edit the function.
  QRSolver(A,Q);

  std::cout<<"The matrix A is :\n";
  std::cout<<"\n";

  for(int i=0;i<A.size();i++)
    {
      for(int j=0; j<A.size();j++)
	{
	  std::cout<<A[i][j]<<" ";
	}
      std::cout<<"\n";
    }
  std::cout<<"\n";

  std::cout<<"The matrix Q is :::::::\n";
  std::cout<<"\n";

  for(int i=0;i<Q.size();i++)
    {
      for(int j=0; j<Q.size();j++)
	{
	  std::cout<<Q[i][j]<<" ";
	}
      std::cout<<"\n";
    }
  std::cout<<"\n";

  std::cout<<"check if the matrix Q is orthogonal\n";
  multiply(Q,transpose(Q));

  std::cout<<"The matrix Q*Q' is :::::::\n";
  std::cout<<"\n";

  for(int i=0;i<Q.size();i++)
    {
      for(int j=0; j<Q.size();j++)
	{
	  std::cout<<Q[i][j]<<" ";
	}
      std::cout<<"\n";
    }
  std::cout<<"\n";

  std::cout<<"hello world!"<<std::endl;
  //SparseMatrix<double> A;
  return 0;
}


/* D.R. Reynolds
   Math 6321 @ SMU
   Fall 2020  */

// Inclusions
#include <stdlib.h>
#include <iostream>
#include <armadillo>
using namespace arma;

// Example routine to show how to perform C++ output from Armadillo's
// `mat' and 'vec' classes, followed by input/plotting in Matlab/Python
int main(int argc, char **argv) {

  // get problem size from command line, otherwise set to 201
  int N = 201;
  if (argc > 1)
    N = atoi(argv[1]);
  std::cout << "\nRunning I/O test using vectors of size N = " << N << std::endl;

  // create x data
  vec x = linspace(-1.0, 1.0, N);

  // create function data (first 5 odd-degree Chebyshev polynomials)
  mat T(N,5);
  for (int j=0; j<5; j++)
    for (int i=0; i<N; i++)
      T(i,j) = cos((j*2+1.0) * acos(x(i)));

  // save data to disk
  x.save("x.txt", arma::raw_ascii);
  T.save("T.txt", arma::raw_ascii);

  std::cout << "Completed writing data to disk: x.txt and T.txt\n\n";
  return 0;
} // end main


/**
 * MIT License

 * Copyright (c) 2018 Javonne Jason Martin

 * 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.
 */

#ifndef NEURALNETWORK_NEURALNETWORK_HPP
#define NEURALNETWORK_NEURALNETWORK_HPP
#include "debug.hpp"
#include <Eigen/Dense>
/**
 * Headers for the neural network class
 *
 *
 *
 *
 */
/**
 * Explaination
 * The weight matrix _weights is an vector of matrices
 * Each element in a row is the incoming weight to a particular node ie
 * a row contains the weights incident on the next neuron
 *
 * This allows the propergation to be performed by computing _w * _a
 * Where _w is the weight matrix and _a is the input from the previous layer
 *
 * In this _a is the output from the current layer and the input to the next layer
 * _z is the input with out the activation function applied to it, for the backpropergation algorithm
 *
 */
class NeuralNetwork {
public:
    enum ACTIVATION {SIGMOID, RELU, SOFTMAX};
    /**
     * Initialises the neural network with a vector of the size of each layer.
     * @param layers, the size of each layer stored in a vector.
     * @param dense
     */
    NeuralNetwork(std::vector<int> layers, float learningRate, bool dense=true);
    /**
     * Train the neural network using the input data and the expected output data.
     * @param input, the input data <Input Layer> x <Datapoints>.
     * @param output, the expected output data <Output layer> x <datapoints>.
     * @param iterations, the number of iterations that the network should train for.
     */
    void train(Eigen::MatrixXf& input, Eigen::MatrixXf& expectedOutput, bool printIteration=false);
    /**
     * Use the network to predict the output using the input
     * @param input, the input is a matrix of <datapoints> x <Input layer>.
     * @param output, the output of the neural network <Output layer> x <datapoints>.
     */
    void predict(Eigen::MatrixXf& input, Eigen::MatrixXf& output);
    void setActivationFunction(ACTIVATION);
    ACTIVATION getActivationFunction();
    /**
     * Returns the number of neurons in the input layer.
     * @return int.
     */
    int getInputSize();
    /**
     * Returns the number of neurons in the output layer.
     * @return int.
     */
    int getOutputSize();
    /**
     * Get number of layers
     * @return
     */
    unsigned long getLayerSize();

private:
    /**
     * Apply the activation function to the input matrix
     * @param input, the matrix that the activation function is applied to elementwise
     */
    void applyActivationFunction(Eigen::MatrixXf& input);
    /**
     * Computes the cost function
     * @param x, the resulted output from the nerual network after the prediction
     * @param y, the expected result
     * @return the cost
     */
    float costFunction(float x, float y);
    /**
     * Applies the cost function to a series of inputs and ouputs
     * @param expectedOuput, the resulted output from the nerual network after the prediction
     * @param output, the expected result
     * @return a Matrix with the costs
     */
    Eigen::MatrixXf costFunction(Eigen::MatrixXf& expectedOuput, Eigen::MatrixXf& output);
    /**
     * Computes the partial derivative of the cost function as a matrix
     * @param expectedOuput, the resulted output from the nerual network after the prediction
     * @param output, the expected result
     * @return a Matrix containing the partial derivatives of the cost function
     */
    Eigen::MatrixXf costFunctionDerivative(Eigen::MatrixXf& expectedOuput, Eigen::MatrixXf& output);

    /**
     * Applies the derivative of the activation funciton
     * @param matrix
     * @return
     */
    Eigen::MatrixXf applyDerivativeActivationFunction(Eigen::MatrixXf& matrix);

    float _learningRate = 1;
    std::vector<Eigen::MatrixXf> _weights;
    std::vector<Eigen::VectorXf> _bias;
    std::vector<Eigen::MatrixXf> _delta;
    std::vector<Eigen::MatrixXf> _deltaW;
    std::vector<Eigen::VectorXf> _deltaB;
    std::vector<Eigen::MatrixXf> _z; //This is the output after applying the weights and bias (the output of the layer without the actviation function)
    std::vector<Eigen::MatrixXf> _a; //This is the output after applying the weights, bias and activation function, (the output of the layer)
    std::vector<int> _layersSize;
    ACTIVATION _activationFunction = SIGMOID;

};

/**
 * TODO Move these activation functions somewhere more logical
 *
 */
//static Eigen::MatrixXf sigmoidDerivativeMatrix(Eigen::MatrixXf& z);
static float sigmoid(float z);
static float sigmoidDerivative(float z);
static float relu(float z);
static float reluDerivative(float z) ;
static float softmax(float z);
static float softmaxDerivative(float z);

#endif //NEURALNETWORK_NEURALNETWORK_HPP


/*
 *
 * Copyright Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 * Copyright Thomas Klimpel 2008
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HEEVX_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HEEVX_HPP

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Eigendecomposition of a complex Hermitian matrix A = Q * D * Q'
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * heevx() computes selected eigenvalues and, optionally, eigenvectors
     * of a complex Hermitian matrix A.  Eigenvalues and eigenvectors can
     * be selected by specifying either a range of values or a range of
     * indices for the desired eigenvalues.
     *
     * heevx() computes the eigendecomposition of a N x N matrix
     * A = Q * D * Q',  where Q is a N x N unitary matrix and
     * D is a diagonal matrix. The diagonal element D(i,i) is an
     * eigenvalue of A and Q(:,i) is a corresponding eigenvector.
     * The eigenvalues are stored in ascending order.
     *
     * On return of heevx, A is overwritten, z contains selected eigenvectors from Q
     * and w contains selected eigenvalues from the main diagonal of D.
     *
     * int heevx (char jobz, char range, char uplo, A& a, T vl, T vu, integer_t il, integer_t iu, T abstol, integer_t& m,
     *            W& w, Z& z, IFail& ifail, Work work) {
     *    jobz :  'V' : compute eigenvectors
     *            'N' : do not compute eigenvectors
     *    range : 'A': all eigenvalues will be found.
     *            'V': all eigenvalues in the half-open interval (vl,vu] will be found.
     *            'I': the il-th through iu-th eigenvalues will be found.
     *    uplo :  'U' : only the upper triangular part of A is used on input.
     *            'L' : only the lower triangular part of A is used on input.
     */

    namespace detail {

      inline void heevx (
        char const jobz, char const range, char const uplo, integer_t const n,
        float* a, integer_t const lda,
        float const vl, float const vu, integer_t const il, integer_t const iu,
        float const abstol, integer_t& m,
        float* w, float* z, integer_t const ldz, float* work, integer_t const lwork,
        integer_t* iwork, integer_t* ifail, integer_t& info)
      {
        LAPACK_SSYEVX (
          &jobz, &range, &uplo, &n,
          a, &lda,
          &vl, &vu, &il, &iu,
          &abstol, &m,
          w, z, &ldz, work, &lwork,
          iwork, ifail, &info);
      }

      inline void heevx (
        char const jobz, char const range, char const uplo, integer_t const n,
        double* a, integer_t const lda,
        double const vl, double const vu, integer_t const il, integer_t const iu,
        double const abstol, integer_t& m,
        double* w, double* z, integer_t const ldz, double* work, integer_t const lwork,
        integer_t* iwork, integer_t* ifail, integer_t& info)
      {
        LAPACK_DSYEVX (
          &jobz, &range, &uplo, &n,
          a, &lda,
          &vl, &vu, &il, &iu,
          &abstol, &m,
          w, z, &ldz, work, &lwork,
          iwork, ifail, &info);
      }

      inline void heevx (
        char const jobz, char const range, char const uplo, integer_t const n,
        traits::complex_f* a, integer_t const lda,
        float const vl, float const vu, integer_t const il, integer_t const iu,
        float const abstol, integer_t& m,
        float* w, traits::complex_f* z, integer_t const ldz, traits::complex_f* work, integer_t const lwork,
        float* rwork, integer_t* iwork, integer_t* ifail, integer_t& info)
      {
        LAPACK_CHEEVX (
          &jobz, &range, &uplo, &n,
          traits::complex_ptr(a), &lda,
          &vl, &vu, &il, &iu, &abstol, &m, w,
          traits::complex_ptr(z), &ldz,
          traits::complex_ptr(work), &lwork,
          rwork, iwork, ifail, &info);
      }

      inline void heevx (
        char const jobz, char const range, char const uplo, integer_t const n,
        traits::complex_d* a, integer_t const lda,
        double const vl, double const vu, integer_t const il, integer_t const iu,
        double const abstol, integer_t& m,
        double* w, traits::complex_d* z, integer_t const ldz, traits::complex_d* work, integer_t const lwork,
        double* rwork, integer_t* iwork, integer_t* ifail, integer_t& info)
      {
        LAPACK_ZHEEVX (
          &jobz, &range, &uplo, &n,
          traits::complex_ptr(a), &lda,
          &vl, &vu, &il, &iu, &abstol, &m, w,
          traits::complex_ptr(z), &ldz,
          traits::complex_ptr(work), &lwork,
          rwork, iwork, ifail, &info);
      }
    } // namespace detail

    namespace detail {

      template <int N>
      struct Heevx{};

      /// Handling of workspace in the case of one workarray.
      template <>
      struct Heevx< 1 > {
        // Function that allocates temporary arrays
        template <typename T, typename R>
        void operator() (
          char const jobz, char const range, char const uplo, integer_t const n,
          T* a, integer_t const lda,
          R vl, R vu, integer_t const il, integer_t const iu,
          R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz, minimal_workspace, integer_t* ifail, integer_t& info) {

          traits::detail::array<T> work( 8*n );
          traits::detail::array<integer_t> iwork( 5*n );

          heevx( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, m, w, z, ldz,
            traits::vector_storage (work), traits::vector_size (work),
            traits::vector_storage (iwork),
            ifail, info);
        }
        // Function that allocates temporary arrays
        template <typename T, typename R>
        void operator() (
          char const jobz, char const range, char const uplo, integer_t const n,
          T* a, integer_t const lda,
          R vl, R vu, integer_t const il, integer_t const iu,
          R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz, optimal_workspace, integer_t* ifail, integer_t& info) {

          traits::detail::array<integer_t> iwork( 5*n );

          T workspace_query;
          heevx( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, m, w, z, ldz,
            &workspace_query, -1,
            traits::vector_storage (iwork),
            ifail, info);

          traits::detail::array<T> work( traits::detail::to_int( workspace_query ) );

          heevx( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, m, w, z, ldz,
            traits::vector_storage (work), traits::vector_size (work),
            traits::vector_storage (iwork),
            ifail, info);
        }
        // Function that uses given workarrays
        template <typename T, typename R, typename W, typename WI>
        void operator() (
          char const jobz, char const range, char const uplo, integer_t const n,
          T* a, integer_t const lda,
          R vl, R vu, integer_t const il, integer_t const iu,
          R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz, detail::workspace2<W, WI> work, integer_t* ifail, integer_t& info) {

          assert (traits::vector_size (work.select(T())) >= 8*n);
          assert (traits::vector_size (work.select(integer_t())) >= 5*n);

          heevx( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, m, w, z, ldz,
            traits::vector_storage (work.select(T())), traits::vector_size (work.select(T())),
            traits::vector_storage (work.select(integer_t())),
            ifail, info);
        }
      };

      /// Handling of workspace in the case of two workarrays.
      template <>
      struct Heevx< 2 > {
        // Function that allocates temporary arrays
        template <typename T, typename R>
        void operator() (
          char const jobz, char const range, char const uplo, integer_t const n,
          T* a, integer_t const lda,
          R vl, R vu, integer_t const il, integer_t const iu,
          R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz, minimal_workspace, integer_t* ifail, integer_t& info) {

          traits::detail::array<T> work( 2*n );
          traits::detail::array<R> rwork( 7*n );
          traits::detail::array<integer_t> iwork( 5*n );

          heevx( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, m, w, z, ldz,
            traits::vector_storage (work), traits::vector_size (work),
            traits::vector_storage (rwork),
            traits::vector_storage (iwork),
            ifail, info);
        }
        // Function that allocates temporary arrays
        template <typename T, typename R>
        void operator() (
          char const jobz, char const range, char const uplo, integer_t const n,
          T* a, integer_t const lda,
          R vl, R vu, integer_t const il, integer_t const iu,
          R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz, optimal_workspace, integer_t* ifail, integer_t& info) {

          traits::detail::array<R> rwork( 7*n );
          traits::detail::array<integer_t> iwork( 5*n );

          T workspace_query;
          heevx( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, m, w, z, ldz,
            &workspace_query, -1,
            traits::vector_storage (rwork),
            traits::vector_storage (iwork),
            ifail, info);

          traits::detail::array<T> work( traits::detail::to_int( workspace_query ) );

          heevx( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, m, w, z, ldz,
            traits::vector_storage (work), traits::vector_size (work),
            traits::vector_storage (rwork),
            traits::vector_storage (iwork),
            ifail, info);
        }
        // Function that uses given workarrays
        template <typename T, typename R, typename WC, typename WR, typename WI>
        void operator() (
          char const jobz, char const range, char const uplo, integer_t const n,
          T* a, integer_t const lda,
          R vl, R vu, integer_t const il, integer_t const iu,
          R abstol, integer_t& m,
          R* w, T* z, integer_t const ldz, detail::workspace3<WC, WR, WI> work, integer_t* ifail, integer_t& info) {

          assert (traits::vector_size (work.select(T())) >= 2*n);
          assert (traits::vector_size (work.select(R())) >= 7*n);
          assert (traits::vector_size (work.select(integer_t())) >= 5*n);

          heevx( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, m, w, z, ldz,
            traits::vector_storage (work.select(T())), traits::vector_size (work.select(T())),
            traits::vector_storage (work.select(R())),
            traits::vector_storage (work.select(integer_t())),
            ifail, info);
        }
      };
    } // namespace detail

    template <typename A, typename T, typename W, typename Z, typename IFail, typename Work>
    int heevx (
      char jobz, char range, A& a, T vl, T vu, integer_t il, integer_t iu, T abstol, integer_t& m,
      W& w, Z& z, IFail& ifail, Work work = optimal_workspace() ) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      typedef typename A::value_type                               value_type ;
      typedef typename traits::type_traits< value_type >::real_type real_type ;
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<A>::matrix_structure,
        traits::hermitian_t
      >::value || (boost::is_same<
        typename traits::matrix_traits<A>::matrix_structure,
        traits::symmetric_t
      >::value && boost::is_same<value_type, real_type>::value)));
#endif

      integer_t const n = traits::matrix_size1 (a);
      assert (traits::matrix_size2 (a) == n);
      assert (traits::vector_size (w) == n);
      assert (traits::vector_size (ifail) == n);
      assert ( range=='A' || range=='V' || range=='I' );
      char uplo = traits::matrix_uplo_tag (a);
      assert ( uplo=='U' || uplo=='L' );
      assert ( jobz=='N' || jobz=='V' );

      integer_t info;
      detail::Heevx< n_workspace_args<typename A::value_type>::value >() (
        jobz, range, uplo, n,
        traits::matrix_storage (a),
        traits::leading_dimension (a),
        vl, vu, il, iu, abstol, m,
        traits::vector_storage (w),
        traits::matrix_storage (z),
        traits::leading_dimension (z),
        work,
        traits::vector_storage (ifail),
        info);
      return info;
    }
  }

}}}

#endif


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __NUMERIC_AD_RT_AD_HPP__
#define __NUMERIC_AD_RT_AD_HPP__

#include <iostream>
#include <Eigen/Dense>
#include <TooN/TooN.h>
#include <libv/lma/ttt/traits/naming.hpp>
#include <cmath>

namespace AdRt
{
//   Eigen::Matrix<T,N,1>
  template<class T, std::size_t N > struct Ad
  {
    typedef T type;
    static const std::size_t dim = N;

    typedef TooN::Vector<N,T> Array;
    T value;
    Array infinite;

    Ad():value(0) {}

    Ad(type value_) : value(value_)
    {
//       infinite.fill(0);
      infinite = TooN::Zeros;
    }

    Ad(type value_, std::size_t k)
    {
      value = value_;
//       infinite.fill(0);
      infinite = TooN::Zeros;
      infinite[k] = type(1);
    }

    Ad(type value_, const Array& infinite_): value(value_),infinite(infinite_) {}

//     bool operator<(type value_) const
//     {
//       return value < value_;
//     }
//
//     bool operator>(type value_) const
//     {
//       return value > value_;
//     }
//
//     bool operator<=(type value_) const
//     {
//       return value <= value_;
//     }
//
//     bool operator<=(const Ad& ad) const
//     {
//       return value <= ad.value;
//     }
//
    bool operator>(const Ad& ad) const
    {
      return value > ad.value;
    }

    Ad<T,N>& operator+=(const Ad<type, dim>& st) { *this = *this + st; return *this; }
//     Ad<T,N>& operator-=(const Ad<type, dim>& st) { return *this -= st; }
//     Ad<T,N>& operator*=(const Ad<type, dim>& st) { return *this *= st; }
//     Ad<T,N>& operator/=(const Ad<type, dim>& st) { return *this /= st;}
  };

  template<class T, std::size_t N> bool operator==(const Ad<T,N>& st, const T& scalar)
  {
    return st.value == scalar;
  }

  template<class T, std::size_t N> bool operator!=(const Ad<T,N>& st, const T& scalar)
  {
    return st.value != scalar;
  }

  //! Arithmetic for Ad
  template<class T, std::size_t N> const Ad<T,N>& operator+(const Ad<T,N>& st)
  {
    return st;
  }

  template<class T, std::size_t N> Ad<T,N> operator-(const Ad<T,N>& st)
  {
    return Ad<T,N>(-st.value,-st.infinite);
  }

  template<class T, std::size_t N> Ad<T,N> operator+(const Ad<T,N>& s, const Ad<T,N>& t)
  {
    return Ad<T,N>(s.value + t.value, s.infinite + t.infinite);
  }

  template<class T, std::size_t N> Ad<T,N> operator+(const Ad<T,N>& s, const T& value)
  {
    return Ad<T,N>(s.value + value, s.infinite);
  }

  template<class T, std::size_t N> Ad<T, N> operator+(const T& value, const Ad<T, N>& s)
  {
    return s + value;
  }

  template<class T, std::size_t N> Ad<T,N> operator-(const Ad<T,N>& s, const Ad<T,N>& t)
  {
    return Ad<T,N>(s.value - t.value, s.infinite - t.infinite);
  }

  template<class T, std::size_t N> Ad<T,N> operator-(const Ad<T,N>& s, const T& value)
  {
    return Ad<T,N>(s.value - value, s.infinite);
  }

  template<class T, std::size_t N> Ad<T,N> operator-(const T& value, const Ad<T,N>& s)
  {
    return -s + value;
  }

  template<class T, std::size_t N> Ad<T,N> operator*(const Ad<T,N>& s, const Ad<T,N>& t)
  {
    return Ad<T,N>(s.value * t.value , s.value * t.infinite + s.infinite * t.value );
  }

  template<class T, std::size_t N> Ad<T,N> operator*(const Ad<T,N>& s, const T& value)
  {
    return Ad<T,N>(s.value * value, s.infinite * value);
  }

  template<class T, std::size_t N> Ad<T,N> operator*(const T& value, const Ad<T,N>& s)
  {
    return s * value;
  }

  template<class T, std::size_t N> Ad<T,N> operator/(const Ad<T,N>& s, const Ad<T,N>& t)
  {
    return Ad<T,N>(s.value / t.value, (s.infinite - s.value / t.value * t.infinite ) / t.value);
  }

  template<class T, std::size_t N> Ad<T,N> operator/(const T& value, const Ad<T,N>& s)
  {
    return Ad<T,N>(value / s.value, - value * s.infinite / ( s.value * s.value ) );
  }

  template<class T, std::size_t N> Ad<T,N> operator/(const Ad<T,N>& s, const T& value)
  {
    return Ad<T,N>(s.value / value, s.infinite / value);
  }

//! trigonometric and others functions

  template <class T, std::size_t N> Ad<T, N> abs(const Ad<T, N>& f)
  {
    return Ad<T,N>(f.value < T(0.0) ? -f : f);
  }

  template <class T, std::size_t N> Ad<T, N> log(const Ad<T, N>& f)
  {
    return Ad<T, N>(log(f.value),f.infinite / f.value);
  }

  template <class T, std::size_t N> Ad<T, N> exp(const Ad<T, N>& f)
  {
    Ad<T, N> g;
    g.value = exp(f.value);
    g.infinite = g.value * f.infinite;
    return g;
  }

  template <class T, std::size_t N> Ad<T, N> sqrt(const Ad<T, N>& f)
  {
    Ad<T, N> g;
    g.value = std::sqrt(f.value);
    g.infinite = f.infinite / (T(2.0) * g.value);
    return g;
  }

  template <class T, std::size_t N> Ad<T, N> cos(const Ad<T, N>& f)
  {
    using std::sin;using std::cos;
    Ad<T, N> g;
    g.value = cos(f.value);
    T sin_a = sin(f.value);
    g.infinite = - sin_a * f.infinite;
    return g;
  }

  template <class T, std::size_t N> Ad<T, N> acos(const Ad<T, N>& f)
  {
    return Ad<T, N>(acos(f.value),- T(1.0) / sqrt(T(1.0) - f.value * f.value) * f.infinite);
  }

  template <class T, std::size_t N> Ad<T, N> sin(const Ad<T, N>& f)
  {
    using std::sin;using std::cos;
    Ad<T, N> g;
    g.value = sin(f.value);
    T cos_a = cos(f.value);
    g.infinite = cos_a * f.infinite;
    return g;
  }

  template <class T, std::size_t N> Ad<T, N> asin(const Ad<T, N>& f)
  {
    return Ad<T, N>(asin(f.a),T(1.0) / sqrt(T(1.0) - f.a * f.a) * f.v);
  }

}//! eon AdRt

  template<class T, std::size_t N> std::ostream& operator<<(std::ostream& o, const AdRt::Ad<T,N>& st)
  {
    return o << " Ad<" << ttt::name<T>() << "," << N << ":value = " << st.value << " ; [" << st.infinite << "]";
  }

#endif


/******************************************************************************
 * Copyright 2017 Baidu Robotic Vision Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/
#include "feature_utils.h"
#include <Eigen/Dense>

#define SUBPIX_VERBOSE 0

namespace XP {
namespace align {

using Eigen::Vector2f;
using Eigen::Vector3f;
using Eigen::Matrix2f;
using Eigen::Matrix3f;
using Eigen::Matrix3i;
const int W_BITS = 14;
const int half_patch_size = 4;
const int patch_size = 8;
const int patch_area = 64;
bool align1D(const cv::Mat& cur_img,
             const Vector2f& dir,  // direction in which the patch is allowed to move
             uint8_t* ref_patch_with_border,
             uint8_t* ref_patch,
             const int n_iter,
             Vector2f* cur_px_estimate,
             float* h_inv) {
  bool converged = false;

  // compute derivative of template and prepare inverse compositional
  float __attribute__((__aligned__(16))) ref_patch_dv[patch_area];
  Matrix2f H; H.setZero();

  // compute gradient and hessian
  const int ref_step = patch_size + 2;
  float* it_dv = ref_patch_dv;
  for (int y=0; y < patch_size; ++y) {
    uint8_t* it = ref_patch_with_border + (y + 1) * ref_step + 1;
    for (int x = 0; x < patch_size; ++x, ++it, ++it_dv) {
      Vector2f J;
      J[0] = 0.5*(dir[0] * (it[1] - it[-1]) + dir[1] * (it[ref_step] - it[-ref_step]));
      J[1] = 1;
      *it_dv = J[0];
      H += J * J.transpose();
    }
  }
  *h_inv = 1.0 / H(0, 0) * patch_size * patch_size;
  Matrix2f Hinv = H.inverse();
  float mean_diff = 0;

  // Compute pixel location in new image:
  float u = (*cur_px_estimate)(0);
  float v = (*cur_px_estimate)(1);

  // termination condition
  const float min_update_squared = 0.03 * 0.03;
  const int cur_step = cur_img.step.p[0];
  float chi2 = 0;
  Vector2f update; update.setZero();
  for (int iter = 0; iter < n_iter; ++iter) {
    int u_r = floor(u);
    int v_r = floor(v);
    if (u_r < half_patch_size || v_r < half_patch_size ||
        u_r >= cur_img.cols-half_patch_size || v_r >= cur_img.rows - half_patch_size) {
      break;
    }
    if (std::isnan(u) || std::isnan(v)) {
      // TODO(SVO): very rarely this can happen, maybe H is singular? should not be at corner.
      return false;
    }

    // compute interpolation weights
    float subpix_x = u - u_r;
    float subpix_y = v - v_r;
    float wTL = (1.0 - subpix_x) * (1.0 - subpix_y);
    float wTR = subpix_x * (1.0 - subpix_y);
    float wBL = (1.0 - subpix_x) * subpix_y;
    float wBR = subpix_x * subpix_y;

    // loop through search_patch, interpolate
    uint8_t* it_ref = ref_patch;
    float* it_ref_dv = ref_patch_dv;
    float new_chi2 = 0.0;
    Vector2f Jres; Jres.setZero();
    for (int y = 0; y < patch_size; ++y) {
      uint8_t* it = static_cast<uint8_t*>(cur_img.data) +
          (v_r + y - half_patch_size) * cur_step + u_r - half_patch_size;
      for (int x = 0; x < patch_size; ++x, ++it, ++it_ref, ++it_ref_dv) {
        float search_pixel = wTL * it[0] + wTR * it[1] + wBL * it[cur_step] + wBR * it[cur_step+1];
        float res = search_pixel - *it_ref + mean_diff;
        Jres[0] -= res * (*it_ref_dv);
        Jres[1] -= res;
        new_chi2 += res * res;
      }
    }

    if (iter > 0 && new_chi2 > chi2) {
#if SUBPIX_VERBOSE
      cout << "error increased." << endl;
#endif
      u -= update[0];
      v -= update[1];
      break;
    }

    chi2 = new_chi2;
    update = Hinv * Jres;
    u += update[0] * dir[0];
    v += update[0] * dir[1];
    mean_diff += update[1];

#if SUBPIX_VERBOSE
    VLOG(2) << "Iter " << iter << ":"
            << "\t u=" << u << ", v=" << v
            << "\t update = " << update[0] << ", " << update[1]
            << "\t new chi2 = " << new_chi2;
#endif

    if (update[0] * update[0] + update[1] * update[1] < min_update_squared) {
#if SUBPIX_VERBOSE
      VLOG(2) << "converged.";
#endif
      converged = true;
      break;
    }
  }

  *cur_px_estimate << u, v;
  return converged;
}

 //逆向组合法进行2D图像对齐,这个好像是在LK十年那个论文里有说过
 //输入右相机的图像,从左相机变换到右相机上的patch_border,从左相机变换到右相机上的patch,最大迭代次数
 //输出最终块匹配残差最小的右目中特征点的像素坐标
 //残差: r = I(p_cur) - I(p_ref) + m ,其中I(*)表示在某个像素位置的光度, m是均值差.cur是右相机,ref是左相机
 //优化变量是p_ref,m，这里为什么是p_ref而不是p_cur,是因为如果用p_cur，每次增量以后还需要再计算一次这个的J,用p_ref只用算一次,均值差也做为优化变量是防止噪声
bool align2D(const cv::Mat& cur_img,
             uint8_t* ref_patch_with_border,
             uint8_t* ref_patch,
             const int n_iter,
             Vector2f* cur_px_estimate,
             bool no_simd)
             {
/*
#ifdef __ARM_NEON__
  Vector2f cur_px_estimate_neon = *cur_px_estimate;
  if (!no_simd) {
    align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, &cur_px_estimate_neon);
  }
#endif
*/
  bool converged = false;
// 模板图像对像素 x,y 坐标进行求导
  // compute derivative of template and prepare inverse compositional
  float __attribute__((__aligned__(16))) ref_patch_dx[patch_area];
  float __attribute__((__aligned__(16))) ref_patch_dy[patch_area];
  Matrix3f H; H.setZero();

  // compute gradient and hessian
  const int ref_step = patch_size+2;//patch框边长
     // 图像导数的指针
  float* it_dx = ref_patch_dx;
  float* it_dy = ref_patch_dy;
  //构造H矩阵
  for (int y = 0; y < patch_size; ++y) {
    uint8_t* it = ref_patch_with_border + (y + 1) * ref_step + 1;
    for (int x = 0; x < patch_size; ++x, ++it, ++it_dx, ++it_dy) {
      Vector3f J;
      //p_ref像素值关于位置，其实就是梯度
      J[0] = 0.5 * (it[1] - it[-1]);
      J[1] = 0.5 * (it[ref_step] - it[-ref_step]);
      J[2] = 1;//均值差的导数
      *it_dx = J[0];
      *it_dy = J[1];
      H += J*J.transpose();
    }
  }
  Matrix3f Hinv = H.inverse();
  float mean_diff = 0;

  // Compute pixel location in new image:
     //初值
  float u = (*cur_px_estimate)(0);
  float v = (*cur_px_estimate)(1);

  // termination condition
  const float min_update_squared = 0.03 * 0.03;//收敛条件
  const int cur_step = cur_img.step.p[0];
  float chi2 = std::numeric_limits<int>::max();
  Vector3f update; update.setZero();//优化变量的增量
  for (int iter = 0; iter < n_iter; ++iter) //优化开始
  {
    int u_r = floor(u);
    int v_r = floor(v);
    //边缘处跳过
    if (u_r < half_patch_size || v_r < half_patch_size ||
        u_r >= cur_img.cols-half_patch_size || v_r >= cur_img.rows-half_patch_size) {
      break;
    }
    if (std::isnan(u) || std::isnan(v)) {
      // TODO(SVO): very rarely this can happen, maybe H is singular? should not be at corner
      return false;
    }

    // compute interpolation weights
    //双线性插值I(i+u,j+v) = (1-u)(1-v)I(i,j) + (1-u)vI(i,j+1) + u(1-v)I(i+1,j) + uvI(i+1,j+1) 以算出这个特征点在右目图像中的像素值
    float subpix_x = u - u_r;
    float subpix_y = v - v_r;
    float wTL = (1.0 - subpix_x) * (1.0 - subpix_y);
    float wTR = subpix_x * (1.0 - subpix_y);
    float wBL = (1.0 - subpix_x) * subpix_y;
    float wBR = subpix_x * subpix_y;

    // loop through search_patch, interpolate
    uint8_t* it_ref = ref_patch;// 左图像 patch指针
    float* it_ref_dx = ref_patch_dx; // 左图像 patch x方向导数指针
    float* it_ref_dy = ref_patch_dy;// 左图像 patch y方向导数指针
    float new_chi2 = 0.0;
    Vector3f Jres; Jres.setZero();
    for (int y = 0; y < patch_size; ++y)
    {
      uint8_t* it = static_cast<uint8_t*>(cur_img.data) +
          (v_r + y - half_patch_size) * cur_step + u_r - half_patch_size;
      for (int x = 0; x < patch_size; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy)
      {
          //残差r = I(p_cur) - I(p_ref) + m
        float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];//双线性插值得到curimg中的像素值
        float res = search_pixel - *it_ref + mean_diff;
        //计算 H * deltax = -Jr中的Jres
        Jres[0] -= res*(*it_ref_dx);
        Jres[1] -= res*(*it_ref_dy);
        Jres[2] -= res;
        new_chi2 += res*res;// 卡方
      }
    }

    /*
    if(iter > 0 && new_chi2 > chi2) {
#if SUBPIX_VERBOSE
      VLOG(2) << "error increased.";
#endif
      u -= update[0];
      v -= update[1];
      break;
    }
    */
    chi2 = new_chi2;

    //更新变量
    //deltax = H.inv * -Jr
    update = Hinv * Jres;
    u += update[0];
    v += update[1];
    mean_diff += update[2];

#if SUBPIX_VERBOSE
    VLOG(2) << "Iter " << iter << ":"
            << "\t u=" << u << ", v=" << v
            << "\t update = " << update[0] << ", " << update[1]
            << "\t chi2 = " << chi2;
#endif

    if (update[0] * update[0] + update[1] * update[1] < min_update_squared)
    {
        //满足收敛条件
#if SUBPIX_VERBOSE
      VLOG(2) << "converged.";
#endif
      converged = true;
      break;
    }
  }
  *cur_px_estimate << u, v;
  return converged;
}

// TODO(mingyu): Add the SSE and NEON version
bool align2D_SSE2(const cv::Mat& cur_img,
                  uint8_t* ref_patch_with_border,
                  uint8_t* ref_patch,
                  const int n_iter,
                  Vector2f* cur_px_estimate) {
  LOG(FATAL) << "align2D_SSE2 is not implemented";
  return false;
}
#ifdef __ARM_NEON__
bool align2D_NEON(const cv::Mat& cur_img,
                  const uint8_t* ref_patch_with_border,
                  const uint8_t* ref_patch,
                  const int n_iter,
                  Vector2f* cur_px_estimate) {
  bool converged = false;

  // compute derivative of template and prepare inverse compositional
  /*************************************
   * memory layout
   * dx dy 1 dx dy 1 dx dy 1 ...
   */
  float __attribute__((__aligned__(16))) ref_patch_dx_dy[patch_area * 3];
  Matrix3f H = Matrix3f::Zero();

  // compute gradient
  const int ref_step = patch_size + 2;
  for (int n = 0; n < patch_size; ++n) {
    const uint8_t* it = ref_patch_with_border + (n + 1) * ref_step + 1;
    int16x8_t horizontal_diff, vertical_diff;
    {
      int16x8_t raw_data_mid_left = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(it - 1)));
      int16x8_t raw_data_mid_right = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(it + 1)));
      int16x8_t raw_data_top = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(it - ref_step)));
      int16x8_t raw_data_bot = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(it + ref_step)));
      horizontal_diff = vsubq_s16(raw_data_mid_right, raw_data_mid_left);
      vertical_diff = vsubq_s16(raw_data_bot, raw_data_top);
    }
    float32x4x3_t left4 = {
      vmulq_n_f32(vcvtq_f32_s32(vmovl_s16(vget_low_s16(horizontal_diff))), 0.5),
      vmulq_n_f32(vcvtq_f32_s32(vmovl_s16(vget_low_s16(vertical_diff))), 0.5),
      vdupq_n_f32(1.f)
    };
    vst3q_f32(ref_patch_dx_dy + n * 24, left4);
    float32x4x3_t right4 = {
      vmulq_n_f32(vcvtq_f32_s32(vmovl_s16(vget_high_s16(horizontal_diff))), 0.5),
      vmulq_n_f32(vcvtq_f32_s32(vmovl_s16(vget_high_s16(vertical_diff))), 0.5),
      vdupq_n_f32(1.f)
    };
    vst3q_f32(ref_patch_dx_dy + n * 24 + 12, right4);
  }
  // compute hessian
  float* ptr_dxy = ref_patch_dx_dy;
  for (int cnt = patch_size * patch_size; cnt > 0; --cnt) {
    Eigen::Map<Vector3f> J(ptr_dxy);
    H += J * J.transpose();
    ptr_dxy += 3;
  }

  Matrix3f Hinv = H.inverse();
  float mean_diff = 0.f;

  // Compute pixel location in new image:
  float u = (*cur_px_estimate)(0);
  float v = (*cur_px_estimate)(1);

  // termination condition
  const float min_update_squared = 0.03 * 0.03;
  const int cur_step = cur_img.step.p[0];
  float chi2 = std::numeric_limits<int>::max();
  Vector3f update = Vector3f::Zero();
  for (int iter = 0; iter < n_iter; ++iter) {
    int u_r = floor(u);
    int v_r = floor(v);
    if (u_r < half_patch_size ||
        v_r < half_patch_size ||
        u_r >= cur_img.cols - half_patch_size ||
        v_r >= cur_img.rows - half_patch_size) {
      break;
    }
    if (std::isnan(u) || std::isnan(v)) {
      // TODO(SVO): very rarely this can happen, maybe H is singular? should not be at corner
      return false;
    }

    // compute interpolation weights
    float subpix_x = u - u_r;
    float subpix_y = v - v_r;

    int iw00 = cvRound((1.f - subpix_x) * (1.f - subpix_y) * (1 << W_BITS));
    int iw01 = cvRound(subpix_x * (1.f - subpix_y) * (1 << W_BITS));
    int iw10 = cvRound((1.f - subpix_x) * subpix_y * (1 << W_BITS));
    int iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
    const int16x4_t viw00 = vdup_n_s16(static_cast<int16_t>(iw00));
    const int16x4_t viw01 = vdup_n_s16(static_cast<int16_t>(iw01));
    const int16x4_t viw10 = vdup_n_s16(static_cast<int16_t>(iw10));
    const int16x4_t viw11 = vdup_n_s16(static_cast<int16_t>(iw11));
    const int32x4_t shift = vdupq_n_s32(-W_BITS);
    const float32x4_t vmean_diff = vdupq_n_f32(mean_diff);

    // loop through search_patch, interpolate
    const uint8_t* it_ref = ref_patch;
    float new_chi2 = 0.f;
    Vector3f Jres = Vector3f::Zero();
    for (int n = 0; n < patch_size; ++n, it_ref += 8) {
      uint8_t* it = static_cast<uint8_t*>(cur_img.data) +
          (v_r + n - half_patch_size) * cur_step + u_r - half_patch_size;
      int16x8_t topleft = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(it)));
      int16x8_t topright = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(it + 1)));
      int16x8_t botleft = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(it + cur_step)));
      int16x8_t botright = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(it + cur_step + 1)));

      int32x4_t left_half1 = vaddq_s32(
                  vmull_s16(vget_low_s16(topleft), viw00),
                  vmull_s16(vget_low_s16(topright), viw01));
      int32x4_t left_half2 = vaddq_s32(
                  vmull_s16(vget_low_s16(botleft), viw10),
                  vmull_s16(vget_low_s16(botright), viw11));

      int32x4_t right_half1 = vaddq_s32(
                  vmull_s16(vget_high_s16(topleft), viw00),
                  vmull_s16(vget_high_s16(topright), viw01));
      int32x4_t right_half2 = vaddq_s32(
                  vmull_s16(vget_high_s16(botleft), viw10),
                  vmull_s16(vget_high_s16(botright), viw11));

      int16x8_t v_it_ref8 = vreinterpretq_s16_u16(vmovl_u8(vld1_u8(it_ref)));
      float32x4x2_t search_pixel = {
        vcvtq_f32_s32(vqrshlq_s32(vaddq_s32(left_half1, left_half2), shift)),
        vcvtq_f32_s32(vqrshlq_s32(vaddq_s32(right_half1, right_half2), shift))
      };

      search_pixel.val[0] = vsubq_f32(search_pixel.val[0],
                           vcvtq_f32_s32(vmovl_s16(vget_low_s16(v_it_ref8))));
      search_pixel.val[1] = vsubq_f32(search_pixel.val[1],
                           vcvtq_f32_s32(vmovl_s16(vget_high_s16(v_it_ref8))));
      search_pixel.val[0] = vaddq_f32(search_pixel.val[0], vmean_diff);
      search_pixel.val[1] = vaddq_f32(search_pixel.val[1], vmean_diff);

      float32x4x3_t dxy_left = vld3q_f32(ref_patch_dx_dy + n * 24);
      float32x4x3_t dxy_right = vld3q_f32(ref_patch_dx_dy + n * 24 + 12);

      // dx
      dxy_left.val[0]  = vmulq_f32(dxy_left.val[0], search_pixel.val[0]);
      dxy_right.val[0] = vmulq_f32(dxy_right.val[0], search_pixel.val[1]);
      // dy
      dxy_left.val[1]  = vmulq_f32(dxy_left.val[1], search_pixel.val[0]);
      dxy_right.val[1] = vmulq_f32(dxy_right.val[1], search_pixel.val[1]);

      Jres[2] -= (vgetq_lane_f32(search_pixel.val[0], 0) + vgetq_lane_f32(search_pixel.val[0], 1) +
                  vgetq_lane_f32(search_pixel.val[0], 2) + vgetq_lane_f32(search_pixel.val[0], 3) +
                  vgetq_lane_f32(search_pixel.val[1], 0) + vgetq_lane_f32(search_pixel.val[1], 1) +
                  vgetq_lane_f32(search_pixel.val[1], 2) + vgetq_lane_f32(search_pixel.val[1], 3));

      search_pixel.val[0] = vmulq_f32(search_pixel.val[0], search_pixel.val[0]);
      search_pixel.val[1] = vmulq_f32(search_pixel.val[1], search_pixel.val[1]);
      dxy_left.val[0] = vaddq_f32(dxy_left.val[0], dxy_right.val[0]);
      dxy_left.val[1] = vaddq_f32(dxy_left.val[1], dxy_right.val[1]);
      Jres[0] -= (vgetq_lane_f32(dxy_left.val[0], 0)  + vgetq_lane_f32(dxy_left.val[0], 1) +
                  vgetq_lane_f32(dxy_left.val[0], 2)  + vgetq_lane_f32(dxy_left.val[0], 3));
      Jres[1] -= (vgetq_lane_f32(dxy_left.val[1], 0)  + vgetq_lane_f32(dxy_left.val[1], 1) +
                  vgetq_lane_f32(dxy_left.val[1], 2)  + vgetq_lane_f32(dxy_left.val[1], 3));
      search_pixel.val[0] = vaddq_f32(search_pixel.val[0], search_pixel.val[1]);
      new_chi2 += vgetq_lane_f32(search_pixel.val[0], 0) + vgetq_lane_f32(search_pixel.val[0], 1) +
                  vgetq_lane_f32(search_pixel.val[0], 2) + vgetq_lane_f32(search_pixel.val[0], 3);
    }
    /*
    if(iter > 0 && new_chi2 > chi2) {
#if SUBPIX_VERBOSE
      VLOG(2) << "error increased.";
#endif
      u -= update[0];
      v -= update[1];
      break;
    }
    */
    chi2 = new_chi2;

    update = Hinv * Jres;
    u += update[0];
    v += update[1];
    mean_diff += update[2];

#if SUBPIX_VERBOSE
    VLOG(2) << "Iter " << iter << ":"
            << "\t u=" << u << ", v=" << v
            << "\t update = " << update[0] << ", " << update[1]
            << "\t chi2 = " << chi2;
#endif

    if (update[0] * update[0] + update[1] * update[1] < min_update_squared) {
#if SUBPIX_VERBOSE
      VLOG(2) << "converged.";
#endif
      converged = true;
      break;
    }
  }

  *cur_px_estimate << u, v;
  return converged;
}
#endif
}  // namespace align
}  // namespace XP


/*
 * StrainInvariants.cpp
 *
 *  Created on: Dec 23, 2018
 *      Author: lorenzo
 */

#include "StrainInvariants.h"

#define QUICKHULL_IMPLEMENTATION
#include <quickhull/quickhull.h>
#include <Eigen/Eigenvalues>

#include <iostream>
#include <sstream>

namespace ashell {

using std::endl;

StrainInvariants::StrainInvariants(std::string n_name) :
		Observable(n_name) {

}

StrainInvariants::~StrainInvariants() {

}

std::string StrainInvariants::as_string() {
	return _line;
}

void StrainInvariants::_observe(ullint step) {
	std::stringstream ss;

	std::vector<qh_vertex_t> vertices(_particles->N());

	auto poss = _particles->positions();
	vec3 com(0., 0., 0.);
	for(uint i = 0; i < _particles->N(); i++) {
		com += poss[i];
	}
	com /= _particles->N();

	for(uint i = 0; i < _particles->N(); i++) {
		vec3 v = poss[i] - com;
		vertices[i].x = v[0];
		vertices[i].y = v[1];
		vertices[i].z = v[2];
	}

	qh_mesh_t mesh = qh_quickhull3d(vertices.data(), _particles->N());

	vec3 ch_com(0., 0., 0.);
	double ch_volume = 0.;
	for(uint i = 0; i < mesh.nindices; i += 3) {
		vec3 p1(mesh.vertices[mesh.indices[i + 0]].x, mesh.vertices[mesh.indices[i + 0]].y, mesh.vertices[mesh.indices[i + 0]].z);
		vec3 p2(mesh.vertices[mesh.indices[i + 1]].x, mesh.vertices[mesh.indices[i + 1]].y, mesh.vertices[mesh.indices[i + 1]].z);
		vec3 p3(mesh.vertices[mesh.indices[i + 2]].x, mesh.vertices[mesh.indices[i + 2]].y, mesh.vertices[mesh.indices[i + 2]].z);

		ch_volume += (p1.dot(p2.cross(p3))) / 6.;
		ch_com += (p1 + p2 + p3);
	}

	ch_com /= mesh.nindices;

	// Gyration tensor
	mat3 gyration_tensor = mat3::Zero();
	for(int i = 0, j = 0; i < (int)mesh.nindices; i += 3, j++) {
		vec3 p1(mesh.vertices[mesh.indices[i + 0]].x, mesh.vertices[mesh.indices[i + 0]].y, mesh.vertices[mesh.indices[i + 0]].z);
		vec3 p2(mesh.vertices[mesh.indices[i + 1]].x, mesh.vertices[mesh.indices[i + 1]].y, mesh.vertices[mesh.indices[i + 1]].z);
		vec3 p3(mesh.vertices[mesh.indices[i + 2]].x, mesh.vertices[mesh.indices[i + 2]].y, mesh.vertices[mesh.indices[i + 2]].z);
		vec3 triangle_com = (p1 + p2 + p3) / 3. - ch_com;

		gyration_tensor(0, 0) += SQR(triangle_com[0]);
		gyration_tensor(0, 1) += triangle_com[0] * triangle_com[1];
		gyration_tensor(0, 2) += triangle_com[0] * triangle_com[2];

		gyration_tensor(1, 1) += SQR(triangle_com[1]);
		gyration_tensor(1, 2) += triangle_com[1] * triangle_com[2];

		gyration_tensor(2, 2) += SQR(triangle_com[2]);
	}
	gyration_tensor(1, 0) = gyration_tensor(0, 1);
	gyration_tensor(2, 0) = gyration_tensor(0, 2);
	gyration_tensor(2, 1) = gyration_tensor(1, 2);
	gyration_tensor /= mesh.nindices / 3.;

	vec3 eigenvalues = gyration_tensor.eigenvalues().real();

	double eigen_volume = 4 * M_PI * sqrt(3) * sqrt(eigenvalues[0]) * sqrt(eigenvalues[1]) * sqrt(eigenvalues[2]);

	ss << " " << ch_volume;
	ss << " " << eigen_volume;
	ss << " " << sqrt(eigenvalues[0]);
	ss << " " << sqrt(eigenvalues[1]);
	ss << " " << sqrt(eigenvalues[2]);

	qh_free_mesh(mesh);

	_line = ss.str();
}

} /* namespace ashell */