/**
 * @file octree_test.cpp
 * @author Ryan Curtin
 *
 * Test various properties of the Octree.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/tree/octree.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"
#include "serialization.hpp"

using namespace mlpack;
using namespace mlpack::math;
using namespace mlpack::tree;
using namespace mlpack::metric;
using namespace mlpack::bound;

BOOST_AUTO_TEST_SUITE(OctreeTest);

/**
 * Build a quad-tree (2-d octree) on 4 points, and guarantee four points are
 * created.
 */
BOOST_AUTO_TEST_CASE(SimpleQuadtreeTest)
{
  // Four corners of the unit square.
  arma::mat dataset("0 0 1 1; 0 1 0 1");

  Octree<> t(dataset, 1);

  BOOST_REQUIRE_EQUAL(t.NumChildren(), 4);
  BOOST_REQUIRE_EQUAL(t.Dataset().n_cols, 4);
  BOOST_REQUIRE_EQUAL(t.Dataset().n_rows, 2);
  BOOST_REQUIRE_EQUAL(t.NumDescendants(), 4);
  BOOST_REQUIRE_EQUAL(t.NumPoints(), 0);
  for (size_t i = 0; i < 4; ++i)
  {
    BOOST_REQUIRE_EQUAL(t.Child(i).NumDescendants(), 1);
    BOOST_REQUIRE_EQUAL(t.Child(i).NumPoints(), 1);
  }
}

/**
 * Build an octree on 3 points and make sure that only three children are
 * created.
 */
BOOST_AUTO_TEST_CASE(OctreeMissingChildTest)
{
  // Only three corners of the unit square.
  arma::mat dataset("0 0 1; 0 1 1");

  Octree<> t(dataset, 1);

  BOOST_REQUIRE_EQUAL(t.NumChildren(), 3);
  BOOST_REQUIRE_EQUAL(t.Dataset().n_cols, 3);
  BOOST_REQUIRE_EQUAL(t.Dataset().n_rows, 2);
  BOOST_REQUIRE_EQUAL(t.NumDescendants(), 3);
  BOOST_REQUIRE_EQUAL(t.NumPoints(), 0);
  for (size_t i = 0; i < 3; ++i)
  {
    BOOST_REQUIRE_EQUAL(t.Child(i).NumDescendants(), 1);
    BOOST_REQUIRE_EQUAL(t.Child(i).NumPoints(), 1);
  }
}

/**
 * Ensure that building an empty octree does not fail.
 */
BOOST_AUTO_TEST_CASE(EmptyOctreeTest)
{
  arma::mat dataset;
  Octree<> t(dataset);

  BOOST_REQUIRE_EQUAL(t.NumChildren(), 0);
  BOOST_REQUIRE_EQUAL(t.Dataset().n_cols, 0);
  BOOST_REQUIRE_EQUAL(t.Dataset().n_rows, 0);
  BOOST_REQUIRE_EQUAL(t.NumDescendants(), 0);
  BOOST_REQUIRE_EQUAL(t.NumPoints(), 0);
}

/**
 * Ensure that maxLeafSize is respected.
 */
BOOST_AUTO_TEST_CASE(MaxLeafSizeTest)
{
  arma::mat dataset(5, 15, arma::fill::randu);
  Octree<> t1(dataset, 20);
  Octree<> t2(std::move(dataset), 20);

  BOOST_REQUIRE_EQUAL(t1.NumChildren(), 0);
  BOOST_REQUIRE_EQUAL(t1.NumDescendants(), 15);
  BOOST_REQUIRE_EQUAL(t1.NumPoints(), 15);

  BOOST_REQUIRE_EQUAL(t2.NumChildren(), 0);
  BOOST_REQUIRE_EQUAL(t2.NumDescendants(), 15);
  BOOST_REQUIRE_EQUAL(t2.NumPoints(), 15);
}

/**
 * Check that the mappings given are correct.
 */
BOOST_AUTO_TEST_CASE(MappingsTest)
{
  // Test with both constructors.
  arma::mat dataset(3, 5, arma::fill::randu);
  arma::mat datacopy(dataset);
  std::vector<size_t> oldFromNewCopy, oldFromNewMove;

  Octree<> t1(dataset, oldFromNewCopy, 1);
  Octree<> t2(std::move(dataset), oldFromNewMove, 1);

  for (size_t i = 0; i < oldFromNewCopy.size(); ++i)
  {
    BOOST_REQUIRE_SMALL(arma::norm(datacopy.col(oldFromNewCopy[i]) -
        t1.Dataset().col(i)), 1e-3);
    BOOST_REQUIRE_SMALL(arma::norm(datacopy.col(oldFromNewMove[i]) -
        t2.Dataset().col(i)), 1e-3);
  }
}

/**
 * Check that the reverse mappings are correct too.
 */
BOOST_AUTO_TEST_CASE(ReverseMappingsTest)
{
  // Test with both constructors.
  arma::mat dataset(3, 300, arma::fill::randu);
  arma::mat datacopy(dataset);
  std::vector<size_t> oldFromNewCopy, oldFromNewMove, newFromOldCopy,
      newFromOldMove;

  Octree<> t1(dataset, oldFromNewCopy, newFromOldCopy);
  Octree<> t2(std::move(dataset), oldFromNewMove, newFromOldMove);

  for (size_t i = 0; i < oldFromNewCopy.size(); ++i)
  {
    BOOST_REQUIRE_SMALL(arma::norm(datacopy.col(oldFromNewCopy[i]) -
        t1.Dataset().col(i)), 1e-3);
    BOOST_REQUIRE_SMALL(arma::norm(datacopy.col(oldFromNewMove[i]) -
        t2.Dataset().col(i)), 1e-3);

    BOOST_REQUIRE_EQUAL(newFromOldCopy[oldFromNewCopy[i]], i);
    BOOST_REQUIRE_EQUAL(newFromOldMove[oldFromNewMove[i]], i);
  }
}

/**
 * Make sure no children at the same level are overlapping.
 */
template<typename TreeType>
void CheckOverlap(TreeType& node)
{
  // Check each combination of children.
  for (size_t i = 0; i < node.NumChildren(); ++i)
    for (size_t j = i + 1; j < node.NumChildren(); ++j)
      BOOST_REQUIRE_EQUAL(node.Child(i).Bound().Overlap(node.Child(j).Bound()),
          0.0); // We need exact equality here.

  for (size_t i = 0; i < node.NumChildren(); ++i)
    CheckOverlap(node.Child(i));
}

BOOST_AUTO_TEST_CASE(OverlapTest)
{
  // Test with both constructors.
  arma::mat dataset(3, 300, arma::fill::randu);

  Octree<> t1(dataset);
  Octree<> t2(std::move(dataset));

  CheckOverlap(t1);
  CheckOverlap(t2);
}

/**
 * Make sure no points are further than the furthest point distance, and that no
 * descendants are further than the furthest descendant distance.
 */
template<typename TreeType>
void CheckFurthestDistances(TreeType& node)
{
  arma::vec center;
  node.Center(center);

  // Compare points held in the node.
  for (size_t i = 0; i < node.NumPoints(); ++i)
  {
    // Handle floating-point inaccuracies.
    BOOST_REQUIRE_LE(metric::EuclideanDistance::Evaluate(node.Dataset().col(node.Point(i)),
        center), node.FurthestPointDistance() * (1 + 1e-5));
  }

  // Compare descendants held in the node.
  for (size_t i = 0; i < node.NumDescendants(); ++i)
  {
    // Handle floating-point inaccuracies.
    BOOST_REQUIRE_LE(metric::EuclideanDistance::Evaluate(node.Dataset().col(node.Descendant(i)),
        center), node.FurthestDescendantDistance() * (1 + 1e-5));
  }

  for (size_t i = 0; i < node.NumChildren(); ++i)
    CheckFurthestDistances(node.Child(i));
}

BOOST_AUTO_TEST_CASE(FurthestDistanceTest)
{
  // Test with both constructors.
  arma::mat dataset(3, 500, arma::fill::randu);

  Octree<> t1(dataset);
  Octree<> t2(std::move(dataset));

  CheckFurthestDistances(t1);
  CheckFurthestDistances(t2);
}

/**
 * The maximum number of children a node can have is limited by the
 * dimensionality.  So we test to make sure there are no cases where we have too
 * many children.
 */
template<typename TreeType>
void CheckNumChildren(TreeType& node)
{
  BOOST_REQUIRE_LE(node.NumChildren(), std::pow(2, node.Dataset().n_rows));
  for (size_t i = 0; i < node.NumChildren(); ++i)
    CheckNumChildren(node.Child(i));
}

BOOST_AUTO_TEST_CASE(MaxNumChildrenTest)
{
  for (size_t d = 1; d < 10; ++d)
  {
    arma::mat dataset(d, 1000 * d, arma::fill::randu);
    Octree<> t(std::move(dataset));

    CheckNumChildren(t);
  }
}

/**
 * Test the copy constructor.
 */
template<typename TreeType>
void CheckSameNode(TreeType& node1, TreeType& node2)
{
  BOOST_REQUIRE_EQUAL(node1.NumChildren(), node2.NumChildren());
  BOOST_REQUIRE_NE(&node1.Dataset(), &node2.Dataset());

  // Make sure the children actually got copied.
  for (size_t i = 0; i < node1.NumChildren(); ++i)
    BOOST_REQUIRE_NE(&node1.Child(i), &node2.Child(i));

  // Check that all the points are the same.
  BOOST_REQUIRE_EQUAL(node1.NumPoints(), node2.NumPoints());
  BOOST_REQUIRE_EQUAL(node1.NumDescendants(), node2.NumDescendants());
  for (size_t i = 0; i < node1.NumPoints(); ++i)
    BOOST_REQUIRE_EQUAL(node1.Point(i), node2.Point(i));
  for (size_t i = 0; i < node1.NumDescendants(); ++i)
    BOOST_REQUIRE_EQUAL(node1.Descendant(i), node2.Descendant(i));

  // Check that the bound is the same.
  BOOST_REQUIRE_EQUAL(node1.Bound().Dim(), node2.Bound().Dim());
  for (size_t d = 0; d < node1.Bound().Dim(); ++d)
  {
    BOOST_REQUIRE_CLOSE(node1.Bound()[d].Lo(), node2.Bound()[d].Lo(), 1e-5);
    BOOST_REQUIRE_CLOSE(node1.Bound()[d].Hi(), node2.Bound()[d].Hi(), 1e-5);
  }

  // Check that the furthest point and descendant distance are the same.
  BOOST_REQUIRE_CLOSE(node1.FurthestPointDistance(),
      node2.FurthestPointDistance(), 1e-5);
  BOOST_REQUIRE_CLOSE(node1.FurthestDescendantDistance(),
      node2.FurthestDescendantDistance(), 1e-5);
}

BOOST_AUTO_TEST_CASE(CopyConstructorTest)
{
  // Use a small random dataset.
  arma::mat dataset(3, 100, arma::fill::randu);

  Octree<> t(dataset);
  Octree<> t2(t);

  CheckSameNode(t, t2);
}

/**
 * Test the move constructor.
 */
BOOST_AUTO_TEST_CASE(MoveConstructorTest)
{
  // Use a small random dataset.
  arma::mat dataset(3, 100, arma::fill::randu);

  Octree<> t(std::move(dataset));
  Octree<> tcopy(t);

  // Move the tree.
  Octree<> t2(std::move(t));

  // Make sure the original tree has no data.
  BOOST_REQUIRE_EQUAL(t.Dataset().n_rows, 0);
  BOOST_REQUIRE_EQUAL(t.Dataset().n_cols, 0);
  BOOST_REQUIRE_EQUAL(t.NumChildren(), 0);
  BOOST_REQUIRE_EQUAL(t.NumPoints(), 0);
  BOOST_REQUIRE_EQUAL(t.NumDescendants(), 0);
  BOOST_REQUIRE_SMALL(t.FurthestPointDistance(), 1e-5);
  BOOST_REQUIRE_SMALL(t.FurthestDescendantDistance(), 1e-5);
  BOOST_REQUIRE_EQUAL(t.Bound().Dim(), 0);

  // Check that the new tree is the same as our copy.
  CheckSameNode(tcopy, t2);
}

/**
 * Test serialization.
 */
BOOST_AUTO_TEST_CASE(SerializationTest)
{
  // Use a small random dataset.
  arma::mat dataset(3, 500, arma::fill::randu);
  Octree<> t(std::move(dataset));

  Octree<>* xmlTree;
  Octree<>* binaryTree;
  Octree<>* textTree;

  SerializePointerObjectAll(&t, xmlTree, binaryTree, textTree);

  CheckSameNode(t, *xmlTree);
  CheckSameNode(t, *binaryTree);
  CheckSameNode(t, *textTree);

  delete xmlTree;
  delete binaryTree;
  delete textTree;
}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <typeinfo>
#define BOOST_TEST_MODULE MixedMIAMultTests
#include "MIAConfig.h"
#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

#include "SparseMIA.h"
#include "DenseMIA.h"
#include "Index.h"

template<class _data_type>
void mult_work(size_t dim1, size_t dim2){

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;
    LibMIA::MIAINDEX l;
    LibMIA::MIAINDEX m;
    LibMIA::MIAINDEX n;

    LibMIA::DenseMIA<_data_type,4> temp_a(dim1,dim2,dim1,dim2);
    LibMIA::DenseMIA<_data_type,4> dense_b(dim2,dim2,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> dense_c;
    LibMIA::DenseMIA<_data_type,2> dense_c2;
    LibMIA::DenseMIA<_data_type,2> dense_b2(dim2,dim2);
    LibMIA::DenseMIA<_data_type,1> dense_d(dim2);
    LibMIA::DenseMIA<_data_type,2> dense_d2(dim2,dim2);
    LibMIA::DenseMIA<_data_type,4> c_mixed;
    LibMIA::DenseMIA<_data_type,2> c2_mixed;

    LibMIA::SparseMIA<_data_type,4> a(dim1,dim2,dim1,dim2);
    LibMIA::SparseMIA<_data_type,4> b(dim2,dim2,dim1,dim1);

    LibMIA::SparseMIA<_data_type,2> b2(dim2,dim2);
    LibMIA::SparseMIA<_data_type,1> d(dim2);
    LibMIA::SparseMIA<_data_type,2> d2(dim2,dim2);
    LibMIA::SparseMIA<_data_type,4> c;

    temp_a.randu(0,20);
    dense_b.randu(0,20);
    dense_b2.randu(0,20);
    dense_d.randu(0,20);
    dense_d2.randu(0,20);

    for(auto it=temp_a.data_begin();it<temp_a.data_end();++it)
        if(*it<15)
            *it=0;
    for(auto it=dense_b.data_begin();it<dense_b.data_end();++it)
        if(*it<15)
            *it=0;
    for(auto it=dense_b2.data_begin();it<dense_b2.data_end();++it)
        if(*it<15)
            *it=0;
    for(auto it=dense_d.data_begin();it<dense_d.data_end();++it)
        if(*it<15)
            *it=0;
    for(auto it=dense_d2.data_begin();it<dense_d2.data_end();++it)
        if(*it<15)
            *it=0;

    const LibMIA::DenseMIA<_data_type,4> dense_a(temp_a);
    a=dense_a;
    b=dense_b;
    b2=dense_b2;
    d=dense_d;
    d2=dense_d2;

    dense_c(i,k,m,n)=dense_a(i,j,k,l)*dense_b(j,l,m,n);
    c_mixed(i,k,m,n)=a(i,j,k,l)*dense_b(j,l,m,n);

    BOOST_CHECK_MESSAGE(c_mixed.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Inner/Outer Product 1a for ")+typeid(_data_type).name() );

    c_mixed(i,k,m,n)=dense_a(i,j,k,l)*b(j,l,m,n);
    BOOST_CHECK_MESSAGE(c_mixed.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Inner/Outer Product 1b for ")+typeid(_data_type).name() );

    dense_c(i,k,m,n)=dense_a(i,l,k,j)*dense_b(l,j,m,n);

    c_mixed(i,k,m,n)=a(i,l,k,j)*dense_b(l,j,m,n);

    BOOST_CHECK_MESSAGE(c_mixed.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Inner/Outer Product 2a for ")+typeid(_data_type).name());

    c_mixed(i,k,m,n)=dense_a(i,l,k,j)*b(l,j,m,n);
    BOOST_CHECK_MESSAGE(c_mixed.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Inner/Outer Product 2b for ")+typeid(_data_type).name());

    dense_c(i,k,m,n)=dense_a(i,l,k,j)*dense_b(l,j,m,n);
    c_mixed(i,k,m,n)=a(i,l,k,j)*dense_b(l,j,m,n);
    BOOST_CHECK_MESSAGE(c_mixed.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Inner/Outer Product 3a for ")+typeid(_data_type).name());
    c_mixed(i,k,m,n)=dense_a(i,l,k,j)*b(l,j,m,n);
    BOOST_CHECK_MESSAGE(c_mixed.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Inner/Outer Product 3b for ")+typeid(_data_type).name());

    dense_c2(i,j)=dense_a(!i,k,!j,l)*dense_b(k,l,!i,!j);
    c2_mixed(i,j)=a(!i,k,!j,l)*dense_b(k,l,!i,!j);
    BOOST_CHECK_MESSAGE(c2_mixed.fuzzy_equals(dense_c2,test_precision<_data_type>()),std::string("Inner/Element-Wise Product 1a for ")+typeid(_data_type).name());
    c2_mixed(i,j)=dense_a(!i,k,!j,l)*b(k,l,!i,!j);
    BOOST_CHECK_MESSAGE(c2_mixed.fuzzy_equals(dense_c2,test_precision<_data_type>()),std::string("Inner/Element-Wise Product 1b for ")+typeid(_data_type).name());

    dense_c2(i,j)=dense_a(!i,l,!j,k)*dense_b(k,l,!i,!j);
    c2_mixed(i,j)=a(!i,l,!j,k)*dense_b(k,l,!i,!j);
    BOOST_CHECK_MESSAGE(c2_mixed.fuzzy_equals(dense_c2,test_precision<_data_type>()),std::string("Inner/Element-Wise Product 2a for ")+typeid(_data_type).name());
    c2_mixed(i,j)=dense_a(!i,l,!j,k)*b(k,l,!i,!j);
    BOOST_CHECK_MESSAGE(c2_mixed.fuzzy_equals(dense_c2,test_precision<_data_type>()),std::string("Inner/Element-Wise Product 2b for ")+typeid(_data_type).name());

    dense_c2(j,i)=dense_a(!j,l,!i,k)*dense_b(k,l,!j,!i);
    c2_mixed(j,i)=a(!j,l,!i,k)*dense_b(k,l,!j,!i);

    BOOST_CHECK_MESSAGE(c2_mixed.fuzzy_equals(dense_c2,test_precision<_data_type>()),std::string("Inner/Element-Wise Product 3a for ")+typeid(_data_type).name());
    c2_mixed(j,i)=dense_a(!j,l,!i,k)*b(k,l,!j,!i);
    BOOST_CHECK_MESSAGE(c2_mixed.fuzzy_equals(dense_c2,test_precision<_data_type>()),std::string("Inner/Element-Wise Product 3b for ")+typeid(_data_type).name());

    dense_c(i,j,k,l)=dense_b2(i,j)*dense_d2(k,l);
    c(i,j,k,l)=b2(i,j)*dense_d2(k,l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Outer Product 1a for ")+typeid(_data_type).name());

    c(i,j,k,l)=dense_b2(i,j)*d2(k,l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Outer Product 1b for ")+typeid(_data_type).name());
//    std::cout << "Sparse " << std::endl;
//    c.print();
//    std::cout << "Dense " << std::endl;
//    dense_c.print();

    dense_c(i,k,l,j)=dense_b2(k,j)*dense_d2(l,i);
    c(i,k,l,j)=b2(k,j)*dense_d2(l,i);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Outer Product 2a for ")+typeid(_data_type).name());
    c(i,k,l,j)=dense_b2(k,j)*d2(l,i);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Outer Product 2b for ")+typeid(_data_type).name());

    dense_c(i,k,l,j)=dense_d2(l,i)*dense_b2(k,j);
    c(i,k,l,j)=d2(l,i)*dense_b2(k,j);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Outer Product 3a for ")+typeid(_data_type).name());
    c(i,k,l,j)=dense_d2(l,i)*b2(k,j);

//    dense_c.print();
//    c.print();
//    c.reset_linIdx_sequence();
//    c.print();
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Outer Product 3b for ")+typeid(_data_type).name());

    dense_c(i,j,k,l)=dense_a(i,!j,k,!l)*dense_b2(!j,!l);
    c(i,j,k,l)=a(i,!j,k,!l)*dense_b2(!j,!l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Element-Wise Product 1a for ")+typeid(_data_type).name());

    c(i,j,k,l)=dense_a(i,!j,k,!l)*b2(!j,!l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Element-Wise Product 1b for ")+typeid(_data_type).name());

    dense_c(i,j,k,l)=dense_a(k,!j,i,!l)*dense_b2(!j,!l);
    c(i,j,k,l)=a(k,!j,i,!l)*dense_b2(!j,!l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Element-Wise Product 2a for ")+typeid(_data_type).name());
    c(i,j,k,l)=dense_a(k,!j,i,!l)*b2(!j,!l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Element-Wise Product 2b for ")+typeid(_data_type).name());

    dense_c(i,j,k,l)=dense_a(k,!l,i,!j)*dense_b2(!j,!l);
    c(i,j,k,l)=a(k,!l,i,!j)*dense_b2(!j,!l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Element-Wise Product 3a for ")+typeid(_data_type).name());
    c(i,j,k,l)=dense_a(k,!l,i,!j)*b2(!j,!l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Outer/Element-Wise Product 3b for ")+typeid(_data_type).name());
//    std::cout << "Sparse " << std::endl;
//    c.print();
//    std::cout << "Dense " << std::endl;
//    dense_c.print();

    dense_c(i,j,k,l)=~(dense_a(i,!j,k,!!l)*dense_b2(!j,!!l))*dense_d(!l);
    c(i,j,k,l)=~(a(i,!j,k,!!l)*dense_b2(!j,!!l))*d(!l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Repeated Element-Wise Product 1a for ")+typeid(_data_type).name());
    c(i,j,k,l)=~(dense_a(i,!j,k,!!l)*b2(!j,!!l))*d(!l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Repeated Element-Wise Product 1b for ")+typeid(_data_type).name());
    c(i,j,k,l)=~(a(i,!j,k,!!l)*b2(!j,!!l))*dense_d(!l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Repeated Element-Wise Product 1c for ")+typeid(_data_type).name());

    dense_c(i,k,m,n)=~(dense_a(i,!j,k,!l)*dense_b(!j,!l,m,n))*dense_d2(j,l);
    c(i,k,m,n)=~(a(i,!j,k,!l)*dense_b(!j,!l,m,n))*d2(j,l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Ternary Inner Product 1a for ")+typeid(_data_type).name() );
    c(i,k,m,n)=~(dense_a(i,!j,k,!l)*b(!j,!l,m,n))*d2(j,l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Ternary Inner Product 1b for ")+typeid(_data_type).name() );

    c(i,k,m,n)=~(a(i,!j,k,!l)*b(!j,!l,m,n))*dense_d2(j,l);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Ternary Inner Product 1c for ")+typeid(_data_type).name() );

    dense_c(i,k,m,n)=~(dense_a(i,!j,k,!l)*dense_b(!j,!l,m,n))*dense_d2(l,j);
    c(i,k,m,n)=~(a(i,!j,k,!l)*dense_b(!j,!l,m,n))*d2(l,j);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Ternary Inner Product 2a for ")+typeid(_data_type).name() );
    c(i,k,m,n)=~(dense_a(i,!j,k,!l)*b(!j,!l,m,n))*d2(l,j);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Ternary Inner Product 2b for ")+typeid(_data_type).name() );
    c(i,k,m,n)=~(a(i,!j,k,!l)*b(!j,!l,m,n))*dense_d2(l,j);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Ternary Inner Product 2c for ")+typeid(_data_type).name() );

    dense_c(i,k,m,n)=~(dense_a(i,!l,k,!j)*dense_b(!j,!l,m,n))*dense_d2(l,j);
    c(i,k,m,n)=~(a(i,!l,k,!j)*dense_b(!j,!l,m,n))*d2(l,j);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Ternary Inner Product 3a for ")+typeid(_data_type).name() );
    c(i,k,m,n)=~(dense_a(i,!l,k,!j)*b(!j,!l,m,n))*d2(l,j);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Ternary Inner Product 3b for ")+typeid(_data_type).name() );
    c(i,k,m,n)=~(a(i,!l,k,!j)*b(!j,!l,m,n))*dense_d2(l,j);
    BOOST_CHECK_MESSAGE(c.fuzzy_equals(dense_c,test_precision<_data_type>()),std::string("Ternary Inner Product 3c for ")+typeid(_data_type).name() );

}

BOOST_AUTO_TEST_CASE( MixedMIAMultTests )
{

    //mult_work<double>(3,3);
    //mult_work<float>(3,3);
    //mult_work<int>(3,3);
    //mult_work<long>(3,3);
//
//

    mult_work<double>(8,5);
    mult_work<float>(8,5);
    mult_work<int>(8,5);
    mult_work<long>(8,5);

    mult_work<double>(5,8);
    mult_work<float>(5,8);
    mult_work<int>(5,8);
    mult_work<long>(5,8);

}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#define FUSION_MAX_VECTOR_SIZE 25
#include <iostream>
//#include <fstream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"
#include "dune/istl/solvers.hh"

#include "algorithm/giant_gbit.hh"
#include "fem/gridmanager.hh"
#include "fem/lagrangespace.hh"
//#include "fem/hierarchicspace.hh"   // ContinuousHierarchicMapper
#include "io/vtk.hh"
#include "utilities/kaskopt.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare

using namespace Kaskade;

#include "sst.hh"

struct InitialValue
{
  using Scalar = double;
  static constexpr int components = 1;
  using ValueType = Dune::FieldVector<Scalar,components>;

  InitialValue(int c): component(c) {}

  template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }
  template <class Cell>
  ValueType value(Cell const& cell,
  Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> const& localCoordinate) const
  {
  // Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> x = cell.geometry().global(localCoordinate);
  if (component==0)
    return 1.0e9;
  else if (component==1)
    return 1.0e9;
  else if (component==2)
    return 1.0e13;
  else if (component==3)
    return 1.0e7;
  else
    assert("wrong index!\n"==0);
  return 0;

  }

private:
  int component;
};

int main(int argc, char *argv[])
{
  using Scalar = double;
  using namespace boost::fusion;

  std::cout << "Start sst transfer tutorial program with inexact damped Newton iteration " << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 1;
  bool dump = true;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  int refinements = getParameter(pt, "refinement", 5),
      order =  getParameter(pt, "order", 2);
//      verbosity   = getParameter(pt, "verbosity", 0);
  Scalar tol = getParameter(pt, "tolerance", 1.0e-10),
         rho = getParameter(pt, "safetyfactor",0.0625);
  //   IterateType iterateType = IterateType::CG;
  //   PrecondType precondType = PrecondType::NONE;
  std::string empty;

  std::cout << "refinements of original mesh   : " << refinements << std::endl;
  std::cout << "discretization order           : " << order << std::endl;
  std::cout << "tolerance for Newton iteration : " << tol << std::endl;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  //direct = getParameter(pt, s, 0);

  //   s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
  //   iterateType = static_cast<IterateType>(getParameter(pt, s, 0));
  //   s = "names.preconditioner." + getParameter(pt, "solver.preconditioner", empty);
  //   precondType = static_cast<PrecondType>(getParameter(pt, s, 0));

  constexpr int dim=2;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<Scalar,LeafView> >;
// using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<Scalar,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<Variable<SpaceIndex<0>,Components<1>,VariableId<0> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<1> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<2> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<3> > >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = SSTFunctional<Scalar,VariableSet>;
  using NonlinearSolver = Giant<Grid,Functional,VariableSet,Spaces>;

  GridManager<Grid> gridManager( createUnitSquare<Grid>() );
  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " triangles, " << std::endl;
  std::cout << "      " << gridManager.grid().size(1) << " edges, " << std::endl;
  std::cout << "      " << gridManager.grid().size(2) << " points" << std::endl;

  // construction of finite element space for the scalar solution T
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),
               order);
  Spaces spaces(&temperatureSpace);
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable
  std::string varNames[4] = { "u0", "u1", "u2", "u3" };
  VariableSet variableSet(spaces,varNames);

  Functional F;
  VariableSet::VariableSet x(variableSet);

  F.scaleInitialValue<0>(InitialValue(0),x);
  F.scaleInitialValue<1>(InitialValue(1),x);
  F.scaleInitialValue<2>(InitialValue(2),x);
  F.scaleInitialValue<3>(InitialValue(3),x);

  LeafView leafGridView = gridManager.grid().leafGridView();
  writeVTKFile(x,"graph/sst_giant_start",IoOptions().setOrder(std::min(order,2)).setPrecision(7));
  gridManager.enforceConcurrentReads(false);

//   constexpr int nvars = Functional::AnsatzVars::noOfVariables;
//   constexpr int n     = variableSet.degreesOfFreedom(0,nvars);
//   std::vector<double> scale(n);
//   x.write(scale.begin());
//   for (int i=0;i<n;i++) scale[i]=2*scale[i];

//   std::fstream monitorStream;
//   monitorStream.open("sst_giant.mon",std::fstream::out);
//   if ( !monitorStream.is_open() )
//   {
//      std::cout << " Failed to open monitorStream file sst_giant.mon" << std::endl;
//      exit(-1);
//   };

  NonlinearSolver nonlinearSolver;

  s = "names.nonlinType." + getParameter(pt, "solver.nonlinType", empty);
  NonlinearSolver::NonlinProblemType
    nonlinType=static_cast<NonlinearSolver::NonlinProblemType>(getParameter(pt,s,(int) NonlinearSolver::NonlinProblemType::highlyNonlinear));

  int preconFillLevel=getParameter(pt,"preconlevel",0);
  nonlinearSolver.setTolerance(tol);
//   nonlinearSolver.setMaximumNoIterations(50);
  nonlinearSolver.setPreconFillLevel(preconFillLevel);
  nonlinearSolver.setSafetyFactor(rho);
//   nonlinearSolver.setErrorLevel(NonlinearSolver::verbose);
  nonlinearSolver.setMonitorLevel(NonlinearSolver::verbose);
  nonlinearSolver.setDataLevel(NonlinearSolver::verbose);
//  nonlinearSolver.setErrorStream(std::cout);
//  nonlinearSolver.setMonitorStream(std::cout);
//   nonlinearSolver.setMonitorStream(monitorStream);
   nonlinearSolver.setNonlinProblemType( nonlinType );
//   nonlinearSolver.setRestricted(false);
//  nonlinearSolver.setScalingVector(&scale);
  nonlinearSolver.setOutFilePrefix((std::string) "graph/sst_giant_");

  boost::timer::cpu_timer nonlinTimer;
  nonlinearSolver.giantGbit(gridManager,F,variableSet,&x,spaces);
  std::cout << "computing time for nonlinear solver: " << boost::timer::format(nonlinTimer.elapsed()) << "\n";
  struct NonlinearSolver::NleqInfo info=nonlinearSolver.getInfo();
//  monitorStream << " The total number of iterative linear solver steps done is " <<
  std::cout << " The total number of iterative linear solver steps done is " <<
               info.noOrdLinIt+info.noSimLinIt << std::endl;
  std::string finalOutputFilename;
  if ( info.returnCode==0 )
    finalOutputFilename="graph/sst_giant_solution";
  else
    finalOutputFilename="graph/sst_giant_final";
  writeVTKFile(x,finalOutputFilename,IoOptions().setOrder(std::min(order,2)).setPrecision(7));

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End sst transfer tutorial program" << std::endl;
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file boolean_operator.cpp
/// \brief Test for boolean expressions.

#include "mcrl2/bes/bes2pbes.h"
#include "mcrl2/bes/boolean_equation_system.h"
#include "mcrl2/bes/io.h"
#include "mcrl2/bes/print.h"
#include <boost/test/minimal.hpp>
#include <cstdio>
#include <iostream>
#include <string>

using namespace mcrl2;

void test_boolean_expressions()
{
  using namespace bes;
  typedef core::term_traits<boolean_expression> tr;

  boolean_variable X1("X1");
  boolean_variable X2("X2");
  boolean_expression t1 = tr::and_(X1, X2);
  boolean_equation e1(fixpoint_symbol::mu(), X1, tr::imp(X1, X2));
  boolean_equation e2(fixpoint_symbol::nu(), X2, tr::or_(X1, X2));
  std::cout << bes::pp(e1) << std::endl;
  std::cout << bes::pp(e2) << std::endl;

  boolean_equation_system p;
  p.equations().push_back(e1);
  p.equations().push_back(e2);
  p.initial_state() = X1;
  std::cout << "----------------" << std::endl;
  std::cout << bes::pp(p) << std::endl;

  std::string filename = "boolean_expression_test.out";
  save_bes(p, filename);
  boolean_equation_system q;
  load_bes(q, filename);
  BOOST_CHECK(p == q);
  remove(filename.c_str());
}

void test_bes2pbes()
{
  using namespace bes;
  typedef core::term_traits<boolean_expression> tr;

  boolean_variable X1("X1");
  boolean_variable X2("X2");
  boolean_variable X3("X3");
  boolean_expression t1 = tr::and_(X1, X2);
  boolean_equation e1(fixpoint_symbol::mu(), X1, tr::imp(X1, X2));
  boolean_equation e2(fixpoint_symbol::nu(), X2, tr::or_(X1, X2));
  boolean_equation e3(fixpoint_symbol::nu(), X3, tr::false_());
  std::cout << bes::pp(e1) << std::endl;
  std::cout << bes::pp(e2) << std::endl;
  std::cout << bes::pp(e3) << std::endl;

  boolean_equation_system p;
  p.equations().push_back(e1);
  p.equations().push_back(e2);
  p.equations().push_back(e3);
  p.initial_state() = X1;
  std::cout << "----------------" << std::endl;
  std::cout << bes::pp(p) << std::endl;

  pbes_system::pbes q = bes2pbes(p);
  std::cout << "----------------" << std::endl;
  std::cout << q << std::endl;
}

void test_precedence()
{
  using namespace bes;
  typedef core::term_traits<boolean_expression> tr;

  boolean_variable X1("X1");
  boolean_variable X2("X2");
  boolean_expression t = tr::and_(X1, X2);
  BOOST_CHECK(left_precedence(t) == 4);

  std::string s = bes::pp(t);
  BOOST_CHECK(s == "X1 && X2");
}

int test_main(int argc, char* argv[])
{
  test_boolean_expressions();
  test_bes2pbes();
  test_precedence();

  return 0;
}


// This example is heavily based on the tutorial at https://open.gl

// OpenGL Helpers to reduce the clutter
#include "Helpers.h"
#include <iostream>
#include <fstream>
#include "math.h"
// GLFW is necessary to handle the OpenGL context
#include <GLFW/glfw3.h>

// Linear Algebra Library
#include <Eigen/Core>
#include <Eigen/Dense>
#include <thread>

// Timer
#include <chrono>
#include <OpenGL/OpenGL.h>

// VertexBufferObject wrapper
VertexBufferObject VBO;
VertexBufferObject VBO_C;

// Contains the vertex positions
Eigen::MatrixXf V(2,3);
Eigen::MatrixXf V_C(2,3);
Eigen::MatrixXf temp2(2,3);
Eigen::Matrix4f view(4,4);
Eigen::MatrixXf animatematrix(3,6);
using namespace std;
int tri_count=0;
int mode=-1;
int tri_num=-1;
double globalx=0;
double globaly=0;
double arr[6];
int tri_rotate=-1;
int ver_color=-1;
int tri_animate=-1;
int counter = -1;

double arrz[9];

void mouse_button_callback(GLFWwindow* window, int button, int action, int mods)
{
    // Get the position of the mouse in the window
    double xpos, ypos;
    glfwGetCursorPos(window, &xpos, &ypos);

    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    Eigen::Vector4f p_screen(xpos,height-1-ypos,0,1);
    Eigen::Vector4f p_canonical((p_screen[0]/width)*2-1,(p_screen[1]/height)*2-1,0,1);
    Eigen::Vector4f p_world = view.inverse()*p_canonical;

    // Update the position of the first vertex if the left button is pressed
    if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS)
        V.col(0) << p_world[0], p_world[1],1;

    // Upload the change to the GPU
    VBO.update(V);
}

void mouse_button_make_triangle(GLFWwindow* window, int button, int action, int mods)
{
    double xpos, ypos;
    glfwGetCursorPos(window, &xpos, &ypos);
    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    Eigen::Vector4f p_screen(xpos,height-1-ypos,0,1);
    Eigen::Vector4f p_canonical((p_screen[0]/width)*2-1,(p_screen[1]/height)*2-1,0,1);
    Eigen::Vector4f p_world = view.inverse()*p_canonical;

    if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS) {
        Eigen::MatrixXf temp(V.rows(),V.cols());
        temp=V;
        //cout<<"Temp is "<<temp<<endl;
        int size = V.cols()+1;
        V.resize(V.rows(),size);
        for(int i=0;i<temp.cols();i++)
        {
            V.col(i) = temp.col(i);
        }
        //cout<<"V aftr copying is"<<V<<endl;
        //cout<<"Adding to V values"<<xworld<< yworld<< 1.0<<endl;
        V.col(size - 1) << p_world[0], p_world[1], 1.0;

        Eigen::MatrixXf tempc(V_C.rows(),V_C.cols());
        tempc=V_C;
//        cout<<"Tempc is "<<tempc<<endl;
        int sizec = V_C.cols()+1;
        V_C.resize(V_C.rows(),sizec);
        for(int i=0;i<tempc.cols();i++)
        {
            V_C.col(i) = tempc.col(i);
        }
        //cout<<"V aftr copying is"<<V<<endl;
        //cout<<"Adding to V values"<<xworld<< yworld<< 1.0<<endl;
        V_C.col(sizec - 1) << 1,0,0;

        //cout<<"V is --> "<<V<<endl;
        tri_count++;
        if(tri_count==3)
        {

            tri_count=0;
            VBO_C.update(V_C);
        }
        VBO.update(V);

    }

}
float area(float x1, float y1, float x2, float y2, float x3, float y3)
{
    return abs((x1*(y2-y3) + x2*(y3-y1)+ x3*(y1-y2))/2.0);
}

/* A function to check whether point P(x, y) lies inside the triangle formed
   by A(x1, y1), B(x2, y2) and C(x3, y3) */
bool isInside(float x1, float y1, float x2, float y2, float x3, float y3, float x, float y)
{   cout<<"INSIDE is inside X,Y"<<x<<","<<y<<endl;
    /* Calculate area of triangle ABC */
    float A = area (x1, y1, x2, y2, x3, y3);

    /* Calculate area of triangle PBC */
    float A1 = area (x, y, x2, y2, x3, y3);

    /* Calculate area of triangle PAC */
    float A2 = area (x1, y1, x, y, x3, y3);

    /* Calculate area of triangle PAB */
    float A3 = area (x1, y1, x2, y2, x, y);

    /* Check if sum of A1, A2 and A3 is same as A */
    return (A == A1 + A2 + A3);
}

bool pointInTriangle(float x1, float y1, float x2, float y2, float x3, float y3, float x, float y)
{
    float denominator = ((y2 -y3)*(x1 - x3) + (x3 -x2) * (y1 -y3));
    float alpha = ((y2 - y3)*(x - x3) + (x3 - x2)*(y - y3))/denominator;
    float beta = ((y3 - y1)* (x - x3) + (x1 - x3)*(y - y3))/denominator;
    float gamma = 1 - alpha - beta;

    return (0 <= alpha && alpha <= 1 && 0 <= beta && beta <= 1 && 0 <= gamma && gamma <= 1);

}

int inside_triangle(float xworld,float yworld)
{
    for(int i=3;i<V.cols();i+=3)
    {
        if(pointInTriangle(V(0,i),V(1,i),V(0,i+1),V(1,i+1),V(0,i+2),V(1,i+2),xworld,yworld) ||
           isInside(V(0,i),V(1,i),V(0,i+1),V(1,i+1),V(0,i+2),V(1,i+2),xworld,yworld))
        {
            arr[0]=V(0,i);
            arr[1]=V(1,i);
            arr[2]=V(0,i+1);
            arr[3]=V(1,i+1);
            arr[4]=V(0,i+2);
            arr[5]=V(1,i+2);
            return i;
        }
    }
    return -1;
}
float distance(float dX0, float dY0, float dX1, float dY1)
{
    return sqrt(abs((dX1 - dX0)*(dX1 - dX0) + (dY1 - dY0)*(dY1 - dY0)));
}
int nearest_vertex(float xworld,float yworld)
{
    int vertex=0;
    float dist=std::numeric_limits<float>::max();
    for(int i=3;i<V.cols();i++)
    {   float tempd = distance(xworld,yworld,V(0,i),V(1,i));
        if(tempd<dist)
        {
            vertex=i;
            dist=tempd;
        }
    }
    cout<<"Nearest vertex is "<<vertex<<endl;
    return vertex;

}
void mouse_button_move_triangle(GLFWwindow* window, int button, int action, int mods) {
    cout << "Calling PRESSED----" <<view <<endl;
    double xpos, ypos;
    glfwGetCursorPos(window, &xpos, &ypos);
    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    // Convert screen position to world coordinates
    Eigen::Vector4f p_screen(xpos,height-1-ypos,0,1);
    Eigen::Vector4f p_canonical((p_screen[0]/width)*2-1,(p_screen[1]/height)*2-1,0,1);
    Eigen::Vector4f p_world = view.inverse()*p_canonical;

    if (inside_triangle((float)p_world[0],(float) p_world[1]) > 0) {
        int start = inside_triangle(p_world[0], p_world[1]);
        if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS) {

            mode = 1;
            tri_rotate=start;
            tri_num = start;
            globalx = p_world[0];
            globaly = p_world[1];

            arrz[0]=V_C(0,start);
            arrz[1]=V_C(1,start);
            arrz[2]=V_C(2,start);
            arrz[3]=V_C(0,start+1);
            arrz[4]=V_C(1,start+1);
            arrz[5]=V_C(2,start+1);
            arrz[6]=V_C(0,start+2);
            arrz[7]=V_C(1,start+2);
            arrz[8]=V_C(2,start+2);

            V_C(0,start)=0;
            V_C(1,start)=0;
            V_C(2,start)=1;
            V_C(0,start+1)=0;
            V_C(1,start+1)=0;
            V_C(2,start+1)=1;
            V_C(0,start+2)=0;
            V_C(1,start+2)=0;
            V_C(2,start+2)=1;
            VBO_C.update(V_C);

        } else if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_RELEASE) {
            cout << "Calling Released" <<view<< endl;
            mode = 2;
            globalx = 0;
            globaly = 0;
            tri_num = -1;
            V_C(0,start)=arrz[0];
            V_C(1,start)=arrz[1];
            V_C(2,start)=arrz[2];
            V_C(0,start+1)=arrz[3];
            V_C(1,start+1)=arrz[4];
            V_C(2,start+1)=arrz[5];
            V_C(0,start+2)=arrz[6];
            V_C(1,start+2)=arrz[7];
            V_C(2,start+2)=arrz[8];
            VBO_C.update(V_C);
        }

    } else
    {
        tri_count = -1;
    }
}
void mouse_button_delete_triangle(GLFWwindow* window, int button, int action, int mods) {
    double xpos, ypos;
    glfwGetCursorPos(window, &xpos, &ypos);
    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    // Convert screen position to world coordinates
    Eigen::Vector4f p_screen(xpos,height-1-ypos,0,1);
    Eigen::Vector4f p_canonical((p_screen[0]/width)*2-1,(p_screen[1]/height)*2-1,0,1);
    Eigen::Vector4f p_world = view.inverse()*p_canonical;

    if (inside_triangle(p_world[0], p_world[1]) > 0) {
        int start = inside_triangle(p_world[0], p_world[1]);
        if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS) {
            int x = V.cols();
            Eigen::MatrixXf tempz(V.rows(), x - 3);
            for (int i = 0; i < start; i++) {
                tempz.col(i) = V.col(i);
            }
            for (int i = start; (i < x) && (i+3 <V.cols()) ; i++) {
                tempz.col(i) = V.col(i + 3);
            }
            V.resize(V.rows(), x - 3);
            V = tempz;
            VBO.update(V);

            x = V_C.cols();
            tempz.resize(V_C.rows(), x - 3);
            for (int i = 0; i < start; i++) {
                tempz.col(i) = V_C.col(i);
            }
            for (int i = start; (i < x) && (i+3 <V_C.cols()) ; i++) {
                tempz.col(i) = V_C.col(i + 3);
            }
            V_C.resize(V_C.rows(), x - 3);
            V_C = tempz;
            VBO_C.update(V_C);

        }

    }
}

void mouse_button_animate_triangle(GLFWwindow* window, int button, int action, int mods) {
    cout << "Calling Animation Selection----" <<view <<endl;
    double xpos, ypos;
    glfwGetCursorPos(window, &xpos, &ypos);
    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    // Convert screen position to world coordinates
    Eigen::Vector4f p_screen(xpos,height-1-ypos,0,1);
    Eigen::Vector4f p_canonical((p_screen[0]/width)*2-1,(p_screen[1]/height)*2-1,0,1);
    Eigen::Vector4f p_world = view.inverse()*p_canonical;

    if (inside_triangle((float)p_world[0],(float) p_world[1]) > 0) {
        int start = inside_triangle(p_world[0], p_world[1]);
        if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS) {

            mode = 9;
            tri_animate=start;

        }

    } else
    {
        tri_animate = -1;
    }
}
//void mouse_button_rotate_triangle(GLFWwindow* window, int button, int action, int mods) {
//    double xpos, ypos;
//    glfwGetCursorPos(window, &xpos, &ypos);
//    // Get the size of the window
//    int width, height;
//    glfwGetWindowSize(window, &width, &height);
//
//
//
//    // Convert screen position to world coordinates
//    double xworld = ((xpos / double(width)) * 2) - 1;
//    double yworld = (((height - 1 - ypos) / double(height)) * 2) - 1; // NOTE: y axis is flipped in glfw
//    if (inside_triangle(xworld, yworld) > 0) {
//        int start = inside_triangle(xworld, yworld);
//        if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS) {
//           tri_rotate=start;
//            cout<<"Setting triangle to"<<tri_rotate<<endl;
//        }
//    }
//    else
//    {
//        tri_rotate=-1;
//    }
//}

void mouse_button_color_triangle(GLFWwindow* window, int button, int action, int mods) {
    double xpos, ypos;
    glfwGetCursorPos(window, &xpos, &ypos);
    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    // Convert screen position to world coordinates
    Eigen::Vector4f p_screen(xpos,height-1-ypos,0,1);
    Eigen::Vector4f p_canonical((p_screen[0]/width)*2-1,(p_screen[1]/height)*2-1,0,1);
    Eigen::Vector4f p_world = view.inverse()*p_canonical;

    ver_color=nearest_vertex(p_world[0],p_world[1]);
}

void change_color(int vertex,float r, float g , float b)
{
    if(mode==4)
    {
        V_C(0,vertex)=r;
        V_C(1,vertex)=g;
        V_C(2,vertex)=b;
    }
    VBO_C.update(V_C);

}

void moveTriangle(GLFWwindow* window)
{
//    cout<<"Starting point--"<<globalx<<","<<globaly<<endl;
    double xpos, ypos;
    glfwGetCursorPos(window, &xpos, &ypos);
    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    Eigen::Vector4f p_screen(xpos,height-1-ypos,0,1);
    Eigen::Vector4f p_canonical((p_screen[0]/width)*2-1,(p_screen[1]/height)*2-1,0,1);
    Eigen::Vector4f p_world = view.inverse()*p_canonical;

    double xworldn = p_world[0]-globalx;
    double yworldn = p_world[1]-globaly;
//            cout<<"Moving by--"<<xworldn<<","<<yworldn<<endl;
    V(0,tri_num)=arr[0]+xworldn;
    V(1,tri_num)=arr[1]+yworldn;
    V(0,tri_num+1)=arr[2]+xworldn;
    V(1,tri_num+1)=arr[3]+yworldn;
    V(0,tri_num+2)=arr[4]+xworldn;
    V(1,tri_num+2)=arr[5]+yworldn;
    VBO.update(V);

}

void drawline(GLFWwindow* window)
{

    double xpos, ypos;
    glfwGetCursorPos(window, &xpos, &ypos);
    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    Eigen::Vector4f p_screen2(xpos,height-1-ypos,0,1);
    Eigen::Vector4f p_canonical2((p_screen2[0]/width)*2-1,(p_screen2[1]/height)*2-1,0,1);
    Eigen::Vector4f p_world2 = view.inverse()*p_canonical2;

    temp2.resize(V.rows(),V.cols()+1);
    for(int i=0;i<V.cols();i++)
    {
        temp2.col(i) = V.col(i);
    }
    temp2.col(temp2.cols() - 1) << p_world2[0], p_world2[1], 1.0;
    VBO.update(temp2);

}

void rotate_point(float cx,float cy,float angle,float &px,float &py)
{
    float s = sin(angle);
    float c = cos(angle);

    // translate point back to origin:
    px -= cx;
    py -= cy;

    // rotate point
    float xnew = px * c - py * s;
    float ynew = px * s + py * c;

    // translate point back:
    px = xnew + cx;
    py = ynew + cy;

}

void scale_point(float cx,float cy,float scale,float &px,float &py)
{

    px = cx + (px-cx)*scale;
    py = cy + (py-cy)*scale;

}

void rotate(GLFWwindow* window, float angle)
{
    if(tri_rotate>=0) {
        float centerX = (V(0, tri_rotate) + V(0, tri_rotate + 1) + V(0, tri_rotate + 2)) / 3;
        float centerY = (V(1, tri_rotate) + V(1, tri_rotate + 1) + V(1, tri_rotate + 2)) / 3;
        cout<<"X,Y before-->"<<V(0, tri_rotate)<<","<<V(1, tri_rotate)<<endl;
        rotate_point(centerX,centerY,angle,V(0, tri_rotate),V(1, tri_rotate));
        rotate_point(centerX,centerY,angle,V(0, tri_rotate+1),V(1, tri_rotate+1));
        rotate_point(centerX,centerY,angle,V(0, tri_rotate+2),V(1, tri_rotate+2));
        cout<<"X,Y after-->"<<V(0, tri_rotate)<<","<<V(1, tri_rotate)<<endl;
        VBO.update(V);

    }

}
void scale(GLFWwindow* window, float scale)
{
    if(tri_rotate>=0) {
        float centerX = (V(0, tri_rotate) + V(0, tri_rotate + 1) + V(0, tri_rotate + 2)) / 3;
        float centerY = (V(1, tri_rotate) + V(1, tri_rotate + 1) + V(1, tri_rotate + 2)) / 3;
        cout<<"Scale X,Y before-->"<<V(0, tri_rotate)<<","<<V(1, tri_rotate)<<endl;
        scale_point(centerX,centerY,scale,V(0, tri_rotate),V(1, tri_rotate));
        scale_point(centerX,centerY,scale,V(0, tri_rotate+1),V(1, tri_rotate+1));
        scale_point(centerX,centerY,scale,V(0, tri_rotate+2),V(1, tri_rotate+2));
        cout<<"Scale X,Y after-->"<<V(0, tri_rotate)<<","<<V(1, tri_rotate)<<endl;
        VBO.update(V);

    }

}

void zoomout()
{
    view << view(0,0)*0.8,0,0,view(0,3),
            0,view(1,1)*0.8,0,view(1,3),
            0,0,view(2,2),0,
            0,0,0,view(3,3);

}

void zoomin()
{
    cout<<"Calling zoomin--> View before -->"<<view<<endl;
    view << view(0,0)*1.2,0,0,view(0,3),
            0,view(1,1)*1.2,0,view(1,3),
            0,0,view(2,2),0,
            0,0,0,view(3,3);

    cout<<"View After -->"<<view<<endl;

}

void pan(GLFWwindow* window,char key)
{

    if(key=='d')
    {
        view << view(0,0),0,0,view(0,3)+0.2,
                0,view(1,1),0,view(1,3),
                0,0,view(2,2),0,
                0,0,0,view(3,3);
    }
    if(key=='a')
    {
        view << view(0,0),0,0,view(0,3)-0.2,
                0,view(1,1),0,view(1,3),
                0,0,view(2,2),0,
                0,0,0,view(3,3);
    }
    if(key=='w')
    {
        view << view(0,0),0,0,view(0,3),
                0,view(1,1),0,view(1,3)+0.2,
                0,0,view(2,2),0,
                0,0,0,view(3,3);
    }
    if(key=='s')
    {
        view << view(0,0),0,0,view(0,3),
                0,view(1,1),0,view(1,3)-0.2,
                0,0,view(2,2),0,
                0,0,0,view(3,3);
    }
}

void store_before()
{

        if(tri_animate>0)
        {
            animatematrix.col(0)=V.col(tri_animate);
            animatematrix.col(1)=V.col(tri_animate+1);
            animatematrix.col(2)=V.col(tri_animate+2);

        }

    cout<<"Stored before"<<animatematrix<<endl;

}

void store_after()
{

        if(tri_animate>0)
        {
            animatematrix.col(3)=V.col(tri_animate);
            animatematrix.col(4)=V.col(tri_animate+1);
            animatematrix.col(5)=V.col(tri_animate+2);
        }
    cout<<"Stored After"<<animatematrix<<endl;
}

void animateposition()
{
    if(mode==9)
    {
        if(tri_animate>0)
        {
//            float stepsx0=(animatematrix(0,3)-animatematrix(0,0))/10;
//            float stepsy0=(animatematrix(1,3)-animatematrix(1,0))/10;
//            float stepsz0=(animatematrix(2,3)-animatematrix(2,0))/10;
//            float stepsx1=(animatematrix(0,4)-animatematrix(0,1))/10;
//            float stepsy1=(animatematrix(1,4)-animatematrix(1,1))/10;
//            float stepsz1=(animatematrix(2,4)-animatematrix(2,1))/10;
//            float stepsx2=(animatematrix(0,5)-animatematrix(0,2))/10;
//            float stepsy2=(animatematrix(1,5)-animatematrix(1,2))/10;
//            float stepsz2=(animatematrix(2,5)-animatematrix(2,2))/10;
//            for(float i = 0;i<10;i++)
//            {   cout<<"Running animation"<<endl;
//                V(0,tri_animate)-=stepsx0;
//                V(1,tri_animate)-=stepsy0;
//                V(2,tri_animate)-=stepsz0;
//                V(0,tri_animate+1)-=stepsx1;
//                V(1,tri_animate+1)-=stepsy1;
//                V(2,tri_animate+1)-=stepsz1;
//                V(0,tri_animate+2)-=stepsx2;
//                V(1,tri_animate+2)-=stepsy2;
//                V(2,tri_animate+2)-=stepsz2;
//
//                std::this_thread::sleep_for(std::chrono::seconds(5));
//
//                VBO.update(V);
//            }
            counter=1;
        }
    }
}

void writetosvg(GLFWwindow* window)
{
    int width, height;
    glfwGetWindowSize(window, &width, &height);
    ofstream myfile;
    myfile.open ("/Users/Rachit/Desktop/example.svg");
    myfile << "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n";
    myfile << "<svg width=\""<<width<<"\" height=\""<<height<<"\" version=\"1.200000\" xmlns=\"http://www.w3.org/2000/svg\""
            " xmlns:xlink=\"http://www.w3.org/1999/xlink\"  >\n";

    for(int i=3;i<V.cols();i=i+3)
    {
        Eigen::Vector4f a(V(0,i),V(1,i),0,1);
        Eigen::Vector4f b(V(0,i+1),V(1,i+1),0,1);
        Eigen::Vector4f c(V(0,i+2),V(1,i+2),0,1);
        a= view * a;
        b= view * b;
        c= view * c;

        float ax,ay,bx,by,cx,cy;
        ax=((( a[0] + 1 ) / 2.0) *
            width );
        ay=((( 1 - a[1] ) / 2.0) *
            height );
        bx=((( b[0] + 1 ) / 2.0) *
            width );
        by=((( 1 - b[1] ) / 2.0) *
            height ) ;
        cx=((( c[0] + 1 ) / 2.0) *
            width );
        cy=((( 1 - c[1] ) / 2.0) *
            height );

        myfile << " <defs>\n"
                "      <linearGradient id=\"fadeA-1"<<i<<"\" gradientUnits=\"objectBoundingBox\" x1=\"0.5\" y1=\"0\" x2=\"1\" y2=\"1\">\n"
                "        <stop offset=\"0%\" stop-color=\"rgb("<<roundf((float)V_C(0,i)*255)<<","<<roundf((float)V_C(1,i)*255)<<","<<roundf((float)V_C(2,i)*255)<<")\"/>\n"
                "        <stop offset=\"100%\" stop-color=\"rgb("<<roundf((float)V_C(0,i+2)*255)<<","<<roundf((float)V_C(1,i+2)*255)<<","<<roundf((float)V_C(2,i+2)*255)<<")\"/>\n"
                "      </linearGradient>\n"
                "      <linearGradient id=\"fadeB-1"<<i<<"\" gradientUnits=\"objectBoundingBox\" x1=\"0\" y1=\"1\" x2=\"0.75\" y2=\"0.5\">\n"
                "        <stop offset=\"0%\" stop-color=\"rgb("<<roundf((float)V_C(0,i+1)*255)<<","<<roundf((float)V_C(1,i+1)*255)<<","<<roundf((float)V_C(2,i+1)*255)<<")\"/>\n"
                "        <stop offset=\"100%\" stop-color=\"rgb("<<roundf((float)V_C(0,i+1)*255)<<","<<roundf((float)V_C(1,i+1)*255)<<","<<roundf((float)V_C(2,i+1)*255)<<")\" stop-opacity=\"0\" />\n"
                "      </linearGradient>\n"
                "      </defs>\n"
                "      <path id=\"pathA-1"<<i<<"\" d=\"M "<<ax<<","<<ay<<" L "<<bx<<","<<by<<" "<<cx<<","<<cy<<" Z\" fill=\"url(#fadeA-1"<<i<<")\"/>\n"
                "      <path id=\"pathB-1"<<i<<"\" d=\"M "<<ax<<","<<ay<<" L "<<bx<<","<<by<<" "<<cx<<","<<cy<<" Z\" fill=\"url(#fadeB-1"<<i<<")\"/>\n";
    }

    myfile <<"</svg>\n";
    myfile.close();
    cout<<"Done writing";

}

void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods)
{
    // Update the position of the first vertex if the keys 1,2, or 3 are pressed
    if(action==GLFW_PRESS)
        switch (key)
        {
            case  GLFW_KEY_1:
                change_color(ver_color,1.0f,0.0f,0.0f);
                break;
            case GLFW_KEY_2:
                change_color(ver_color,0.0f,1.0f,0.0f);
                break;
            case  GLFW_KEY_3:
                change_color(ver_color,0.0f,0.0f,1.0f);
                break;
            case  GLFW_KEY_4:
                change_color(ver_color,1.0f,1.0f,1.0f);
                break;
            case  GLFW_KEY_5:
                change_color(ver_color,0.0f,0.0f,0.0f);
                break;
            case  GLFW_KEY_6:
                change_color(ver_color,0.5f,0.5f,0.0f);
                break;
            case  GLFW_KEY_7:
                change_color(ver_color,0.0f,0.5f,0.5f);
                break;
            case  GLFW_KEY_8:
                change_color(ver_color,0.5f,0.0f,0.5f);
                break;
            case  GLFW_KEY_9:
                change_color(ver_color,0.25f,0.5f,0.25f);
                break;
            case GLFW_KEY_I:
                mode=0;
                tri_count=0;
                glfwSetMouseButtonCallback(window, mouse_button_make_triangle);
                break;
            case GLFW_KEY_O:
                mode=1;
                glfwSetMouseButtonCallback(window, mouse_button_move_triangle);
                break;
            case GLFW_KEY_P:
                mode=3;
                glfwSetMouseButtonCallback(window, mouse_button_delete_triangle);
                break;
            case GLFW_KEY_C:
                mode=4;
                glfwSetMouseButtonCallback(window, mouse_button_color_triangle);
                break;
//        case GLFW_KEY_T:
//            mode = -1;
//            glfwSetMouseButtonCallback(window, mouse_button_rotate_triangle);
//            break;
            case GLFW_KEY_H:
                //positive
                rotate(window,0.174533);
                break;
            case GLFW_KEY_J:
                //negative
                rotate(window,-0.174533);
                break;
            case GLFW_KEY_K:
                //scaleup
                scale(window,1.25);
                break;
            case GLFW_KEY_L:
                //scaledown
                scale(window,0.75);
                break;
            case GLFW_KEY_EQUAL:
                //Zoomin
                zoomin();
                break;
            case GLFW_KEY_MINUS:
                //Zoomout
                zoomout();
                break;
            case GLFW_KEY_W:
                pan(window,'w');
                break;
            case GLFW_KEY_S:
                pan(window,'s');
                break;
            case GLFW_KEY_A:
                pan(window,'a');
                break;
            case GLFW_KEY_D:
                pan(window,'d');
                break;

            case GLFW_KEY_Q:
                mode=9;
                glfwSetMouseButtonCallback(window, mouse_button_animate_triangle);
                break;

            case GLFW_KEY_Z:
                store_before();
                break;
            case GLFW_KEY_X:
                mode=9;
                store_after();
                animateposition();
                break;
            case GLFW_KEY_U:
                writetosvg(window);
                break;
             default:
                break;
        }

    // Upload the change to the GPU
    VBO.update(V);
}

int main(void)
{
    GLFWwindow* window;

    // Initialize the library
    if (!glfwInit())
        return -1;

    // Activate supersampling
    glfwWindowHint(GLFW_SAMPLES, 8);

    // Ensure that we get at least a 3.2 context
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 2);

    // On apple we have to load a core profile with forward compatibility
#ifdef __APPLE__
    glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
    glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
#endif

    // Create a windowed mode window and its OpenGL context
    window = glfwCreateWindow(700, 700, "Hello World", NULL, NULL);
    if (!window)
    {
        glfwTerminate();
        return -1;
    }

    // Make the window's context current
    glfwMakeContextCurrent(window);

    // Initialize the VAO
    // A Vertex Array Object (or VAO) is an object that describes how the vertex
    // attributes are stored in a Vertex Buffer Object (or VBO). This means that
    // the VAO is not the actual object storing the vertex data,
    // but the descriptor of the vertex data.
    VertexArrayObject VAO;
    VAO.init();
    VAO.bind();

    // Initialize the VBO with the vertices data
    // A VBO is a data container that lives in the GPU memory
    VBO.init();

//    V.resize(2,3);
//    V << -0.5,  0.5, 0.5, 0.5, 0.5, -0.5;
//
//    VBO.update(V);

    // Initialize the OpenGL Program
    // A program controls the OpenGL pipeline and it must contains
    // at least a vertex shader and a fragment shader to be valid
    Program program;
    const GLchar* vertex_shader =
            "#version 150 core\n"
                    "in vec2 position;"
                    "in vec3 color;"
                    "out vec3 f_color;"
                    "uniform mat4 view;"
                    "void main()"
                    "{"
                    "    gl_Position =  view * vec4(position, 0.0, 1.0) ;"
                    "    f_color = color;"
                    "}";
    const GLchar* fragment_shader =
            "#version 150 core\n"
                    "in vec3 f_color;"
                    "out vec4 outColor;"
                    "uniform vec3 triangleColor;"
                    "void main()"
                    "{"
                    "    outColor = vec4(f_color, 1.0);"
                    "}";

    // Compile the two shaders and upload the binary to the GPU
    // Note that we have to explicitly specify that the output "slot" called outColor
    // is the one that we want in the fragment buffer (and thus on screen)
    program.init(vertex_shader,fragment_shader,"outColor");
    program.bind();

    // The vertex shader wants the position of the vertices as an input.
    // The following line connects the VBO we defined above with the position "slot"
    // in the vertex shader
//    program.bindVertexAttribArray("position",VBO);

    //custom code
    VBO.init();
    V.resize(3,3);
//    cout<<"Values after resize are "<<V<<endl;
    V  << 0,0,0,0,0,0,1,1,1;

//    cout<<"orignal V is"<<V<<endl;
//V << 0.5,  -0.5, -0.5,-0.5,0.5,0.5, -0.5, -0.5, 0.5,0.5,0.5,-0.5,1,1,1,1,1,1;
//            -0.5,  0.5, 0.5, 0.5, 0.5, -0.5,1;
    VBO.update(V);

    VBO_C.init();
    V_C.resize(3,3);
    V_C<<
       1,  0, 0,
            1,  0, 0,
            1,  0, 0;

    VBO_C.update(V_C);

    view << 1,0,0,0,
            0,1,0,0,
            0,0,1,0,
            0,0,0,1;

    program.bindVertexAttribArray("position",VBO);
    program.bindVertexAttribArray("color",VBO_C);

    // Save the current time --- it will be used to dynamically change the triangle color
    auto t_start = std::chrono::high_resolution_clock::now();

    // Register the keyboard callback
    glfwSetKeyCallback(window, key_callback);

    // Register the mouse callback
//    glfwSetMouseButtonCallback(window, mouse_button_callback);

    // Loop until the user closes the window
    int widths, heights;
    glfwGetWindowSize(window, &widths, &heights);
    float aspect_start=float(heights)/float(widths);
    int i=0;
    while (!glfwWindowShouldClose(window))
    {
        // Bind your VAO (not necessary if you have only one)
//        VAO.bind();
        int width, height;
        glfwGetWindowSize(window, &width, &height);
        float aspect_ratiox = float(height)/float(heights); // corresponds to the necessary width scaling
        float aspect_ratioy = float(width)/float(widths); // corresponds to the necessary width scaling
        float aspect_ratio=float(height)/float(width);
        if(aspect_ratio!=aspect_start) {
            view(0, 0) = view(0, 0) * aspect_ratioy;
            view(1,1) *= aspect_ratiox;
            aspect_start=aspect_ratio;
            widths=width;
            heights=height;
        }
        // Bind your program
        program.bind();

        // Set the uniform value depending on the time difference
//        auto t_now = std::chrono::high_resolution_clock::now();
//        float time = std::chrono::duration_cast<std::chrono::duration<float>>(t_now - t_start).count();
//        glUniform3f(program.uniform("triangleColor"), (float)(sin(time * 4.0f) + 1.0f) / 2.0f, 0.0f, 0.0f);
//        glUniform3f(program.uniform("triangleColor"), 1.0f, 0.0f, 0.0f);
        // Clear the framebuffer
        glClearColor(0.5f, 0.5f, 0.5f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT);

        // Draw a triangle
//        if(V.cols()%3==0)
        glDrawArrays(GL_TRIANGLES, 0, V.cols());
//        glDrawArrays(GL_TRIANGLES, 3, 3);
        // Swap front and back buffers
        if(tri_count==1 && mode==0) {

            drawline(window);
            glDrawArrays(GL_LINE_LOOP, temp2.cols()-2, 2);
//            cout<<"temp 2 is --> "<<temp2<<endl;
        }
        if(tri_count==2 && mode==0) {

            drawline(window);
            glDrawArrays(GL_LINE_LOOP, temp2.cols()-3, 3);
//            cout<<"temp 2 is --> "<<temp2<<endl;
        }
        if(mode==1 && tri_num > 0)
        {
            moveTriangle(window);
            int state = glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT);
            if(state == GLFW_RELEASE)
            {
                cout<<"Calling Released"<<endl;
                mode = 2;
                globalx=0;
                globaly=0;
                tri_num=-1;
                arr[0]=0;
                arr[1]=0;
                arr[2]=0;
                arr[3]=0;
                arr[4]=0;
                arr[5]=0;
            }
        }
        if(mode==9 && counter==1)
        {
            float stepsx0=(animatematrix(0,3)-animatematrix(0,0))/20;
            float stepsy0=(animatematrix(1,3)-animatematrix(1,0))/20;
            float stepsz0=(animatematrix(2,3)-animatematrix(2,0))/20;
            float stepsx1=(animatematrix(0,4)-animatematrix(0,1))/20;
            float stepsy1=(animatematrix(1,4)-animatematrix(1,1))/20;
            float stepsz1=(animatematrix(2,4)-animatematrix(2,1))/20;
            float stepsx2=(animatematrix(0,5)-animatematrix(0,2))/20;
            float stepsy2=(animatematrix(1,5)-animatematrix(1,2))/20;
            float stepsz2=(animatematrix(2,5)-animatematrix(2,2))/20;
            if (i<=19)
            {   cout<<"Running animation"<<endl;
                V(0,tri_animate)-=stepsx0;
                V(1,tri_animate)-=stepsy0;
                V(2,tri_animate)-=stepsz0;
                V(0,tri_animate+1)-=stepsx1;
                V(1,tri_animate+1)-=stepsy1;
                V(2,tri_animate+1)-=stepsz1;
                V(0,tri_animate+2)-=stepsx2;
                V(1,tri_animate+2)-=stepsy2;
                V(2,tri_animate+2)-=stepsz2;

                std::this_thread::sleep_for(std::chrono::milliseconds(100));

                VBO.update(V);
                i++;
            }
            if(i==20) {
                counter = 0;
                i=0;
            }
        }

        //Binding the view matrix
        glUniformMatrix4fv(program.uniform("view"),1,GL_FALSE,view.data());

        glfwSwapBuffers(window);

        // Poll for and process events
        glfwPollEvents();
    }

    // Deallocate opengl memory
    program.free();
    VAO.free();
    VBO.free();

    // Deallocate glfw internals
    glfwTerminate();
    return 0;
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <sstream>
#include <string>
#include <type_traits>
using namespace boost::hana;
using namespace std::literals;

int main() {

{

//! [adjust_if]
BOOST_HANA_CONSTEXPR_LAMBDA auto negative = [](auto x) {
    return x < 0;
};

BOOST_HANA_CONSTEXPR_LAMBDA auto negate = [](auto x) {
    return -x;
};

BOOST_HANA_CONSTEXPR_CHECK(
    adjust_if(make<Tuple>(-3, -2, -1, 0, 1, 2, 3), negative, negate)
    ==
    make<Tuple>(3, 2, 1, 0, 1, 2, 3)
);
//! [adjust_if]

}{

//! [adjust]
BOOST_HANA_CONSTEXPR_LAMBDA auto negate = [](auto x) {
    return -x;
};

BOOST_HANA_CONSTEXPR_CHECK(
    adjust(make<Tuple>(1, 4, 9, 2, 3, 4), 4, negate)
    ==
    make<Tuple>(1, -4, 9, 2, 3, -4)
);
//! [adjust]

}{

//! [fill]
BOOST_HANA_CONSTEXPR_CHECK(
    fill(make<Tuple>(1, '2', 3.3, nullptr), 'x') == make<Tuple>('x', 'x', 'x', 'x')
);

BOOST_HANA_CONSTANT_CHECK(fill(nothing, 'x') == nothing);
BOOST_HANA_CONSTEXPR_CHECK(fill(just('y'), 'x') == just('x'));
//! [fill]

}{

//! [transform]
auto to_string = [](auto x) {
    return static_cast<std::ostringstream const&>(std::ostringstream{} << x).str();
};

BOOST_HANA_RUNTIME_CHECK(
    transform(make<Tuple>(1, '2', "345", std::string{"67"}), to_string)
    ==
    make<Tuple>("1", "2", "345", "67")
);

BOOST_HANA_CONSTANT_CHECK(transform(nothing, to_string) == nothing);
BOOST_HANA_RUNTIME_CHECK(transform(just(123), to_string) == just("123"s));

BOOST_HANA_CONSTANT_CHECK(
    transform(tuple_t<void, int(), char[10]>, template_<std::add_pointer_t>)
            ==
    tuple_t<void*, int(*)(), char(*)[10]>
);
//! [transform]

}{

//! [replace_if]
BOOST_HANA_CONSTEXPR_LAMBDA auto negative = [](auto x) {
    return x < 0;
};

BOOST_HANA_CONSTEXPR_CHECK(
    replace_if(make<Tuple>(-3, -2, -1, 0, 1, 2, 3), negative, 0)
    ==
    make<Tuple>(0, 0, 0, 0, 1, 2, 3)
);
//! [replace_if]

}{

//! [replace]
BOOST_HANA_CONSTEXPR_CHECK(
    replace(make<Tuple>(1, 1, 1, 2, 3, 1, 4, 5), 1, 0)
    ==
    make<Tuple>(0, 0, 0, 2, 3, 0, 4, 5)
);
//! [replace]

}

}


#include <iostream>                      // std::cout/endl/dec/hex
#include <boost/multiprecision/gmp.hpp>  // boost::multiprecision::gmp_int

using namespace std;

int main()
{
    typedef boost::multiprecision::number<boost::multiprecision::gmp_int,
                                          boost::multiprecision::et_off>
        int_type;

    int_type a{"0x123456789abcdef0"};
    int_type b = 16;
    int_type c{"0400"};
    int_type result = a * b / c;
    cout << hex << result << endl;
    cout << dec << result << endl;
}


#ifndef STAN_OPTIMIZATION_NEWTON_HPP
#define STAN_OPTIMIZATION_NEWTON_HPP

#include <stan/model/grad_hess_log_prob.hpp>
#include <stan/model/log_prob_grad.hpp>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
#include <vector>

namespace stan {
namespace optimization {

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_d;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vector_d;

// Negates any positive eigenvalues in H so that H is negative
// definite, and then solves Hu = g and stores the result into
// g. Avoids problems due to non-log-concave distributions.
inline void make_negative_definite_and_solve(matrix_d& H, vector_d& g) {
  Eigen::SelfAdjointEigenSolver<matrix_d> solver(H);
  matrix_d eigenvectors = solver.eigenvectors();
  vector_d eigenvalues = solver.eigenvalues();
  vector_d eigenprojections = eigenvectors.transpose() * g;
  for (int i = 0; i < g.size(); i++) {
    eigenprojections[i] = -eigenprojections[i] / fabs(eigenvalues[i]);
  }
  g = eigenvectors * eigenprojections;
}

template <typename M>
double newton_step(M& model, std::vector<double>& params_r,
                   std::vector<int>& params_i,
                   std::ostream* output_stream = 0) {
  std::vector<double> gradient;
  std::vector<double> hessian;

  double f0 = stan::model::grad_hess_log_prob<true, false>(
      model, params_r, params_i, gradient, hessian);
  matrix_d H(params_r.size(), params_r.size());
  for (size_t i = 0; i < hessian.size(); i++) {
    H(i) = hessian[i];
  }
  vector_d g(params_r.size());
  for (size_t i = 0; i < gradient.size(); i++)
    g(i) = gradient[i];
  make_negative_definite_and_solve(H, g);
  //         H.ldlt().solveInPlace(g);

  std::vector<double> new_params_r(params_r.size());
  double step_size = 2;
  double min_step_size = 1e-50;
  double f1 = -1e100;

  while (f1 < f0) {
    step_size *= 0.5;
    if (step_size < min_step_size)
      return f0;

    for (size_t i = 0; i < params_r.size(); i++)
      new_params_r[i] = params_r[i] - step_size * g[i];
    try {
      f1 = stan::model::log_prob_grad<true, false>(model, new_params_r,
                                                   params_i, gradient);
    } catch (std::exception& e) {
      // FIXME:  this is not a good way to handle a general exception
      f1 = -1e100;
    }
  }
  for (size_t i = 0; i < params_r.size(); i++)
    params_r[i] = new_params_r[i];

  return f1;
}

}  // namespace optimization
}  // namespace stan
#endif


#include <cmath>
#include <complex>
#include <ctime>
#include <string>
// Boost random generator
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
// Eigen dense matrices
#include <Eigen/Dense>

#include "function_dispatcher.h"
#include "json_object.h"
#include "numeric_utils.h"
#include "wittig_sinha.h"

stochastic::WittigSinha::WittigSinha(std::string exposure_category,
                                     double gust_speed, double height,
                                     unsigned int num_floors, double total_time)
    : StochasticModel(),
      exposure_category_{exposure_category},
      gust_speed_{gust_speed * 0.44704}, // Convert from mph to m/s
      bldg_height_{height},
      num_floors_{num_floors},
      seed_value_{std::numeric_limits<int>::infinity()},
      local_x_{std::vector<double>(1, 1.0)},
      local_y_{std::vector<double>(1, 1.0)},
      freq_cutoff_{5.0},
      time_step_{1.0 / (2.0 * freq_cutoff_)} {
  model_name_ = "WittigSinha";
  num_times_ =
      static_cast<unsigned int>(std::ceil(total_time / time_step_)) % 2 == 0
          ? static_cast<unsigned int>(std::ceil(total_time / time_step_))
          : static_cast<unsigned int>(std::ceil(total_time / time_step_) + 1);

  // Calculate range of frequencies based on cutoff frequency
  num_freqs_ = num_times_ / 2;
  frequencies_.resize(num_freqs_);

  for (unsigned int i = 0; i < frequencies_.size(); ++i) {
    frequencies_[i] = (i + 1) * freq_cutoff_ / num_freqs_;
  }

  // Calculate heights of each floor
  heights_ = std::vector<double>(num_floors_);
  heights_[0] = bldg_height_ / num_floors_;

  for (unsigned int i = 1; i < heights_.size(); ++i) {
    heights_[i] = heights_[i - 1] + bldg_height_ / num_floors_;
  }

  // Calculate velocity profile
  friction_velocity_ =
      Dispatcher<double, const std::string&, const std::vector<double>&, double,
                 double, std::vector<double>&>::instance()
          ->dispatch("ExposureCategoryVel", exposure_category, heights_, 0.4,
                     gust_speed, wind_velocities_);
}

stochastic::WittigSinha::WittigSinha(std::string exposure_category,
                                     double gust_speed, double height,
                                     unsigned int num_floors, double total_time,
                                     int seed_value)
    : WittigSinha(exposure_category, gust_speed, height, num_floors,
                  total_time)
{
  seed_value_ = seed_value;
}

stochastic::WittigSinha::WittigSinha(std::string exposure_category,
                                     double gust_speed,
                                     const std::vector<double>& heights,
                                     const std::vector<double>& x_locations,
                                     const std::vector<double>& y_locations,
                                     double total_time)
    : StochasticModel(),
      exposure_category_{exposure_category},
      gust_speed_{gust_speed * 0.44704}, // Convert from mph to m/s
      seed_value_{std::numeric_limits<int>::infinity()},
      heights_{heights},
      local_x_{x_locations},
      local_y_{y_locations},
      freq_cutoff_{5.0},
      time_step_{1.0 / (2.0 * freq_cutoff_)}
{
  model_name_ = "WittigSinha";
  num_times_ =
      static_cast<unsigned int>(std::ceil(total_time / time_step_)) % 2 == 0
          ? static_cast<unsigned int>(std::ceil(total_time / time_step_))
          : static_cast<unsigned int>(std::ceil(total_time / time_step_) + 1);

  // Calculate range of frequencies based on cutoff frequency
  num_freqs_ = num_times_ / 2;
  frequencies_.resize(num_freqs_);

  for (unsigned int i = 0; i < frequencies_.size(); ++i) {
    frequencies_[i] = i * freq_cutoff_ / num_freqs_;
  }

  // Calculate velocity profile
  friction_velocity_ =
      Dispatcher<double, const std::string&, const std::vector<double>&, double,
                 double, std::vector<double>&>::instance()
          ->dispatch("ExposureCategoryVel", exposure_category, heights_, 0.4,
                     gust_speed, wind_velocities_);
}

stochastic::WittigSinha::WittigSinha(std::string exposure_category,
                                     double gust_speed,
                                     const std::vector<double>& heights,
                                     const std::vector<double>& x_locations,
                                     const std::vector<double>& y_locations,
                                     double total_time, int seed_value)
  : WittigSinha(exposure_category, gust_speed, heights, x_locations, y_locations, total_time)
{
  seed_value_ = seed_value;
}

utilities::JsonObject stochastic::WittigSinha::generate(const std::string& event_name, bool units) {
  // Initialize wind velocity vectors
  std::vector<std::vector<std::vector<std::vector<double>>>> wind_vels(
      local_x_.size(),
      std::vector<std::vector<std::vector<double>>>(
          local_y_.size(),
          std::vector<std::vector<double>>(
              heights_.size(), std::vector<double>(num_times_, 0.0))));

  Eigen::MatrixXcd complex_random_vals(num_freqs_, heights_.size());

  // Loop over heights to find time histories
  try {
    for (unsigned int i = 0; i < local_x_.size(); ++i) {
      for (unsigned int j = 0; j < local_y_.size(); ++j) {
        // Generate complex random numbers to use for calculation of discrete
        // time series
        complex_random_vals = complex_random_numbers();
        for (unsigned int k = 0; k < heights_.size(); ++k) {
          wind_vels[i][j][k] = gen_location_hist(complex_random_vals, k, units);
        }
      }
    }
  } catch (const std::exception& e) {
    std::cerr << "\nERROR: In stochastic::WittigSinha::generate: "
              << e.what() << std::endl;
  }

  // Create JsonObject for event
  auto event = utilities::JsonObject();
  event.add_value("dT", time_step_);
  event.add_value("numSteps", num_times_);

  // Consider case when only looking at floor loads, so only have time histories as
  // one location along the z-axis
  if (local_x_.size() == 1 && local_y_.size() == 1) {
    // Arrays of patterns and time histories for each floor
    std::vector<utilities::JsonObject> pattern_array(heights_.size());
    std::vector<utilities::JsonObject> event_array(1);
    std::vector<utilities::JsonObject> time_history_array(heights_.size());
    auto time_history = utilities::JsonObject();
    event_array[0].add_value("type", "Wind");
    event_array[0].add_value("subtype", model_name_);

    for (unsigned int i = 0; i < heights_.size(); ++i) {
      // Create pattern
      pattern_array[i].add_value("name", std::to_string(i + 1));
      pattern_array[i].add_value("timeSeries", std::to_string(i + 1));
      pattern_array[i].add_value("type", "WindFloorLoad");
      pattern_array[i].add_value("floor", std::to_string(i + 1));
      pattern_array[i].add_value("dof", 1);
      pattern_array[i].add_value("profileVelocity", wind_velocities_[i]);

      // Create time histories
      time_history.add_value("name", std::to_string(i + 1));
      time_history.add_value("dT", time_step_);
      time_history.add_value("type", "Value");
      time_history.add_value("data", wind_vels[0][0][i]);
      time_history_array[i] = time_history;
      time_history.clear();
    }

    event_array[0].add_value("timeSeries", time_history_array);
    event_array[0].add_value("pattern", pattern_array);
    event.add_value("Events", event_array);
  } else {
    throw std::runtime_error(
        "ERROR: In stochastic::WittigSinha::generate: Currently, only supports "
        "time histories along z-axis at single location\n");
  }

  return event;
}

bool stochastic::WittigSinha::generate(const std::string& event_name,
                                       const std::string& output_location,
                                       bool units) {

  bool status = true;
  // Generate time histories at specified locations
  try {
    auto json_output = generate(event_name, units);
    json_output.write_to_file(output_location);
  } catch (const std::exception& e) {
    std::cerr << e.what();
    status = false;
    throw;
  }

  return status;
}

Eigen::MatrixXd stochastic::WittigSinha::cross_spectral_density(double frequency) const {
  // Coefficient for coherence function
  double coherence_coeff = 10.0;
  Eigen::MatrixXd cross_spectral_density =
      Eigen::MatrixXd::Zero(heights_.size(), heights_.size());

  for (unsigned int i = 0; i < cross_spectral_density.rows(); ++i) {
    cross_spectral_density(i, i) =
        200.0 * friction_velocity_ * friction_velocity_ * heights_[i] /
        (wind_velocities_[i] *
         std::pow(1.0 + 50.0 * frequency * heights_[i] / wind_velocities_[i],
                  5.0 / 3.0));
  }

  for (unsigned int i = 0; i < cross_spectral_density.rows(); ++i) {
    for (unsigned int j = i + 1; j < cross_spectral_density.cols(); ++j) {
      cross_spectral_density(i, j) =
          std::sqrt(cross_spectral_density(i, i) *
                    cross_spectral_density(j, j)) *
          std::exp(-coherence_coeff * frequency *
                   std::abs(heights_[i] - heights_[j]) /
                   (0.5 * (wind_velocities_[i] + wind_velocities_[j]))) *
          0.999;
    }
  }

  // Get diagonal of cross spectral density matrix--avoids compiler errors where type
  // of diagonal matrix is not correctly deduced
  Eigen::MatrixXd diag_mat = cross_spectral_density.diagonal().asDiagonal();

  return cross_spectral_density.transpose() + cross_spectral_density - diag_mat;
}

Eigen::MatrixXcd stochastic::WittigSinha::complex_random_numbers() const {
  // Construct random number generator for standard normal distribution
  static unsigned int history_seed = static_cast<unsigned int>(std::time(nullptr));
  history_seed = history_seed + 10;

  auto generator =
    seed_value_ != std::numeric_limits<int>::infinity()
    ? boost::random::mt19937(static_cast<unsigned int>(seed_value_ + 10))
    : boost::random::mt19937(history_seed);

  boost::random::normal_distribution<> distribution;
  boost::random::variate_generator<boost::random::mt19937&,
                                   boost::random::normal_distribution<>>
      distribution_gen(generator, distribution);

  // Generate white noise consisting of complex numbers
  Eigen::MatrixXcd white_noise(heights_.size(), num_freqs_);

  for (unsigned int i = 0; i < white_noise.rows(); ++i) {
    for (unsigned int j = 0; j < white_noise.cols(); ++j) {
      white_noise(i, j) = std::complex<double>(
          distribution_gen() * std::sqrt(0.5),
          distribution_gen() * std::sqrt(std::complex<double>(-0.5)).imag());
    }
  }

  // Iterator over all frequencies and generate complex random numbers
  // for discrete time series simulation
  Eigen::MatrixXd cross_spec_density_matrix(heights_.size(), heights_.size());
  Eigen::MatrixXcd complex_random(num_freqs_, heights_.size());

  for (unsigned int i = 0; i < frequencies_.size(); ++i) {
    // Calculate cross-spectral density matrix for current frequency
    cross_spec_density_matrix = cross_spectral_density(frequencies_[i]);

    // Find lower Cholesky factorization of cross-spectral density
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> lower_cholesky;

    try {
      auto llt = cross_spec_density_matrix.llt();
      lower_cholesky = llt.matrixL();

      if (llt.info() == Eigen::NumericalIssue) {
        throw std::runtime_error(
            "\nERROR: In stochastic::WittigSinha::generate method: Cross-Spectral Density "
            "matrix is not positive semi-definite\n");
      }
    } catch (const std::exception& e) {
      std::cerr << "\nERROR: In time history generation: " << e.what()
                << std::endl;
    }

    // This is Equation 5(a) from Wittig & Sinha (1975)
    complex_random.row(i) = num_freqs_ *
                            std::sqrt(2.0 * freq_cutoff_ / num_freqs_) *
                            lower_cholesky * white_noise.col(i);
  }

  return complex_random;
}

std::vector<double> stochastic::WittigSinha::gen_location_hist(
    const Eigen::MatrixXcd& random_numbers, unsigned int column_index,
    bool units) const {

  // This following block implements what is expressed in Equations 7 & 8
  Eigen::VectorXcd complex_full_range = Eigen::VectorXcd::Zero(2 * num_freqs_);

  complex_full_range.segment(1, num_freqs_) =
      random_numbers.block(0, column_index, num_freqs_, 1);

  complex_full_range.segment(num_freqs_ + 1, num_freqs_ - 1) =
      random_numbers.block(0, column_index, num_freqs_ - 1, 1)
          .reverse()
          .conjugate();

  complex_full_range(num_freqs_) = std::abs(random_numbers(num_freqs_ - 1, column_index));

  // Calculate wind speed using real portion of inverse Fast Fourier Transform
  // full range of random numbers
  std::vector<double> node_time_history(complex_full_range.size());
  numeric_utils::inverse_fft(complex_full_range, node_time_history);

  // Check if time histories need to be converted to ft/s
  if (units) {
    for (auto & val : node_time_history) {
      val = val * 3.28084;
    }
  }

  return node_time_history;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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 BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_FAST_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_FAST_HYPOT_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/fast_hypot.hpp>
#include <boost/simd/include/functions/scalar/sqrt.hpp>
#include <boost/simd/include/functions/scalar/sqr.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>
#include <boost/simd/include/functions/scalar/is_inf.hpp>
#include <boost/simd/include/functions/scalar/ldexp.hpp>
#include <boost/simd/include/functions/scalar/fma.hpp>
#include <boost/simd/include/constants/eps.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_hypot_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                              (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return boost::simd::fast_hypot(static_cast<result_type>(a0),
                                     static_cast<result_type>(a1));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_hypot_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                              (scalar_< double_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
#if !defined(BOOST_SIMD_NO_NANS) && !defined(BOOST_SIMD_NO_INFINITIES)
      A0 x =  boost::simd::abs(a0);
      A0 y =  boost::simd::abs(a1);
      if (boost::simd::is_inf(x+y)) return Inf<float>();
      if (boost::simd::is_inf(x)) return Inf<float>();
      if (boost::simd::is_inf(y)) return Inf<float>();
      if (y > x) std::swap(x, y);
      if (x*Eps<A0>() >=  y) return x;
      return x*boost::simd::sqrt(One<A0>()+boost::simd::sqr(y/x));
#else
      return boost::simd::sqrt(boost::simd::fma(a0, a0, a1*a1));
#endif
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_hypot_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                              (scalar_< single_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
#if !defined(BOOST_SIMD_NO_NANS) && !defined(BOOST_SIMD_NO_INFINITIES)
      // flibc do that in ::fast_hypotf(a0, a1) in asm with no more speed!
      // proper impl as for double is 30% slower
      return static_cast<result_type>(boost::simd::sqrt(fma(static_cast<double>(a0), static_cast<double>(a0),
                                                            boost::simd::sqr(static_cast<double>(a1)))));
#else
      return boost::simd::sqrt(boost::simd::fma(a0, a0, a1*a1));
#endif
    }
  };
} } }

#endif