//==================================================================================================
/*!
  @file

  Copyright 2016 NumScale SAS

  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/simd/pack.hpp>
#include <boost/simd/function/log10.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/ten.hpp>
#include <boost/simd/constant/three.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : bs::rec(T(i));
    b[i] = bs::plain_(bs::log10)(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_ULP_EQUAL(/*bs::plain_*/(bs::log10)(aa1), bb, 0.5);
}

STF_CASE_TPL("Check log10 on pack", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

STF_CASE_TPL (" log10", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::log10;
  using p_t = bs::pack<T>;

  using r_t = decltype(bs::plain_(log10)(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);
  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::plain_(log10)(bs::Inf<p_t>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(bs::plain_(log10)(bs::Minf<p_t>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(bs::plain_(log10)(bs::Nan<p_t>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(bs::plain_(log10)(bs::Mone<p_t>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(bs::plain_(log10)(bs::Zero<p_t>()), bs::Minf<r_t>(), 0);
  STF_ULP_EQUAL(bs::plain_(log10)(bs::Mzero<p_t>()), bs::Minf<r_t>(), 0);
#endif
  STF_ULP_EQUAL(bs::plain_(log10)(bs::One<p_t>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(bs::plain_(log10)(bs::Two<p_t>()), p_t(0.301029995663981195213738894724), 0);
  STF_ULP_EQUAL(bs::plain_(log10)(bs::Three<p_t>()),p_t(0.477121254719662437295027903255), 0.5);
}


#include <iostream>
#include <typeinfo>
#define BOOST_TEST_MODULE SparseMIAAddSubtractTests

#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 "DenseMIA.h"
#include "SparseMIA.h"
#include "Index.h"
#include "LibMIAUtil.h"
template<class _data_type>
void do_work(size_t dim1,size_t dim2){

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

    LibMIA::DenseMIA<_data_type,4> dense_a(dim1,dim1,dim2,dim2);
    LibMIA::DenseMIA<_data_type,4> dense_b(dim2,dim1,dim2,dim1);
    LibMIA::DenseMIA<_data_type,4> dense_c(dim2,dim1,dim2,dim1);

    dense_a.ones();
    dense_b.ones();

    LibMIA::SparseMIA<_data_type,4> a(dense_a);
    LibMIA::SparseMIA<_data_type,4> b(dense_b);
    LibMIA::SparseMIA<_data_type,4> c(dim2,dim1,dim2,dim1);
    //a.print();
    //b.print();
    //boost::timer::cpu_timer scan_t,total_t;

    std::array<size_t,4> new_linIdxSequence{{2,0,3,1}};
    std::array<size_t,4> new_linIdxSequence2{{1,3,0,2}};
    a.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);

    //scan_t.stop();
    //c.print();
    //std::cout << "Scan add " << boost::timer::format(scan_t.elapsed()) << std::endl;
    //boost::timer::cpu_timer dense_t;
    dense_c(i,j,k,l)=dense_b(i,j,k,l)+dense_a(j,l,i,k);
    //std::cout << "Dense Scan add " << boost::timer::format(dense_t.elapsed()) << std::endl;

    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Add (Scan) 1a for ")+typeid(_data_type).name());

    //now check when b is not sorted
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Add (Scan) 1b for ")+typeid(_data_type).name());

    //now check when a and b have non default sort orders
    a.change_linIdx_sequence(new_linIdxSequence);
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Add (Scan) 1c for ")+typeid(_data_type).name());

    c(i,j,k,l)=b(i,j,k,l)+b(k,l,i,j);
    dense_c(i,j,k,l)=dense_b(i,j,k,l)+dense_b(k,l,i,j);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Self addition check 1 for ")+typeid(_data_type).name());

    //test when a is not sorted (uses a different merge algorithm)
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    b.change_linIdx_sequence(new_linIdxSequence);
    //boost::timer::cpu_timer sort_t;
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);

    //sort_t.stop();
    //std::cout << "Sort add " << boost::timer::format(sort_t.elapsed()) << std::endl;
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Add (Sort) 1a for ")+typeid(_data_type).name());

    //test when a is not sorted (uses a different merge algorithm) and both have different sort orders
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    b.change_linIdx_sequence(new_linIdxSequence2);
    //boost::timer::cpu_timer sort_t;
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);

    //sort_t.stop();
    //std::cout << "Sort add " << boost::timer::format(sort_t.elapsed()) << std::endl;
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Add (Sort) 1b for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)+=a(j,l,i,k);
    dense_b(i,j,k,l)+=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Add (Scan) 1a for ")+typeid(_data_type).name());

    dense_b.ones();
    b=dense_b;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)+=a(j,l,i,k);
    dense_b(i,j,k,l)+=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Add (Scan) 1b for ")+typeid(_data_type).name());

    dense_b.ones();
    b=dense_b;
    a.change_linIdx_sequence(new_linIdxSequence);
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    b(i,j,k,l)+=a(j,l,i,k);
    dense_b(i,j,k,l)+=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Add (Scan) 1c for ")+typeid(_data_type).name());

    dense_b.ones();
    b=dense_b;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    b.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)+=a(j,l,i,k);
    dense_b(i,j,k,l)+=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Add (Sort) 1a for ")+typeid(_data_type).name());

    dense_b.ones();
    b=dense_b;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    b.change_linIdx_sequence(new_linIdxSequence2);
    b(i,j,k,l)+=a(j,l,i,k);
    dense_b(i,j,k,l)+=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Add (Sort) 1b for ")+typeid(_data_type).name());

    dense_b.fill(3);
    b=dense_b;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    dense_c(i,j,k,l)=dense_b(i,j,k,l)-dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Subtract (Scan) 1a for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Subtract (Scan) 1b for ")+typeid(_data_type).name());

    a.change_linIdx_sequence(new_linIdxSequence);
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Subtract (Scan) 1c for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    a.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Subtract (Sort) 1a for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    a.change_linIdx_sequence(new_linIdxSequence2);
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Subtract (Sort) 1b for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)-=a(j,l,i,k);
    dense_b(i,j,k,l)-=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Subtract (Scan) 1a for ")+typeid(_data_type).name());

    dense_b.fill(3);
    b=dense_b;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)-=a(j,l,i,k);
    dense_b(i,j,k,l)-=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Subtract (Scan) 1b for ")+typeid(_data_type).name());

    dense_b.fill(3);
    b=dense_b;
    a.change_linIdx_sequence(new_linIdxSequence);
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    b(i,j,k,l)-=a(j,l,i,k);
    dense_b(i,j,k,l)-=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Subtract (Scan) 1c for ")+typeid(_data_type).name());

    dense_b.fill(3);
    b=dense_b;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    b.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)-=a(j,l,i,k);
    dense_b(i,j,k,l)-=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Subtract (Sort) 1a for ")+typeid(_data_type).name());

    dense_b.fill(3);
    b=dense_b;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence2);
    b.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)-=a(j,l,i,k);
    dense_b(i,j,k,l)-=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Subtract (Sort) 1b for ")+typeid(_data_type).name());

    //****Now try with lots of zeros in our MIAs
    LibMIA::DenseMIA<_data_type,4> dense_a_orig(dim1,dim1,dim2,dim2);
    LibMIA::DenseMIA<_data_type,4> dense_b_orig(dim2,dim1,dim2,dim1);
    dense_a_orig.randu(0,20);
    dense_b_orig.randu(0,20);
    for(auto it=dense_a_orig.data_begin();it<dense_a_orig.data_end();++it)
        if(*it<15)
            *it=0;
    for(auto it=dense_b_orig.data_begin();it<dense_b_orig.data_end();++it)
        if(*it<15)
            *it=0;
    a=dense_a_orig;
    b=dense_b_orig;
    dense_a=dense_a_orig;
    dense_b=dense_b_orig;

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);
    dense_c(i,j,k,l)=dense_b(i,j,k,l)+dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Add (Scan) 2a for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Add (Scan) 2b for ")+typeid(_data_type).name());

    a.change_linIdx_sequence(new_linIdxSequence);
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Add (Scan) 2c for ")+typeid(_data_type).name());

    //test when a is not sorted (uses a different merge algorithm)
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    a.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Add (Sort) 2a for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    a.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Add (Sort) 2b for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)+a(j,l,i,k)+b(k,l,i,j)+a(l,j,i,k);
    dense_c(i,j,k,l)=dense_b(i,j,k,l)+dense_a(j,l,i,k)+dense_b(k,l,i,j)+dense_a(l,j,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Chained Non-destructive Add (Sort) 2a for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)+=a(j,l,i,k);
    dense_b(i,j,k,l)+=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Add (Scan) 2a for ")+typeid(_data_type).name());

    b=dense_b_orig;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)+=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Add (Scan) 2b for ")+typeid(_data_type).name());

    b=dense_b_orig;
    a.change_linIdx_sequence(new_linIdxSequence);
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    b(i,j,k,l)+=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Add (Scan) 2c for ")+typeid(_data_type).name());

    b=dense_b_orig;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    a.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)+=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Add (Sort) 2a for ")+typeid(_data_type).name());

    b=dense_b_orig;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    a.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)+=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Add (Sort) 2b for ")+typeid(_data_type).name());

    dense_b=dense_b_orig;
    b=dense_b;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    dense_c(i,j,k,l)=dense_b(i,j,k,l)-dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Subtract (Scan) 2a for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Subtract (Scan) 2b for ")+typeid(_data_type).name());

    a.change_linIdx_sequence(new_linIdxSequence);
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Subtract (Scan) 2c for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    a.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Subtract (Sort) 2a for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    a.change_linIdx_sequence(new_linIdxSequence);
    c(i,j,k,l)=b(i,j,k,l)-a(j,l,i,k);
    BOOST_CHECK_MESSAGE(c==dense_c,std::string("Non-destructive Subtract (Sort) 2b for ")+typeid(_data_type).name());

    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    a.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)-=a(j,l,i,k);
    dense_b(i,j,k,l)-=dense_a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Subtract (Scan) 2a for ")+typeid(_data_type).name());

    b=dense_b_orig;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)-=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Subtract (Scan) 2b for ")+typeid(_data_type).name());

    b=dense_b_orig;
    a.change_linIdx_sequence(new_linIdxSequence);
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    b(i,j,k,l)-=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Subtract (Scan) 2c for ")+typeid(_data_type).name());

    b=dense_b_orig;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence);
    a.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)-=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Subtract (Sort) 2a for ")+typeid(_data_type).name());

    b=dense_b_orig;
    a.reset_linIdx_sequence();
    a.sort();
    b.reset_linIdx_sequence();
    b.sort();
    b.change_linIdx_sequence(new_linIdxSequence2);
    a.change_linIdx_sequence(new_linIdxSequence);
    b(i,j,k,l)-=a(j,l,i,k);
    BOOST_CHECK_MESSAGE(b==dense_b,std::string("Destructive Subtract (Sort) 2b for ")+typeid(_data_type).name());

}

BOOST_AUTO_TEST_CASE( SparseMIAAddSubtractTests )
{

    //do_work<double>(2,3);
    do_work<double>(5,8);
    do_work<float>(5,8);
    do_work<int>(5,8);
    do_work<long long int>(5,8);

//    do_work<double>(10,8);
//    do_work<float>(10,8);
//    do_work<int>(10,8);
//    do_work<long long int>(10,8);

}


/*
 *  random.cpp
 *
 *  Created by Ania M. Kedzierska on 11/11/11.
 *  Copyright 2011 Politecnic University of Catalonia, Center for Genomic Regulation.  This is program can be redistributed, modified or else as given by the terms of the GNU General Public License.
 *
 */

#include <ctime>

#include "random.h"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/discrete_distribution.hpp>

boost::random::mt19937 random_gen((unsigned int) std::time(NULL)+getpid());  // Random number generator

boost::random::uniform_real_distribution<double> uni_real_dist;  // Uniform distribution
boost::random::uniform_int_distribution<long> uni_int_dist;      // Uniform distribution
boost::random::discrete_distribution<long, double> disc_dist;    // Discrete distribution

// Initialize the seed of the random number generator
void random_initialize(unsigned int seed) {
  random_gen.seed(seed);
}

// Initialize with a time derived seed
void random_initialize() {
  random_initialize((unsigned int) std::time(NULL)+getpid());
}

// uniformly random real in [a,b)
double uniform_real(double a, double b) {
  boost::random::uniform_real_distribution<double>::param_type param(a, b);
  uni_real_dist.param(param);
  return uni_real_dist(random_gen);
}

// random integer in[a,b]
  long uniform_int(long a, long b) {
  boost::random::uniform_int_distribution<long>::param_type param(a, b);
  uni_int_dist.param(param);
  return uni_int_dist(random_gen);
}

// random integer between 0 and p.size()-1 according to the probabilities in p.
long discrete(std::vector<double> &p) {
  boost::random::discrete_distribution<long, double>::param_type param(p);
  disc_dist.param(param);
  return disc_dist(random_gen);
}


/*
 * cwise_binary.hpp
 *
 *  Created on: Apr 11, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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.
 */

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace math {

template<typename ScalarMajor, typename ScalarMinor>
Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
scale(const Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container, ScalarMinor factor);

template<typename ScalarMajor, typename ScalarMinor>
Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>
scale(const Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>& container, ScalarMinor factor);

template<typename ScalarMajor, typename ScalarMinor>
Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
cwise_product(
		const Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_a,
		const Eigen::Matrix<ScalarMinor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_b);

template<typename ScalarMajor, typename ScalarMinor>
Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>
cwise_product(
		const Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>& container_a,
		const Eigen::Tensor<ScalarMinor, 3, Eigen::ColMajor>& container_b);

template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
cwise_add_constant(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container, Scalar constant);

template<typename Scalar>
Eigen::Tensor<Scalar, 3, Eigen::ColMajor>
cwise_add_constant(const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& container, Scalar constant);

template<typename ScalarMajor, typename ScalarMinor>
inline
Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
cwise_add(
		const Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_a,
		const Eigen::Matrix<ScalarMinor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_b){
	return (container_a.array() + container_b.array()).matrix();
}

template<typename ScalarMajor, typename ScalarMinor>
inline
Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>
cwise_add(
		const Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>& container_a,
		const Eigen::Tensor<ScalarMinor, 3, Eigen::ColMajor>& container_b){
	return container_a + container_b;
}

template<typename ScalarMajor, typename ScalarMinor>
inline
Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
cwise_subtract(
		const Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_a,
		const Eigen::Matrix<ScalarMinor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_b){
	return (container_a.array() - container_b.array()).matrix();
}

template<typename ScalarMajor, typename ScalarMinor>
inline
Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>
cwise_subtract(
		const Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>& container_a,
		const Eigen::Tensor<ScalarMinor, 3, Eigen::ColMajor>& container_b){
	return container_a - container_b;
}

}  // namespace math


//=======================================================================
// Copyright (c) 2005 Aaron Windsor
//
// 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)
//
//=======================================================================
#include <string>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <cassert>

#include <boost/graph/max_cardinality_matching.hpp>

using namespace boost;

typedef adjacency_list< vecS, vecS, undirectedS > my_graph;

int main()
{

    // Create the following graph: (it'll look better when output
    // to the terminal in a fixed width font...)

    const int n_vertices = 18;

    std::vector< std::string > ascii_graph;

    ascii_graph.push_back("           0       1---2       3       ");
    ascii_graph.push_back("            \\     /     \\     /        ");
    ascii_graph.push_back("             4---5       6---7         ");
    ascii_graph.push_back("             |   |       |   |         ");
    ascii_graph.push_back("             8---9      10---11        ");
    ascii_graph.push_back("            /     \\     /     \\        ");
    ascii_graph.push_back("     12   13      14---15      16   17 ");

    // It has a perfect matching of size 8. There are two isolated
    // vertices that we'll use later...

    my_graph g(n_vertices);

    // our vertices are stored in a vector, so we can refer to vertices
    // by integers in the range 0..15

    add_edge(1, 2, g);
    add_edge(0, 4, g);
    add_edge(1, 5, g);
    add_edge(2, 6, g);
    add_edge(3, 7, g);
    add_edge(4, 5, g);
    add_edge(6, 7, g);
    add_edge(4, 8, g);
    add_edge(5, 9, g);
    add_edge(6, 10, g);
    add_edge(7, 11, g);
    add_edge(8, 9, g);
    add_edge(10, 11, g);
    add_edge(8, 13, g);
    add_edge(9, 14, g);
    add_edge(10, 15, g);
    add_edge(11, 16, g);
    add_edge(14, 15, g);

    std::vector< graph_traits< my_graph >::vertex_descriptor > mate(n_vertices);

    // find the maximum cardinality matching. we'll use a checked version
    // of the algorithm, which takes a little longer than the unchecked
    // version, but has the advantage that it will return "false" if the
    // matching returned is not actually a maximum cardinality matching
    // in the graph.

    bool success = checked_edmonds_maximum_cardinality_matching(g, &mate[0]);
    assert(success);

    std::cout << "In the following graph:" << std::endl << std::endl;

    for (std::vector< std::string >::iterator itr = ascii_graph.begin();
         itr != ascii_graph.end(); ++itr)
        std::cout << *itr << std::endl;

    std::cout << std::endl
              << "Found a matching of size " << matching_size(g, &mate[0])
              << std::endl;

    std::cout << "The matching is:" << std::endl;

    graph_traits< my_graph >::vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        if (mate[*vi] != graph_traits< my_graph >::null_vertex()
            && *vi < mate[*vi])
            std::cout << "{" << *vi << ", " << mate[*vi] << "}" << std::endl;

    std::cout << std::endl;

    // now we'll add two edges, and the perfect matching has size 9

    ascii_graph.pop_back();
    ascii_graph.push_back("     12---13      14---15      16---17 ");

    add_edge(12, 13, g);
    add_edge(16, 17, g);

    success = checked_edmonds_maximum_cardinality_matching(g, &mate[0]);
    assert(success);

    std::cout << "In the following graph:" << std::endl << std::endl;

    for (std::vector< std::string >::iterator itr = ascii_graph.begin();
         itr != ascii_graph.end(); ++itr)
        std::cout << *itr << std::endl;

    std::cout << std::endl
              << "Found a matching of size " << matching_size(g, &mate[0])
              << std::endl;

    std::cout << "The matching is:" << std::endl;

    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
        if (mate[*vi] != graph_traits< my_graph >::null_vertex()
            && *vi < mate[*vi])
            std::cout << "{" << *vi << ", " << mate[*vi] << "}" << std::endl;

    return 0;
}


#include <algorithm>
#include <cmath>
#include <Eigen/Dense>
#include "../../common/kernel.hpp"
#include "../../common/unary.hpp"

extern "C"
{
  static void add_bias(float *c, float *y, int m, int n) {
    for (int row = 0; row < m; row++)
    {
      for (int col = 0; col < n; col++)
      {
        y[row * n + col] += c[col];
      }
    }
  }

  static void do_gemm_transa0_transb0(float *a, float *b, float *y, int m, int n, int k)
  {
    // 'const' float *a raises compile error
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > a_mat(a, m, k);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > b_mat(b, k, n);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > y_mat(y, m, n);

    y_mat.noalias() = a_mat * b_mat;
  }

  void WEBDNN_KERNEL kernel_gemm_transa0_transb0(float *a, float *b, float *y, int m, int n, int k)
  {
    do_gemm_transa0_transb0(a, b, y, m, n, k);
  }

  void WEBDNN_KERNEL kernel_gemm_transa0_transb0_c(float *a, float *b, float *c, float *y, int m, int n, int k)
  {
    do_gemm_transa0_transb0(a, b, y, m, n, k);
    add_bias(c, y, m, n);
  }

  static void do_gemm_transa0_transb1(float *a, float *b, float *y, int m, int n, int k)
  {
    // 'const' float *a raises compile error
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > a_mat(a, m, k);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> > b_mat(b, k, n);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > y_mat(y, m, n);

    y_mat.noalias() = a_mat * b_mat;
  }

  void WEBDNN_KERNEL kernel_gemm_transa0_transb1(float *a, float *b, float *y, int m, int n, int k)
  {
    do_gemm_transa0_transb1(a, b, y, m, n, k);
  }

  void WEBDNN_KERNEL kernel_gemm_transa0_transb1_c(float *a, float *b, float *c, float *y, int m, int n, int k)
  {
    do_gemm_transa0_transb1(a, b, y, m, n, k);
    add_bias(c, y, m, n);
  }

  static void do_gemm_transa1_transb0(float *a, float *b, float *y, int m, int n, int k)
  {
    // 'const' float *a raises compile error
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> > a_mat(a, m, k);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > b_mat(b, k, n);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > y_mat(y, m, n);

    y_mat.noalias() = a_mat * b_mat;
  }

  void WEBDNN_KERNEL kernel_gemm_transa1_transb0(float *a, float *b, float *y, int m, int n, int k)
  {
    do_gemm_transa1_transb0(a, b, y, m, n, k);
  }

  void WEBDNN_KERNEL kernel_gemm_transa1_transb0_c(float *a, float *b, float *c, float *y, int m, int n, int k)
  {
    do_gemm_transa1_transb0(a, b, y, m, n, k);
    add_bias(c, y, m, n);
  }

  static void do_gemm_transa1_transb1(float *a, float *b, float *y, int m, int n, int k)
  {
    // 'const' float *a raises compile error
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> > a_mat(a, m, k);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> > b_mat(b, k, n);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > y_mat(y, m, n);

    y_mat.noalias() = a_mat * b_mat;
  }

  void WEBDNN_KERNEL kernel_gemm_transa1_transb1(float *a, float *b, float *y, int m, int n, int k)
  {
    do_gemm_transa1_transb1(a, b, y, m, n, k);
  }

  void WEBDNN_KERNEL kernel_gemm_transa1_transb1_c(float *a, float *b, float *c, float *y, int m, int n, int k)
  {
    do_gemm_transa1_transb1(a, b, y, m, n, k);
    add_bias(c, y, m, n);
  }
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <Eigen/Dense>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>
#include <functional>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned,K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

// New structure to store first ring neighbors of a vertex
struct vertex_first_ring{
    size_t vertex_id;
    std::vector< std::pair<unsigned,unsigned> > faces;
    std::vector< unsigned > edges;
};

int main(){
    clock_t t1;
    t1 = clock();
    for(auto i=0; i < 1; ++i){
        // *************************** Triangulation *************************//
        auto reader = vtkSmartPointer<vtkPolyDataReader>::New();
        reader->SetFileName("T7.vtk");
        reader->Update();
        auto poly = reader->GetOutput();
        auto N = poly->GetNumberOfPoints();
        auto pts = (double*) poly->GetPoints()->GetData()->GetVoidPointer(0);
        Map3Xd points(pts,3,N);

        // Project points to unit sphere
        points.colwise().normalize();

        // Reset the center of the sphere to origin by translating
        Vector3d center = points.rowwise().mean();
        points = points.colwise() - center;

        // Rotate all points so that the point in 0th column is along z-axis
        Vector3d c = points.col(0);
        double_t cos_t = c(2);
        double_t sin_t = std::sqrt( 1 - cos_t*cos_t );
        Vector3d axis;
        axis << c(1), -c(0), 0.;
        Matrix3d rotMat, axis_cross, outer;
        axis_cross << 0. , -axis(2), axis(1),
                        axis(2), 0., -axis(0),
                        -axis(1), axis(0), 0.;

        outer.noalias() = axis*axis.transpose();

        rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1-cos_t)*outer;
        Matrix3Xd rPts(3,N);
        rPts = rotMat*points; // The points on a sphere rotated

        // Calculate the stereographic projections
        Vector3d p0;
        Map3Xd l0( &(rPts(0,1)), 3, N-1 );
        Matrix3Xd l(3,N-1), proj(3,N-1);
        p0 << 0,0,-1;
        c = rPts.col(0);
        l = (l0.colwise() - c).colwise().normalized();
        for( auto j=0; j < N-1; ++j ){
            proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
        }
        // Insert the projected points in a CGAL vertex_with_info vector
        std::vector< std::pair< Point, unsigned> > verts;
        for( auto j=0; j < N-1; ++j )
            verts.push_back(std::make_pair(Point(proj(0,j),proj(1,j)),j+1));

        Delaunay dt( verts.begin(), verts.end() );

        // Write the finite faces of the triangulation to a VTK file
        vtkNew<vtkCellArray> triangles;
        for( auto ffi = dt.finite_faces_begin(); ffi != dt.finite_faces_end(); ++ffi){
            triangles->InsertNextCell(3);
            for(auto j=2; j >= 0; --j)
                triangles->InsertCellPoint(ffi->vertex(j)->info());
        }

        // Iterate over infinite faces
        Face_circulator fc = dt.incident_faces(dt.infinite_vertex()), done3(fc);
        if (fc != 0) {
            do{
                triangles->InsertNextCell(3);
                for(auto j=2; j >= 0; --j){
                    auto vh = fc->vertex(j);
                    auto id = dt.is_infinite(vh)? 0 : vh->info();
                    triangles->InsertCellPoint(id);
                }
            }while(++fc != done3);
        }
        poly->SetPolys(triangles);

        // Write to VTK file
        vtkNew<vtkPolyDataWriter> writer;
        writer->SetFileName("CGALStereoMesh.vtk");
        writer->SetInputData(poly);
        writer->Write();

        // *************************** Our data structure *************************//
        std::vector<vertex_first_ring> first_ring;
        std::set<std::set<unsigned>> tri, edges;

        // Iterate over all vertices and collect first ring neighbors
        for(auto fvi = dt.all_vertices_begin(); fvi != dt.all_vertices_end(); ++fvi){

            vertex_first_ring vfr;
            std::vector<Delaunay::Vertex_handle> rvh;
            auto vid = dt.is_infinite(fvi)? 0 : fvi->info();
            vfr.vertex_id = vid;
            Delaunay::Edge_circulator ec = dt.incident_edges(fvi), done(ec);

            // Lambda function to get the vertex id for the edge
            auto getVertexId = [](int a, int b){
                std::set<int> index{0,1,2};
                index.erase(a);
                index.erase(b);
                return *index.begin();
            };

            if( ec != 0){
                do{

                    auto fh = ec->first;
                    auto edgeIndex = getVertexId(fh->index(fvi),ec->second);
                    auto verH = fh->vertex(edgeIndex);
                    auto edgeId = dt.is_infinite(verH)? 0 : verH->info();
                    std::set<unsigned> edge{vid,edgeId};
                    auto tryInsertEdge = edges.insert(edge);
                    if(tryInsertEdge.second){
                        vfr.edges.push_back(edgeId);
                        rvh.push_back(verH);
                    }

                }while(++ec != done);
            }

            // Check which edges form a unique face
            auto numEdges = rvh.size();
            for( auto k = 0; k < numEdges; ++k){
                auto next = (k+1) % numEdges;
                // Check if face is formed
                if(dt.is_face(fvi,rvh[k],rvh[next] )){
                    // Check if face is unique
                    std::set<unsigned> face{vid,vfr.edges[k],vfr.edges[next]};
                    auto tryInsertFace = tri.insert(face);
                    if(tryInsertFace.second)
                        vfr.faces.push_back(std::make_pair(k,next));
                }
            }

            first_ring.push_back(vfr);
        }

        //*************************** Print first ring ******************************//
        auto edgeNum = 0;
        auto faceNum = 0;
        for( const auto & vfr : first_ring){
            std::cout<<" Center Point = " << vfr.vertex_id << std::endl;
            std::cout<<"\t Faces = "<< std::endl;
            for(const auto & face : vfr.faces){
                std::cout<<"\t\t"<< face.first << " " << face.second << std::endl;
                faceNum++;
            }
            std::cout<<"\t Edges = "<< std::endl;
            for(const auto & edge : vfr.edges){
                std::cout<<"\t\t"<< edge << std::endl;
                edgeNum++;
            }
        }
        std::cout<< "Number of faces = " << faceNum << std::endl;
        std::cout<< "Number of edges = " << edgeNum << std::endl;
    }
    float diff((float)clock() - (float)t1);
    std::cout << "Time elapsed : " << diff / CLOCKS_PER_SEC
              << " seconds" << std::endl;
    return 0;
}


#include <vector>
#include "..\include\myfluidBox.h"
#include <Eigen/Dense>

using namespace std;

namespace particleSystem{
	unsigned int myFluidBox::ID_gen = 0;

	myFluidBox::myFluidBox() :ID(++ID_gen) {}//std::cout<<"building fluid box ID : "<<ID<<std::endl;	}
	myFluidBox::myFluidBox(int _numCellX, int _numCellY, int _numCellZ, double _diffusion, double _viscosity, double _deltaT, int _sviters, const Eigen::Ref<const Eigen::Vector3d>& _cellSz) :
		ID(++ID_gen), numCellX(_numCellX), numCellY(_numCellY), numCellZ(_numCellZ), diff(_diffusion), visc(_viscosity), slvIters(_sviters), startLoc(0, 0, 0), sphereBnds(), ctrSzHalfNC(0, 0, 0), //sphereExtBnds(), 				//precalculated (center - sz*halfNumCell)/sz for force query for particles
		deltaT(_deltaT), center(0, 0, 0), numCellXY(numCellX * numCellY), numCells(numCellX * numCellY * numCellZ), cellSz(_cellSz), isMesh(false), radSq(0) {

		vortEps = deltaT * .01;			//TODO allow for UI input
		initVecs();

		halfNmCellX = (numCellX / 2);
		halfNmCellY = (numCellY / 2);
		halfNmCellZ = (numCellZ / 2);

		//corresponds to x,y,z idx of "internal" array (inside single cube boundary layer
		sx1i = numCellX - 1;
		sy1i = numCellY - 1;
		sz1i = numCellZ - 1;
		//corresponds to x,y,z size of internal cube array of nodes used for sim
		sx2i = numCellX - 2;
		sy2i = numCellY - 2;
		sz2i = numCellZ - 2;
		hO2Sxd = 1.0 / (2.0* sx2i); hO2Syd = 1.0 / (2.0* sy2i); hO2Szd = 1.0 / (2.0* sz2i);
		hSxd = 0.5f* sx2i; hSyd = 0.5f* sy2i; hSzd = 0.5f* sz2i;
		memSetNumElems = sizeof(Vx0[0]) * numCells;
		//location to start rendering
		setStartLoc();
	}

	myFluidBox::~myFluidBox() {
		delete[] oldDensity;
		delete[] density;
		delete[] isOOB;
		delete[] Vx;
		delete[] Vy;
		delete[] Vz;
		delete[] Vx0;
		delete[] Vy0;
		delete[] Vz0;
	}
	void myFluidBox::set_bndCube(int b, double* x) {
		//int N = size;//for reading ease
		//int sz1 = numCellZ - 1, sz2 = numCellZ - 2,
		//	sx1 = numCellX - 1, sx2 = numCellX - 2,
		//	sy1 = sy1i, sy2 = numCellY - 2;
		if (b == 0) {//diffusion
			for (unsigned int j = 1; j < sy1i; ++j) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, j, 0)] = x[IX(i, j, 1)]; x[IX(i, j, sz1i)] = x[IX(i, j, sz2i)];}}
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, 0, k)] = x[IX(i, 1, k)]; x[IX(i, sy1i, k)] = x[IX(i, sy2i, k)];}}
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int j = 1; j < sy1i; ++j) {	x[IX(0, j, k)] = x[IX(1, j, k)]; x[IX(sx1i, j, k)] = x[IX(sx2i, j, k)];}}
		}
		else if (b == 1) {//reflecting in x
			for (unsigned int j = 1; j < sy1i; ++j) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, j, 0)] = x[IX(i, j, 1)];  x[IX(i, j, sz1i)] = x[IX(i, j, sz2i)]; } }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, 0, k)] = x[IX(i, 1, k)];  x[IX(i, sy1i, k)] = x[IX(i, sy2i, k)]; } }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int j = 1; j < sy1i; ++j) {	x[IX(0, j, k)] = -x[IX(1, j, k)]; x[IX(sx1i, j, k)] = -x[IX(sx2i, j, k)];} }
		}
		else if (b == 2) {//reflecting in y
			for (unsigned int j = 1; j < sy1i; ++j) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, j, 0)] = x[IX(i, j, 1)]; x[IX(i, j, sz1i)] = x[IX(i, j, sz2i)]; } }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, 0, k)] = -x[IX(i, 1, k)];x[IX(i, sy1i, k)] = -x[IX(i, sy2i, k)];} }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int j = 1; j < sy1i; ++j) {	x[IX(0, j, k)] = x[IX(1, j, k)]; x[IX(sx1i, j, k)] = x[IX(sx2i, j, k)]; } }
		}
		else if (b == 3) {//refelecting in z
			for (unsigned int j = 1; j < sy1i; ++j) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, j, 0)] = -x[IX(i, j, 1)]; x[IX(i, j, sz1i)] = -x[IX(i, j, sz2i)]; } }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, 0, k)] = x[IX(i, 1, k)];  x[IX(i, sy1i, k)] = x[IX(i, sy2i, k)]; } }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int j = 1; j < sy1i; ++j) {	x[IX(0, j, k)] = x[IX(1, j, k)];  x[IX(sx1i, j, k)] = x[IX(sx2i, j, k)]; } }
		}

		// edges
		for (unsigned int i = 1; i < sx1i; ++i) {
			x[IX(i, 0, 0)] = 0.5f * (x[IX(i, 1, 0)] + x[IX(i, 0, 1)]);
			x[IX(i, sy1i, 0)] = 0.5f * (x[IX(i, sy2i, 0)] + x[IX(i, sy1i, 1)]);
			x[IX(i, 0, sz1i)] = 0.5f * (x[IX(i, 1, sz1i)] + x[IX(i, 0, sz2i)]);
			x[IX(i, sy1i, sz1i)] = 0.5f * (x[IX(i, sy2i, sz1i)] + x[IX(i, sy1i, sz2i)]);
		}

		for (unsigned int j = 1; j < sy1i; ++j) {
			x[IX(0, j, 0)] = 0.5f * (x[IX(1, j, 0)] + x[IX(0, j, 1)]);
			x[IX(sx1i, j, 0)] = 0.5f * (x[IX(sx2i, j, 0)] + x[IX(sx1i, j, 1)]);
			x[IX(0, j, sz1i)] = 0.5f * (x[IX(1, j, sz1i)] + x[IX(0, j, sz2i)]);
			x[IX(sx1i, j, sz1i)] = 0.5f * (x[IX(sx2i, j, sz1i)] + x[IX(sx1i, j, sz2i)]);
		}

		for (unsigned int k = 1; k < sz1i; ++k) {
			x[IX(0, 0, k)] = 0.5f * (x[IX(0, 1, k)] + x[IX(1, 0, k)]);
			x[IX(0, sy1i, k)] = 0.5f * (x[IX(0, sy2i, k)] + x[IX(1, sy1i, k)]);
			x[IX(sx1i, 0, k)] = 0.5f * (x[IX(sx1i, 1, k)] + x[IX(sx2i, 0, k)]);
			x[IX(sx1i, sy1i, k)] = 0.5f * (x[IX(sx1i, sy2i, k)] + x[IX(sx2i, sy1i, k)]);
		}

		// corners
		double calcVal = (1 / 3.0);
		x[IX(0, 0, 0)] = calcVal*(x[IX(0, 1, 0)] + x[IX(1, 0, 0)] + x[IX(0, 0, 1)]);
		x[IX(sx1i, 0, 0)] = calcVal*(x[IX(sx2i, 0, 0)] + x[IX(sx1i, 1, 0)] + x[IX(sx1i, 0, 1)]);
		x[IX(0, sy1i, 0)] = calcVal*(x[IX(0, sy2i, 0)] + x[IX(1, sy1i, 0)] + x[IX(0, sy1i, 1)]);
		x[IX(sx1i, sy1i, 0)] = calcVal*(x[IX(sx1i, sy2i, 0)] + x[IX(sx2i, sy1i, 0)] + x[IX(sx1i, sy1i, 1)]);

		x[IX(0, 0, sz1i)] = calcVal*(x[IX(0, 1, sz1i)] + x[IX(1, 0, sz1i)] + x[IX(0, 0, sz2i)]);
		x[IX(sx1i, 0, sz1i)] = calcVal*(x[IX(sx2i, 0, sz1i)] + x[IX(sx1i, 1, sz1i)] + x[IX(sx1i, 0, sz2i)]);
		x[IX(0, sy1i, sz1i)] = calcVal*(x[IX(0, sy2i, sz1i)] + x[IX(1, sy1i, sz1i)] + x[IX(0, sy1i, sz2i)]);
		x[IX(sx1i, sy1i, sz1i)] = calcVal*(x[IX(sx1i, sy2i, sz1i)] + x[IX(sx2i, sy1i, sz1i)] + x[IX(sx1i, sy1i, sz2i)]);
	}

	//handle advection for passed arrays of velocities
	void myFluidBox::advect(int b, double* d, double* d0, double* velocX, double* velocY, double* velocZ) {
		double s0, s1, t0, t1, u0, u1;
		//double tmp1, tmp2, tmp3;
		double x, y, z;

		//double idouble = 1, jdouble = 1, kdouble = 1;
		int i0, i1, j0, j1, k0, k1;

		double dtx = deltaT * (sx2i),
			dty = deltaT * (sy2i),
			dtz = deltaT * (sz2i);

		int IXidx;

		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					IXidx = IX(i, j, k);

					x = i - dtx * velocX[IXidx];
					y = j - dty * velocY[IXidx];
					z = k - dtz * velocZ[IXidx];
					x = forceIDXBndD(x, sx1i + 0.5, .5);
					i0 = floor(x);	i1 = i0 + 1.0;  s1 = x - i0;	s0 = 1.0 - s1;
					y = forceIDXBndD(y, sy1i + 0.5, .5);
					j0 = floor(y);	j1 = j0 + 1.0;	t1 = y - j0;	t0 = 1.0 - t1;
					z = forceIDXBndD(z, sz1i + 0.5, .5);
					k0 = floor(z);	k1 = k0 + 1.0;	u1 = z - k0;	u0 = 1.0 - u1;

					d[IXidx] =
						s0 * (t0 * (u0 * d0[IX(i0, j0, k0)]
						+ u1 * d0[IX(i0, j0, k1)])
						+ (t1 * (u0 * d0[IX(i0, j1, k0)]
						+ u1 * d0[IX(i0, j1, k1)])))
						+ s1 * (t0 * (u0 * d0[IX(i1, j0, k0)]
						+ u1 * d0[IX(i1, j0, k1)])
						+ (t1 * (u0 * d0[IX(i1, j1, k0)]
						+ u1 * d0[IX(i1, j1, k1)])));
				}//for i
			}//for j
		}//for k
		set_bndCube(b, d);
	}

	void myFluidBox::project(double* velocX, double* velocY, double* velocZ, double* p, double* div) {
		//double sx2 = sx2i, sy2 = numCellY - 2, sz2 = numCellZ - 2;
		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					div[IX(i, j, k)] = -(
						  (velocX[IX(i + 1, j, k)] - velocX[IX(i - 1, j, k)])*hO2Sxd
						+ (velocY[IX(i, j + 1, k)] - velocY[IX(i, j - 1, k)])*hO2Syd
						+ (velocZ[IX(i, j, k + 1)] - velocZ[IX(i, j, k - 1)])*hO2Szd
						);
					p[IX(i, j, k)] = 0;
				}
			}
		}
		set_bndCube(0, div);
		//set_bnd(0, p);
		lin_solve(0, p, div, 1, 6);

		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					velocX[IX(i, j, k)] -= hSxd *(p[IX(i + 1, j, k)] - p[IX(i - 1, j, k)]);
					velocY[IX(i, j, k)] -= hSyd *(p[IX(i, j + 1, k)] - p[IX(i, j - 1, k)]);
					velocZ[IX(i, j, k)] -= hSzd *(p[IX(i, j, k + 1)] - p[IX(i, j, k - 1)]);
				}
			}
		}
		set_bndCube(1, velocX);
		set_bndCube(2, velocY);
		set_bndCube(3, velocZ);
	}

	//void myFluidBox::diffuse(int b, double* x, double* xOld, double viscdiff, int _numCells) {
	//	double delVisc = (deltaT*viscdiff*(numCells));
	//	lin_solve(b, x, xOld, delVisc, 1 + 6 * delVisc);
	//}

	void myFluidBox::myFluidBoxTimeStep() {
		addSource(Vx, Vx0);
		addSource(Vy, Vy0);
		addSource(Vz, Vz0);
		addSource(density, oldDensity);
		//if (isMesh) { myFluidSphereTimeStep(); return; }

		std::swap(Vx, Vx0); std::swap(Vy, Vy0); std::swap(Vz, Vz0);

		diffuse(1, Vx, Vx0, visc, numCellX);
		diffuse(2, Vy, Vy0, visc, numCellY);
		diffuse(3, Vz, Vz0, visc, numCellZ);

		project(Vx, Vy, Vz, Vx0, Vy0);

		std::swap(Vx, Vx0); std::swap(Vy, Vy0); std::swap(Vz, Vz0);

		advect(1, Vx, Vx0, Vx0, Vy0, Vz0);
		advect(2, Vy, Vy0, Vx0, Vy0, Vz0);
		advect(3, Vz, Vz0, Vx0, Vy0, Vz0);

		project(Vx, Vy, Vz, Vx0, Vy0);

		std::swap(density, oldDensity);
		diffuse(0, density, oldDensity, diff, numCellX);
		std::swap(density, oldDensity);
		advect(0, density, oldDensity, Vx0, Vy0, Vz0);
		resetOldVals();
	}//myFluidBoxTimeStep

	/////////////sphere stuff

	 //timestepping for sphere, to handle 3d bounds together instead of 1 dim at a time
	void myFluidBox::myFluidSphereTimeStep() {
		//addSource(Vx, Vx0);
		//addSource(Vy, Vy0);
		//addSource(Vz, Vz0);
		//addSource(density, oldDensity);

		std::swap(Vx, Vx0); std::swap(Vy, Vy0); std::swap(Vz, Vz0);
		double delVisc = (deltaT*visc*numCells);
		//diffusion of velocity
		lin_solveSphere(Vx, Vx0, Vy, Vy0, Vz, Vz0, delVisc, 1 + 6 * delVisc);

		projectSphere(Vx, Vy, Vz, Vx0, Vy0);

		std::swap(Vx, Vx0); std::swap(Vy, Vy0); std::swap(Vz, Vz0);
		advectSphere(Vx, Vx0, Vy, Vy0, Vz, Vz0);

		projectSphere(Vx, Vy, Vz, Vx0, Vy0);

		std::swap(density, oldDensity);
		diffSphDens(density, oldDensity, diff);
		std::swap(density, oldDensity);
		advSphDens(density, oldDensity, Vx, Vy, Vz);
		//vort confine here
		vorticityConfinement(oldDensity);
		//vorticity particle method
		//vorticityParticles();

		resetOldVals();
	}//myFluidSphereTimeStep

	//handle advection for passed arrays of velocities
	void myFluidBox::advectSphere(double* _velx, double* _velx0, double* _vely, double* _vely0, double* _velz, double* _velz0) {
		double s0, s1, t0, t1, u0, u1,x, y, z;

		int i0, i1, j0, j1, k0, k1;
		//precalced idx's
		int idx0, idx1, idx2, idx3, idx4, idx5, idx6, idx7;

		double dtx = deltaT * (sx2i),
			dty = deltaT * (sy2i),
			dtz = deltaT * (sz2i);
		int IXidx;

		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					IXidx = IX(i, j, k);
					if (isOOB[IXidx]) { continue; }

					x = i - dtx * _velx0[IXidx];
					x = forceIDXBndD(x, sx2i + 0.5, .5);
					i0 = floor(x);	i1 = i0 + 1.0;  s1 = x - i0;	s0 = 1.0 - s1;

					y = j - dty * _vely0[IXidx];
					y = forceIDXBndD(y, sy2i + 0.5, .5);
					j0 = floor(y);	j1 = j0 + 1.0;	t1 = y - j0;	t0 = 1.0 - t1;

					z = k - dtz * _velz0[IXidx];
					z = forceIDXBndD(z, sz2i + 0.5, .5);
					k0 = floor(z);	k1 = k0 + 1.0;	u1 = z - k0;	u0 = 1.0 - u1;

					idx0 = IX(i0, j0, k0);idx1 = IX(i0, j0, k1);idx2 = IX(i0, j1, k0);idx3 = IX(i0, j1, k1);
					idx4 = IX(i1, j0, k0);idx5 = IX(i1, j0, k1);idx6 = IX(i1, j1, k0);idx7 = IX(i1, j1, k1);

					_velx[IXidx] =
						s0 * (t0 * (u0 * _velx0[idx0] + u1 * _velx0[idx1]) + (t1 * (u0 * _velx0[idx2] + u1 * _velx0[idx3]))) +
						s1 * (t0 * (u0 * _velx0[idx4] + u1 * _velx0[idx5]) + (t1 * (u0 * _velx0[idx6] + u1 * _velx0[idx7])));

					_vely[IXidx] =
						s0 * (t0 * (u0 * _vely0[idx0] + u1 * _vely0[idx1]) + (t1 * (u0 * _vely0[idx2] + u1 * _vely0[idx3]))) +
						s1 * (t0 * (u0 * _vely0[idx4] + u1 * _vely0[idx5]) + (t1 * (u0 * _vely0[idx6] + u1 * _vely0[idx7])));

					_velz[IXidx] =
						s0 * (t0 * (u0 * _velz0[idx0] + u1 * _velz0[idx1]) + (t1 * (u0 * _velz0[idx2] + u1 * _velz0[idx3]))) +
						s1 * (t0 * (u0 * _velz0[idx4] + u1 * _velz0[idx5]) + (t1 * (u0 * _velz0[idx6] + u1 * _velz0[idx7])));
				}//for i
			}//for j
		}//for k
		set_bndSphere3(_velx, _vely, _velz);
	}//advectSphere

	//diffuse density - 1 d but use sphere bnds
	void myFluidBox::diffSphDens(double* x, double* x0, double viscdiff) {
		double a = (deltaT*viscdiff*(numCells)), c = (1 + 6 * a);
		int idx;
		if(a==0){
			std::memcpy(&x, &x0, sizeof x0);
			set_bndDiffSphere(x);
		} else {
			for (unsigned int itr = 0; itr < slvIters; ++itr) {
				for (unsigned int k = 1; k < sz1i; ++k) {
					for (unsigned int j = 1; j < sy1i; ++j) {
						for (unsigned int i = 1; i < sx1i; ++i) {
							idx = IX(i, j, k);
							if (isOOB[idx]) { continue; }
							x[idx] = (x0[idx] + a *
								(x[IX(i + 1, j, k)]
									+ x[IX(i - 1, j, k)]
									+ x[IX(i, j + 1, k)]
									+ x[IX(i, j - 1, k)]
									+ x[IX(i, j, k + 1)]
									+ x[IX(i, j, k - 1)])) / c;
						}//for i
					}//for j
				}//for k
				set_bndDiffSphere(x);
			}
		}//for itr
	}//diffSphDens
	//advect density through sphere, using sphere bounds
	void myFluidBox::advSphDens(double* d, double* d0, double* velocX, double* velocY, double* velocZ) {
		double s0, s1, t0, t1, u0, u1, x, y, z, dtx = deltaT * (sx2i),dty = deltaT * (sy2i),dtz = deltaT * (sz2i);
		unsigned int i0, i1, j0, j1, k0, k1, i, j, k, IXidx;

		for (k = 1; k < sz1i; ++k) {
			for (j = 1; j < sy1i; ++j) {
				for (i = 1; i < sx1i; ++i) {
					IXidx = IX(i, j, k);
					if (isOOB[IXidx]) { continue; }
					x = i - dtx * velocX[IXidx];
					y = j - dty * velocY[IXidx];
					z = k - dtz * velocZ[IXidx];
					x = forceIDXBndD(x, sx2i + 0.5, .5);
					i0 = floor(x);	i1 = i0 + 1.0;  s1 = x - i0;	s0 = 1.0 - s1;

					y = forceIDXBndD(y, sy2i + 0.5, .5);
					j0 = floor(y);	j1 = j0 + 1.0;	t1 = y - j0;	t0 = 1.0 - t1;

					z = forceIDXBndD(z, sz2i + 0.5, .5);
					k0 = floor(z);	k1 = k0 + 1.0;	u1 = z - k0;	u0 = 1.0 - u1;

					d[IXidx] =
						s0 * (t0 * (u0 * d0[IX(i0, j0, k0)]	+ u1 * d0[IX(i0, j0, k1)]) + (t1 * (u0 * d0[IX(i0, j1, k0)] + u1 * d0[IX(i0, j1, k1)]))) +
						s1 * (t0 * (u0 * d0[IX(i1, j0, k0)]	+ u1 * d0[IX(i1, j0, k1)]) + (t1 * (u0 * d0[IX(i1, j1, k0)]	+ u1 * d0[IX(i1, j1, k1)])));
				}//for i
			}//for j
		}//for k
		set_bndDiffSphere(d);
	}//advSphDens

	void myFluidBox::lin_solveSphere(double* x, double* x0, double* y, double* y0, double* z, double* z0, double a, double c) {
		if (a == 0) {
			std::memcpy(&x, &x0, sizeof x0);
			std::memcpy(&y, &y0, sizeof y0);
			std::memcpy(&z, &z0, sizeof z0);
			set_bndSphere3(x, y, z);
		}
		else {
			unsigned int idx0, idx1, idx2, idx3, idx4, idx5, idx6;
			for (unsigned int itr = 0; itr < slvIters; ++itr) {
				for (unsigned int k = 1; k < sz1i; ++k) {
					for (unsigned int j = 1; j < sy1i; ++j) {
						for (unsigned int i = 1; i < sx1i; ++i) {
							idx0 = IX(i, j, k);
							if (isOOB[idx0]) { continue; }
							idx1 = IX(i + 1, j, k);
							idx2 = IX(i - 1, j, k);
							idx3 = IX(i, j + 1, k);
							idx4 = IX(i, j - 1, k);
							idx5 = IX(i, j, k + 1);
							idx6 = IX(i, j, k - 1);
							x[idx0] = (x0[idx0] + a * (x[idx1] + x[idx2] + x[idx3] + x[idx4] + x[idx5] + x[idx6])) / c;
							y[idx0] = (y0[idx0] + a * (y[idx1] + y[idx2] + y[idx3] + y[idx4] + y[idx5] + y[idx6])) / c;
							z[idx0] = (z0[idx0] + a * (z[idx1] + z[idx2] + z[idx3] + z[idx4] + z[idx5] + z[idx6])) / c;
						}//for i
					}//for j
				}//for k
				set_bndSphere3(x, y, z);
			}//for itr
		}//if a != 0
	}//lin_solveSphere

	//vorticity confinement - add back vorticity details lost through numerical dissipation
	//vortN is unused array to hold calcs
	void myFluidBox::vorticityConfinement(double* vortN) {
		unsigned int idx, idx_ijp1k, idx_ijm1k, idx_ip1jk, idx_im1jk, idx_ijkm1, idx_ijkp1;
		//double vortEps = deltaT * .01;	//TODO change to allow for user input
		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					idx = IX(i, j, k);
					if (isOOB[idx]) { continue; }
					idx_ip1jk = IX(i + 1, j, k); idx_im1jk = IX(i - 1, j, k); idx_ijp1k = IX(i, j + 1, k); idx_ijm1k = IX(i, j - 1, k); idx_ijkp1 = IX(i, j, k + 1); idx_ijkm1 = IX(i, j, k - 1);
					//curl operation del cross u -> partial z w/respect to y is the finite diff of the z vels across the y coords
					vortVec[idx] <<
						((Vy[idx_ijkp1] - Vy[idx_ijkm1]) * hO2Syd) - ((Vz[idx_ijp1k] - Vz[idx_ijm1k]) * hO2Szd),
						((Vz[idx_ip1jk] - Vz[idx_im1jk]) * hO2Szd) - ((Vx[idx_ijkp1] - Vx[idx_ijkm1]) * hO2Sxd),
						((Vx[idx_ijp1k] - Vx[idx_ijm1k]) * hO2Sxd) - ((Vy[idx_ip1jk] - Vy[idx_im1jk]) * hO2Syd);
				}
			}
		}
		set_bndDiffVec(vortVec);
		for (idx = 0; idx < numCells; ++idx) { vortN[idx] = (isOOB[idx]) ?  0 : vortVec[idx].norm(); }

		Eigen::Vector3d eta, vf; eta.setZero();	vf.setZero();

		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					idx = IX(i, j, k);
					if (vortN[idx] < .0000001) {	continue;}
					vortVec[idx].normalize();
					eta << ((vortN[IX(i + 1, j, k)] - vortN[IX(i - 1, j, k)]) * hO2Sxd), ((vortN[IX(i, j + 1, k)] - vortN[IX(i, j - 1, k)]) * hO2Syd), ( (vortN[IX(i, j, k + 1)] - vortN[IX(i, j, k - 1)]) * hO2Szd);
					eta.normalize();
					vf = vortEps * (eta.cross(vortVec[idx]));
					//cout << "Vx " << idx << " before :  " << Vx[idx] << " vortN : " << vortN[idx] << " invDivX : " << invDivX << " eta : " << eta(0) << "," << eta(1) << "," << eta(2) << " vf : " << vf(0) << "," << vf(1) << "," << vf(2) << " vort : " << vort(0) << "," << vort(1) << "," << vort(2);
					Vx[idx] += vf(0) * sx2i;
					Vy[idx] += vf(1) * sy2i;
					Vz[idx] += vf(2) * sz2i;
					//cout << "Vx " << idx << " after :  " << Vx[idx]<<endl;
				}
			}
		}
		set_bndSphere3(Vx, Vy, Vz);
	}//vorticityConfinement

	//vorticity particle method TODO
	void myFluidBox::vorticityParticles() {
		int idx, idx_ijp1k, idx_ijm1k, idx_ip1jk, idx_im1jk, idx_ijkm1, idx_ijkp1;
		//find accelerations via finite diff
		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					idx = IX(i, j, k);
					if (isOOB[idx]) { continue; }
					idx_ip1jk = IX(i + 1, j, k);idx_im1jk = IX(i - 1, j, k);idx_ijp1k = IX(i, j + 1, k);idx_ijm1k = IX(i, j - 1, k);idx_ijkp1 = IX(i, j, k + 1);idx_ijkm1 = IX(i, j, k - 1);

					accelVecX[idx] << (Vx[idx_ip1jk] - Vx[idx_im1jk]) * hO2Sxd, (Vx[idx_ijp1k] - Vx[idx_ijm1k]) * hO2Syd, (Vx[idx_ijkp1] - Vx[idx_ijkm1]) * hO2Szd;//interpAccel(Vx, idx_ip1jk, idx_im1jk, idx_ijp1k, idx_ijm1k, idx_ijkp1, idx_ijkm1);
					accelVecY[idx] << (Vy[idx_ip1jk] - Vy[idx_im1jk]) * hO2Sxd, (Vy[idx_ijp1k] - Vy[idx_ijm1k]) * hO2Syd, (Vy[idx_ijkp1] - Vy[idx_ijkm1]) * hO2Szd;//interpAccel(Vy, idx_ip1jk, idx_im1jk, idx_ijp1k, idx_ijm1k, idx_ijkp1, idx_ijkm1);
					accelVecZ[idx] << (Vz[idx_ip1jk] - Vz[idx_im1jk]) * hO2Sxd, (Vz[idx_ijp1k] - Vz[idx_ijm1k]) * hO2Syd, (Vz[idx_ijkp1] - Vz[idx_ijkm1]) * hO2Szd;//interpAccel(Vz, idx_ip1jk, idx_im1jk, idx_ijp1k, idx_ijm1k, idx_ijkp1, idx_ijkm1);

				}
			}
		}
		set_bndDiffVec(accelVecX);
		set_bndDiffVec(accelVecY);
		set_bndDiffVec(accelVecZ);

		//vort particle code here TODO

	}//vorticityParticles

	void myFluidBox::projectSphere(double* velocX, double* velocY, double* velocZ, double* p, double* div) {
		unsigned int idx;
		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					idx = IX(i, j, k);
					p[idx] = 0;
					if (isOOB[idx]) {	div[idx] = 0;	continue; }
					//when using mac grid, change to * 1/sxi instead of 1/2sxi
					div[idx] = -(
						(velocX[IX(i + 1, j, k)] - velocX[IX(i - 1, j, k)]) * hO2Sxd
						+(velocY[IX(i, j + 1, k)] - velocY[IX(i, j - 1, k)]) * hO2Syd
						+(velocZ[IX(i, j, k + 1)] - velocZ[IX(i, j, k - 1)]) * hO2Szd
						);
				}
			}
		}
		set_bndDiffSphere(div);
		set_bndDiffSphere(p);
		for (unsigned int itr = 0; itr < slvIters; ++itr) {
			for (unsigned int k = 1; k < sz1i; ++k) {
				for (unsigned int j = 1; j < sy1i; ++j) {
					for (unsigned int i = 1; i < sx1i; ++i) {
						idx = IX(i, j, k);
						if (isOOB[idx]) { continue; }
						p[idx] = (div[idx] + (p[IX(i + 1, j, k)] + p[IX(i - 1, j, k)] + p[IX(i, j + 1, k)] + p[IX(i, j - 1, k)] + p[IX(i, j, k + 1)] + p[IX(i, j, k - 1)])) / 6.0;
					}//for i
				}//for j
			}//for k
			set_bndDiffSphere(p);
		}//for each iteration

		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					idx = IX(i, j, k);
					if (isOOB[idx]) { continue; }
					velocX[idx] -= hSxd *(p[IX(i + 1, j, k)] - p[IX(i - 1, j, k)]);
					velocY[idx] -= hSyd *(p[IX(i, j + 1, k)] - p[IX(i, j - 1, k)]);
					velocZ[idx] -= hSzd *(p[IX(i, j, k + 1)] - p[IX(i, j, k - 1)]);
				}
			}
		}
		set_bndSphere3(velocX, velocY, velocZ);
	}//projectSphere

	void myFluidBox::set_bndDiffSphere(double* x) {
		for (sphereBndMap::iterator it = sphereBnds.begin(); it != sphereBnds.end(); ++it) { x[it->first] *= it->second->mag; }//scale to amt of cube in bounds
	}
	void myFluidBox::set_bndDiffVec(eignVecTyp& egVec) {
		Eigen::Vector3d  velNorm(0, 0, 0);
		for (sphereBndMap::iterator it = sphereBnds.begin(); it != sphereBnds.end(); ++it) {
			//velNorm = (egVec[it->first].dot(it->second->norm)* it->second->mag)  *it->second->norm;			//velocity in the normal direction toward center of sphere
			velNorm = (egVec[it->first].dot(it->second->norm))  *it->second->norm;			//velocity in the normal direction toward center of sphere
			egVec[it->first] -= velNorm;													//remove velocity component in opposite direction of normal
			egVec[it->first] *= velNorm.norm();												//amplify remaining component by same amount - increase tangent velocity

		}
	}
	//address boundary layer values
	void myFluidBox::set_bndSphere3(double* x, double* y, double* z) {
		//int vIdx = b - 1, xIdx;
		Eigen::Vector3d velVec(0, 0, 0), velNorm(0, 0, 0);
		for (sphereBndMap::iterator it = sphereBnds.begin(); it != sphereBnds.end(); ++it) {
			velVec << x[it->first], y[it->first], z[it->first];
			//velNorm = (velVec.dot(it->second->norm)* it->second->mag)  *it->second->norm;			//velocity in the normal direction toward center of sphere
			velNorm = (velVec.dot(it->second->norm))  *it->second->norm;							//velocity in the normal direction toward center of sphere
																									//subtract velocity in direction of sphere wall
			x[it->first] -= velNorm(0);
			y[it->first] -= velNorm(1);
			z[it->first] -= velNorm(2);
			//scale result (tangent dir) by lost velocity magnitude - don't want to lose velocity, just redirect it
			x[it->first] *= velNorm.norm();
			y[it->first] *= velNorm.norm();
			z[it->first] *= velNorm.norm();
		}
	}//set_bndSphere

	void myFluidBox::myFluidBoxAddDensity(int x, int y, int z, double amount) { density[IX(x, y, z)] += amount; }

	void myFluidBox::resetOldVals() {
		//all elems same type
		//int numElems = sizeof(Vx0[0]) * numCells;
		memset(Vx0, 0, memSetNumElems);
		memset(Vy0, 0, memSetNumElems);
		memset(Vz0, 0, memSetNumElems);
		memset(oldDensity, 0, memSetNumElems);
	}

	void myFluidBox::myFluidBoxAddForce(const Eigen::Ref<const Eigen::Vector3d>& cellLoc, const Eigen::Ref<const Eigen::Vector3d>& amount) {
		//cout<<"force addition location in cube :("<< cellLoc (0)<<","<< cellLoc(1) <<","<< cellLoc(2) <<")"<<endl;

		int idx = IX(forceIDXBnd((int)(cellLoc(0)), sx1i,0),
					 forceIDXBnd((int)(cellLoc(1)), sy1i,0),
					 forceIDXBnd((int)(cellLoc(2)), sz1i,0));
		//static int iters = 0;
		//cout << "add force in fluidbox @idx : " << idx << " iter : " << iters++ << "\n";
		//Vx0[idx] = amount(0);
		//Vy0[idx] = amount(1);
		//Vz0[idx] = amount(2);
		Vx[idx] += amount(0);
		Vy[idx] += amount(1);
		Vz[idx] += amount(2);
	}

	//cellloc is a particle position - cell idx is going to be floor of each coord
	Eigen::Vector3d myFluidBox::getVelAtCell(const Eigen::Ref<const Eigen::Vector3d>& testLoc) {
		//ctrSzHalfNC == (ctr - (halfNumCell * cellSz))/cellSz
		//cout<<ctrSzHalfNC << "\n";
		Eigen::Vector3d tmpTestLoc = testLoc.cwiseQuotient(cellSz),
			testLocInFluid = tmpTestLoc - ctrSzHalfNC;

		//double locX = (testLoc(0) - center(0)) / cellSz(0) + halfNmCellX,
		//	locY = (testLoc(1) - center(1)) / cellSz(1) + halfNmCellY,
		//	locZ = (testLoc(2) - center(2)) / cellSz(2) + halfNmCellZ;
		int intLocX = (int)testLocInFluid(0),
			intLocY = (int)testLocInFluid(1),
			intLocZ = (int)testLocInFluid(2);

		double interpX = testLocInFluid(0) - intLocX, interpM1X = 1 - interpX,
			   interpY = testLocInFluid(1) - intLocY, interpM1Y = 1 - interpY,
			   interpZ = testLocInFluid(2) - intLocZ, interpM1Z = 1 - interpZ;
		//bound idx's
		//int tX[2], tY[2], tZ[2];
		//tX[0] = forceIDXBnd(intLocX, sx1i),
		//tY[0] = forceIDXBnd(intLocY, sy1i),
		//tZ[0] = forceIDXBnd(intLocZ, sz1i),
		//tX[1] = forceIDXBnd(intLocX + 1, sx1i),
		//tY[1] = forceIDXBnd(intLocY + 1, sy1i),
		//tZ[1] = forceIDXBnd(intLocZ + 1, sz1i);
	/*
		vector<int> idxs(8); int cnt = 0;
		for (unsigned int z = 0; z < 2; ++z) {for (unsigned int y = 0; y < 2; ++y) {for (unsigned int x = 0; x < 2; ++x) { idxs[cnt++] = IX(tX[x], tY[y], tZ[z]);}}}
	*/
		int idx000 = IX(forceIDXBnd(intLocX, sx1i, 0), forceIDXBnd(intLocY, sy1i, 0), forceIDXBnd(intLocZ, sz1i, 0)),
			idx111 = IX(forceIDXBnd(intLocX + 1, sx1i, 0), forceIDXBnd(intLocY + 1, sy1i, 0), forceIDXBnd(intLocZ + 1, sz1i, 0));

		//int idx000 = IX(forceIDXBnd(intLocX, sx2i, 1), forceIDXBnd(intLocY, sy2i, 1), forceIDXBnd(intLocZ, sz2i, 1)),
		//	idx111 = IX(forceIDXBnd(intLocX + 1, sx2i, 1), forceIDXBnd(intLocY + 1, sy2i, 1), forceIDXBnd(intLocZ + 1, sz2i, 1));

		//double valx = interpM1X * Vx[idx000] + interpX*Vx[idx111],
		//	   valy = interpM1Y * Vy[idx000] + interpY*Vy[idx111],
		//	   valz = interpM1Z * Vz[idx000] + interpZ*Vz[idx111];

		//int idx = IX(forceIDXBnd((int)((testLoc(0) - center(0)) / cellSz(0) + halfNmCellX), sx1i),
		//			 forceIDXBnd((int)((testLoc(1) - center(1)) / cellSz(1) + halfNmCellY), sy1i),
		//			 forceIDXBnd((int)((testLoc(2) - center(2)) / cellSz(2) + halfNmCellZ), sz1i));

		//return Eigen::Vector3d(Vx[idx], Vy[idx], Vz[idx]);
		return Eigen::Vector3d(interpM1X * Vx[idx000] + interpX*Vx[idx111], interpM1Y * Vy[idx000] + interpY*Vy[idx111], interpM1Z * Vz[idx000] + interpZ*Vz[idx111]);
	}//getVelAtCell

}//namespace particleSystem


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  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/simd/function/scalar/expocvt.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/detail/constant/maxexponent.hpp>

STF_CASE_TPL (" expocvt real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using uiT = bd::as_integer_t<T, unsigned>;
  using iT = bd::as_integer_t<T>;
  using bs::expocvt;
  using r_t = decltype(expocvt(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T); //(bd::as_integer_t<T, signed>));

  // specific values tests

  for( iT i=-bs::Maxexponent<T>(); i < bs::Maxexponent<T>(); ++i)
  {
    std::cout << i << std::endl;
    STF_EQUAL(expocvt(T(i)),std::ldexp(T(1), i));
  }
} // end of test for floating_


#pragma once
#include <Eigen/Core>
#include <numeric>

namespace mtao::linear_algebra {
    // M is matrix, B is initial vector, Q is the basis for the basis
    // N is the dimension of the subspace

    namespace internal {
        template <typename Derived, typename BDerived, typename VDerived, typename TDerived>
            auto lanczos(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& v1, Eigen::PlainObjectBase<VDerived>& V, Eigen::PlainObjectBase<TDerived>& T) {
                const int N = T.rows();
                using Scalar = typename Derived::Scalar;
                constexpr Scalar eps = std::numeric_limits<Scalar>::epsilon();
                using Vec = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime, 1>;

                V.setZero();
                T.setZero();

                V.col(0) = v1;
                Vec w;

                {// first iteration
                    auto v = V.col(0);
                    w = M * v;
                    const Scalar& a = T(0,0) = w.dot(v);

                    w -= a * v;
                }
                for(int j = 1; j < N; ++j) {
                    const Scalar& b = T(j,j-1) = T(j-1,j) = w.norm();
                    if(b < eps) {
                        return;
                    }
                    auto vjm = V.col(j-1);
                    auto vj = V.col(j) = w/b;
                    w = M * vj;
                    const Scalar& a = T(j,j) = w.dot(vj);
                    w -= a * vj + b * vjm;
                }
            }
    }

    template <typename Derived, typename BDerived>
        auto lanczos(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& B, int N) {
            assert(M.rows() == M.cols());

            using Scalar = typename Derived::Scalar;
            using Mat = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime,Eigen::Dynamic>;
            using DMat = Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>;

            Mat Q(M.rows(),N);
            DMat H(N,N);

            internal::lanczos(M,B,Q,H);
            return std::make_tuple(Q,H);
        }
    template <int N, typename Derived, typename BDerived>
        auto lanczos(const Eigen::MatrixBase<Derived>& M, const Eigen::MatrixBase<BDerived>& B) {
            assert(M.rows() == M.cols());

            using Scalar = typename Derived::Scalar;
            using Mat = Eigen::Matrix<Scalar,Eigen::Dynamic,N>;
            using HMat = Eigen::Matrix<Scalar,N,N>;

            Mat Q(M.rows(),N);
            HMat H(N,N);
            internal::lanczos(M,B,Q,H);
            return std::make_tuple(Q,H);
        }
    template <typename Derived>
        auto lanczos(const Eigen::MatrixBase<Derived>& M, int N) {
            using Scalar = typename Derived::Scalar;
            auto B = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime, 1>::Random(M.rows()) ;
            return lanczos(M,B,N);
        }
    template <int N, typename Derived>
        auto lanczos(const Eigen::MatrixBase<Derived>& M) {
            using Scalar = typename Derived::Scalar;
            auto B = Eigen::Matrix<Scalar,Derived::RowsAtCompileTime, 1>::Random(M.rows()) ;
            return lanczos<N>(M,B);
        }
}


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

#include <iostream>
#include <iomanip>
#include <normal_equations.h>
#undef Success
#include <Eigen/Dense>

namespace pose {

NormalEquations::NormalEquations() {
  _A.resize(36, 0);
  _B.resize(6, 0);
  _dTdR.resize(6, 0);
}

void NormalEquations::reset() {
  std::fill(_A.begin(), _A.end(), 0);
  std::fill(_B.begin(), _B.end(), 0);
  std::fill(_dTdR.begin(), _dTdR.end(), 0);
}

double NormalEquations::squaredNormDeltaT() const {
  return (_dTdR.at(0) * _dTdR.at(0) + _dTdR.at(1) * _dTdR.at(1) +
          _dTdR.at(1) * _dTdR.at(1));
}

void NormalEquations::compose(const float *CO, const float *CD) {

  _A[0] = CO[0];
  _A[1] = 0.0;
  _A[2] = CO[1];
  _A[3] = CO[2];
  _A[4] = CO[3];
  _A[5] = CO[4];
  _A[6] = 0.0;
  _A[7] = CO[0];
  _A[8] = CO[5];
  _A[9] = CO[6];
  _A[10] = -CO[2];
  _A[11] = CO[7];
  _A[12] = CO[1];
  _A[13] = CO[5];
  _A[14] = CO[8];
  _A[15] = CO[9];
  _A[16] = CO[10];
  _A[17] = 0.0;
  _A[18] = CO[2];
  _A[19] = CO[6];
  _A[20] = CO[9];
  _A[21] = CO[11];
  _A[22] = CO[12];
  _A[23] = CO[13];
  _A[24] = CO[3];
  _A[25] = -CO[2];
  _A[26] = CO[10];
  _A[27] = CO[12];
  _A[28] = CO[14];
  _A[29] = CO[15];
  _A[30] = CO[4];
  _A[31] = CO[7];
  _A[32] = 0.0;
  _A[33] = CO[13];
  _A[34] = CO[15];
  _A[35] = CO[16];

  _A[0] += CD[0];
  _A[1] += CD[1];
  _A[2] += CD[2];
  _A[3] += CD[3];
  _A[4] += CD[4];
  _A[5] += CD[5];
  _A[6] += CD[1];
  _A[7] += CD[6];
  _A[8] += CD[7];
  _A[9] += CD[8];
  _A[10] += CD[9];
  _A[11] += CD[10];
  _A[12] += CD[2];
  _A[13] += CD[7];
  _A[14] += CD[11];
  _A[15] += CD[12];
  _A[16] += CD[13];
  _A[17] += CD[14];
  _A[18] += CD[3];
  _A[19] += CD[8];
  _A[20] += CD[12];
  _A[21] += CD[15];
  _A[22] += CD[16];
  _A[23] += CD[17];
  _A[24] += CD[4];
  _A[25] += CD[9];
  _A[26] += CD[13];
  _A[27] += CD[16];
  _A[28] += CD[18];
  _A[29] += CD[19];
  _A[30] += CD[5];
  _A[31] += CD[10];
  _A[32] += CD[14];
  _A[33] += CD[17];
  _A[34] += CD[19];
  _A[35] += CD[20];

  for (int i = 0; i < 6; i++)
    _B[i] = CO[17 + i] + CD[21 + i];
}

void NormalEquations::solve(float *dTdR) {

  Eigen::Map<Eigen::Matrix<double, 6, 6> > A(_A.data());
  Eigen::Map<Eigen::Matrix<double, 6, 1> > B(_B.data());
  Eigen::Map<Eigen::Matrix<double, 6, 1> > double_dTdR(_dTdR.data());

  double_dTdR = A.ldlt().solve(B);

  Eigen::Map<Eigen::Matrix<float, 6, 1> > float_dTdR(dTdR);
  float_dTdR = double_dTdR.cast<float>();
}

void NormalEquations::preCondition() {
  for (auto &it : _A)
    it *= 1.0e-7;
}

void NormalEquations::show() const {
  for (int row = 0; row < 6; row++) {
    std::cout << std::scientific;
    std::cout.precision(3);
    for (int col = 0; col < 6; col++)
      std::cout << std::setw(10) << _A.at(row * 6 + col) << " ";
    std::cout << std::fixed;
    std::cout.precision(6);
    std::cout << ((row == 2) ? "X " : "  ") << std::setw(9) << _dTdR.at(row);
    std::cout << std::scientific;
    std::cout.precision(3);
    std::cout << ((row == 2) ? " = " : "   ") << std::setw(10) << _B.at(row)
              << std::endl;
  }
}
}


//
// Created by Amir Masoud Abdol on 2019-04-25.
//

#include <algorithm>
#include <cmath>

#include <spdlog/spdlog.h>
#include <fmt/core.h>

#include "sam.h"

#include "MetaAnalysis.h"
#include "Journal.h"
#include "TestStrategy.h"
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/non_central_t.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/binomial.hpp>

#include <mlpack/methods/linear_regression/linear_regression.hpp>

using namespace std;
using namespace sam;

MetaAnalysis::~MetaAnalysis(){

}

std::unique_ptr<MetaAnalysis> MetaAnalysis::build(std::string name) {

  spdlog::debug("Building a Meta Analysis Method");

  if (name == "FixedEffectEstimator") {
    return std::make_unique<FixedEffectEstimator>();
  }else if (name == "RandomEffectEstimator") {
    return std::make_unique<RandomEffectEstimator>();
  }else if (name == "EggersTestEstimator") {
    return std::make_unique<EggersTestEstimator>();
  }else if (name == "TestOfObsOverExptSig") {
    return std::make_unique<TestOfObsOverExptSig>();
  }else if (name == "TrimAndFill") {
    return std::make_unique<TrimAndFill>();
  }else if (name == "RankCorrelation") {
    return std::make_unique<RankCorrelation>();
  }else{
    spdlog::critical("Invalid Meta Analysis Strategy.");
    exit(1);
  }
}

std::unique_ptr<MetaAnalysis> MetaAnalysis::build(const json &config) {
  if (config["name"] == "FixedEffectEstimator") {
    return std::make_unique<FixedEffectEstimator>();
  }else if (config["name"] == "RandomEffectEstimator") {

    auto p = config.get<RandomEffectEstimator::Parameters>();
    return std::make_unique<RandomEffectEstimator>(p);

  }else if (config["name"] == "EggersTestEstimator") {
    auto p = config.get<EggersTestEstimator::Parameters>();
    return std::make_unique<EggersTestEstimator>(p);

  }else if (config["name"] == "TestOfObsOverExptSig") {
    auto p = config.get<TestOfObsOverExptSig::Parameters>();
    return std::make_unique<TestOfObsOverExptSig>(p);

  }else if (config["name"] == "TrimAndFill") {
    auto p = config.get<TrimAndFill::Parameters>();
    return std::make_unique<TrimAndFill>(p);

  }else if (config["name"] == "RankCorrelation") {
    auto p = config.get<RankCorrelation::Parameters>();
    return std::make_unique<RankCorrelation>(p);

  }else{
    spdlog::critical("Invalid Meta Analysis Strategy.");
    exit(1);
  }
}

std::vector<std::string> MetaAnalysis::Columns(std::string name) {
  if (name == "FixedEffectEstimator") {
    return FixedEffectEstimator::ResultType::Columns();
  }else if (name == "RandomEffectEstimator") {
    return RandomEffectEstimator::ResultType::Columns();
  }else if (name == "EggersTestEstimator") {
    return EggersTestEstimator::ResultType::Columns();
  }else if (name == "TestOfObsOverExptSig") {
    return TestOfObsOverExptSig::ResultType::Columns();
  }else if (name == "TrimAndFill") {
    return TrimAndFill::ResultType::Columns();
  }else if (name == "RankCorrelation") {
    return RankCorrelation::ResultType::Columns();
  }else{
    spdlog::critical("Invalid Meta Analysis Strategy.");
    exit(1);
  }
}

void FixedEffectEstimator::estimate(Journal *journal) {
  spdlog::debug("Computing Fixed Effect Estimate...");

  journal->storeMetaAnalysisResult(FixedEffect(journal->yi, journal->vi));
}

void RandomEffectEstimator::estimate(Journal *journal) {

  spdlog::debug("Computing Random Effect Estimate...");

  float tau2 {0};

  if (params.estimator.find("DL") != std::string::npos){
    tau2 = RandomEffectEstimator::DL(journal->yi, journal->vi, journal->wi);
  }else if (params.estimator.find("PM") != std::string::npos){
    spdlog::critical("Not implemented yet!");
    exit(1);
//    tau2 = RandomEffectEstimator::PM(journal->yi, journal->vi, tau2);
  }

  journal->storeMetaAnalysisResult(RandomEffect(journal->yi, journal->vi, tau2));
}

void EggersTestEstimator::estimate(Journal *journal) {

  spdlog::debug("Computing Eggers Estimate...");

  journal->storeMetaAnalysisResult(EggersTest(journal->yi, journal->vi, params.alpha));
}

RandomEffectEstimator::ResultType
RandomEffectEstimator::RandomEffect(const arma::Row<float> &yi, const arma::Row<float> &vi, float tau2) {

  using boost::math::normal;
  using boost::math::chi_squared;

  normal norm(0, 1);

  // Weight per study
  arma::Row<float> wi = 1. / (vi + tau2);
  // Meta-analytic estimate
  auto est = arma::accu(yi % wi) / arma::accu(wi);
  // Standard error of meta-analytic estimate
  auto se = sqrt(1. / arma::accu(wi));
  // Lower bound CI meta-analytical estimate
  auto ci_lb = est - quantile(norm, 0.975) * se;
  // Upper bound CI meta-analytical estimate
  auto ci_ub = est + quantile(norm, 0.975) * se;
  // Z-value for test of no effect
  auto zval = est/se;
  // Compute one-sided p-value
  auto pval_one = cdf(complement(norm, zval));
  // Compute two-tailed p-value
  auto pval = pval_one > 0.5 ? (1. - pval_one) * 2 : pval_one * 2;

  arma::Row<float> wi_fe = 1. / vi;
  auto est_fe = arma::accu(wi_fe % yi)/ arma::accu(wi_fe);

  // Q-statistic
  auto q_stat = arma::accu(wi_fe % arma::pow(yi - est_fe, 2));

  chi_squared chisq(yi.n_elem - 1);
  // p-value of Q-statistic
  auto q_pval = cdf(complement(chisq, q_stat));

  return ResultType{est, static_cast<float>(se), static_cast<float>(ci_lb), static_cast<float>(ci_ub), static_cast<float>(zval), static_cast<float>(pval), q_stat, static_cast<float>(q_pval), tau2};
}

// General method-of-moments estimate (Eq. 6 in DerSimonian and Kacker, 2007)
float RandomEffectEstimator::DL(const arma::Row<float> &yi, const arma::Row<float> &vi, const arma::Row<float> &wi) {

  spdlog::trace("→ Estimating the tau2 using DL ...");

  auto q = arma::accu(wi % arma::pow(yi - (arma::accu(wi % yi)/arma::accu(wi)), 2));
  // spdlog::trace("Q: {}", q);

  auto tau2 = (q - (yi.n_elem - 1)) / (arma::accu(wi) - (arma::accu(arma::pow(wi, 2))/arma::accu(wi)));
  // spdlog::trace("Tau2: {}", tau2);

  tau2 = tau2 < 0 ? 0 : tau2;

  return tau2;
}

// Function for estimating tau2 with Paule-Mandel estimator
float RandomEffectEstimator::PM(const arma::Row<float> &yi, const arma::Row<float> &vi, const float tau2) {
  // Degrees of freedom of Q-statistic (df is also expected value because chi square distributed)
  auto df = yi.n_elem - 1;
  // Weights in meta-analysis
  arma::Row<float> wi = 1. / (vi + tau2);
  // Meta-analytic effect size
  auto theta = arma::accu(yi % wi)/arma::accu(wi);
  // Q-statistic
  auto Q = arma::accu(wi % arma::pow(yi - theta, 2));

  // Stop iterating if computed Q-statistic equals degrees of freedom

  return (Q - df);
}

EggersTestEstimator::ResultType
EggersTestEstimator::EggersTest(const arma::Row<float> &yi, const arma::Row<float> &vi, float alpha) {

  using namespace mlpack;
  using namespace mlpack::regression;

  using boost::math::students_t;

  arma::Row<double> Yi = arma::conv_to<arma::Row<double>>::from(yi);
  arma::Row<double> Vi = arma::conv_to<arma::Row<double>>::from(vi);

  auto n = Yi.n_elem;
  auto p = 2;
  float df = n - p;

  arma::Row<double> Wi = 1./Vi;
  arma::Row<double> wts = arma::sqrt(Wi);
  arma::Row<double> si = arma::sqrt(Vi);

  arma::Row<double> predictions(n);

  arma::Mat<double> X;
  X.ones(2, n);
  X.row(1) = si;

  LinearRegression lg(X, Yi, Wi);
  lg.Train(X, Yi, Wi, false);
  lg.Predict(X, predictions);

  arma::Row<double> errors = Yi - predictions;

  auto slope = lg.Parameters().at(1);

  arma::Mat<double> W = arma::diagmat(Wi);

  double res_var_2 = sqrt(arma::accu(Wi % arma::pow(errors, 2)) / (n - 2));
  arma::Mat<double> S_2 = arma::diagmat(arma::pow(res_var_2 / sqrt(Wi), 2));

  arma::Mat<double> Z = X.t();
  arma::Mat<double> var_betas = arma::sqrt(arma::inv(Z.t() * W * Z) * (Z.t() * W * S_2 * W.t() * Z) * arma::inv(Z.t() * W * Z));

  double slope_se = var_betas.diag().at(1);

  double slope_stat = slope / slope_se;

  auto res = TTest::compute_pvalue(slope_stat, n - 2, 0.1, TestStrategy::TestAlternative::TwoSided);

  return ResultType{static_cast<float>(slope), static_cast<float>(slope_se), static_cast<float>(slope_stat), res.first, res.second, df};
}

sam::TestOfObsOverExptSig::ResultType
TestOfObsOverExptSig::TES(const arma::Row<float> &sigs, const arma::Row<float> &ni, float beta, float alpha) {

  using boost::math::students_t;
  using boost::math::non_central_t;
  using boost::math::chi_squared;

  float k = sigs.n_elem;

  float O = arma::accu(sigs);

  arma::Row<float> tcvs(k);
  tcvs.imbue([&, i = 0]() mutable {
    students_t tdist(ni[i] - 1); i++;
    return quantile(tdist, 0.95);
  });

  // non-central t-statistics
  arma::Row<float> powers(k);
  powers.imbue([&, i = 0]() mutable {
    non_central_t nct(ni[i] - 1, beta * sqrt(ni[i]));
    return cdf(complement(nct, tcvs[i++]));
  });

  float E = arma::accu(powers);

  /// @note If E is absolute zero, I'm adding some noise that I don't have to deal with the explosion
  if (E < 0.0000001)
    E = 1e-10;

  /// A is most likely different from what R spit out, due to brutal rounding that's happening in R.
  float A {100000};
  float pval {0.0};
  if (k != E) {
    A = pow(O - E, 2.) / E + pow(O - E, 2.) / (k - E);

    if (!isnan(A) and !isinf(A)) {
      chi_squared chisq(1);
      pval = cdf(complement(chisq, A));
    }
  }

  return TestOfObsOverExptSig::ResultType{E, A, pval, pval < alpha};
}

void TestOfObsOverExptSig::estimate(Journal *journal) {

  spdlog::debug("Computing Test Of Obs Over Expt Significance...");

  float beta = FixedEffectEstimator::FixedEffect(journal->yi, journal->vi).est;

  arma::Row<float> sigs(journal->yi.n_elem);
  sigs.imbue([&, i = 0]() mutable {
    return journal->publications_list[i++].dv_.sig_;
  });

  arma::Row<float> ni(journal->yi.n_elem);
  ni.imbue([&, i = 0]() mutable {
    return journal->publications_list[i++].dv_.nobs_;
  });

  journal->storeMetaAnalysisResult(TestOfObsOverExptSig::TES(sigs, ni, beta, 0.05));
}

void TrimAndFill::estimate(Journal *journal) {

  spdlog::debug("Computing Trim And Fill...");

  arma::Row<float> ni(journal->yi.n_elem);
  ni.imbue([&, i = 0]() mutable {
    return journal->publications_list[i++].dv_.nobs_;
  });

  journal->storeMetaAnalysisResult(TrimAndFill::TF(journal->yi, journal->vi, ni, params));
}

TrimAndFill::ResultType TrimAndFill::TF(arma::Row<float> yi, arma::Row<float> vi, arma::Row<float> ni, const Parameters &params) {

  int k = yi.n_elem;
  arma::Row<float> wi = 1. / vi;

  std::string side = params.side;

  /// Determining the side
  float beta = FixedEffectEstimator::FixedEffect(yi, vi).est;

  if (params.side.find("auto") != std::string::npos) {
    if (beta < 0) {
      side = "right";
    } else {
      side = "left";
    }
  }

  /// flip data if examining right side
  if (side.find("right") != std::string::npos){
    yi = -1. * yi;
  }

  /// sort data by increasing yi
  arma::uvec ix = arma::sort_index(yi);
  arma::Row<float> yi_s = yi.elem(ix).as_row();
  arma::Row<float> vi_s = vi.elem(ix).as_row();
  arma::Row<float> wi_s = wi.elem(ix).as_row();
  arma::Row<float> ni_s = wi.elem(ix).as_row();

  int iter{0};
  int maxiter{100};

  float k0_sav{-1};
  float k0{0}; // estimated number of missing studies;
  float se_k0{0};
  float Sr{0};
  float varSr{0};
  float k0_pval{0};

  arma::Row<float> yi_c;
  arma::Row<float> yi_c_r;
  arma::Row<float> yi_c_r_s;

  while (abs(k0 - k0_sav) > 0) {

    k0_sav = k0; // save current value of k0;

    iter++;

    if (iter > maxiter)
      break;

    //  truncated data
    arma::uvec elems = arma::regspace<arma::uvec>(0, 1, k - k0 - 1);
    arma::Row<float> yi_t = yi_s.elem(elems).as_row();
    arma::Row<float> vi_t = vi_s.elem(elems).as_row();
    arma::Row<float> wi_t = wi_s.elem(elems).as_row();
    arma::Row<float> ni_t = wi_s.elem(elems).as_row();

    //  intercept estimate based on truncated data
    beta = FixedEffectEstimator::FixedEffect(yi_t, vi_t).est;

    yi_c     = yi_s - beta;                             ///  centered values;
    yi_c_r   = rankdata(abs(yi_c), "average").as_row(); /// @todo ties_method="first"); //  ranked absolute centered values;
    yi_c_r_s = arma::sign(yi_c) % yi_c_r;               ///  signed ranked centered values;

    //  estimate the number of missing studies with the R0 estimator

    if (params.estimator.find("R0") != std::string::npos) {
      arma::uvec inx = arma::find(yi_c_r_s < 0);
      k0 = (k - arma::max(-1. * yi_c_r_s.elem(inx))) - 1;
      se_k0 = sqrt(2 * std::max(static_cast<float>(0.), k0) + 2);
    }

    ///  estimate the number of missing studies with the L0 estimator
    if (params.estimator.find("L0") != std::string::npos) {
      arma::uvec inx = arma::find(yi_c_r_s > 0);
      Sr = arma::accu(yi_c_r_s.elem(inx));
      k0 = (4.*Sr - k*(k+1.)) / (2.*k - 1.);
      varSr = 1./24 * (k*(k+1.)*(2.*k+1.) + 10.*pow(k0,3) + 27.*pow(k0,2) + 17.*k0 - 18.*k*pow(k0,2) - 18.*k*k0 + 6.*pow(k,2)*k0);
      se_k0 = 4.*sqrt(varSr) / (2*k - 1);
    }

    ///  estimate the number of missing studies with the Q0 estimator
    if (params.estimator.find("Q0") != std::string::npos) {
      arma::uvec inx = arma::find(yi_c_r_s > 0);
      Sr = arma::accu(yi_c_r_s.elem(inx));
      k0 = k - 1./2 - sqrt(2*pow(k,2) - 4.*Sr + 1./4);
      varSr = 1./24 * (k*(k+1.)*(2*k+1.) + 10.*pow(k0,3) + 27.*pow(k0,2) + 17.*k0 - 18.*k*pow(k0,2) - 18.*k*k0 + 6.*pow(k,2)*k0);
      se_k0 = 2. * sqrt(varSr) / sqrt(pow(k-0.5,2) - k0*(2.*k - k0 - 1.));
    }

    ///  round k0 and make sure that k0 is non-negative
    k0 = std::max(static_cast<float>(0.), std::round(k0));
    se_k0 = std::max(static_cast<float>(0.), se_k0);

  }

  /// ------------------ Filling and estimating ----------------

  auto res = FixedEffectEstimator::FixedEffect(yi, vi);
  float imputed_est = res.est;
  float imputed_pval = res.pval;

  /// if estimated number of missing studies is > 0
  if (k0 > 0) {

    /// flip data back if side is right
    if (side.find("right") != std::string::npos) {
      yi_c = -1 * (yi_c - beta);
      yi = -1 * yi;
    } else {
      yi_c = yi_c - beta;
    }

    /// create filled-in data set
    arma::Row<float> yi_f = yi_c;
    arma::Row<float> yi_fill = yi;
    yi_fill.insert_cols(yi_f.n_elem, -1. * yi_c.elem(arma::regspace<arma::uvec>(k - k0, 1, k - 1)).as_row());

    /// apply limits if specified
    /// @todo: to be implemented
    //    if (!missing(ilim)) {
    //      ilim = sort(ilim)
    //      if (length(ilim) != 2L)
    //        stop(mstyle$stop("Argument 'ilim' must be of length 2_"))
    //        yi_fill[yi_fill < ilim[1]] = ilim[1]
    //        yi_fill[yi_fill > ilim[2]] = ilim[2]
    //        }

    arma::Row<float> vi_fill = vi;
    vi_fill.insert_cols(vi.n_elem, vi.elem(arma::regspace<arma::uvec>(k - k0, 1, k - 1)).as_row());
    arma::Row<float> wi_fill = wi;
    wi_fill.insert_cols(wi.n_elem, wi.elem(arma::regspace<arma::uvec>(k - k0, 1, k - 1)).as_row());
    arma::Row<float> ni_fill = ni;
    ni_fill.insert_cols(ni.n_elem, ni.elem(arma::regspace<arma::uvec>(k - k0, 1, k - 1)).as_row());

    /// fit model with imputed data
    auto res = FixedEffectEstimator::FixedEffect(yi_fill, vi_fill);
    imputed_est = res.est;
    imputed_pval = res.pval;

  }

  /// @todo need to be integrated!
  std::optional<float> p_k0;

  /// Adjustment for p_k0
  if (params.estimator.find("R0") != std::string::npos) {
    arma::Row<float> m {arma::regspace<arma::Row<float>>(-1, 1, (k0-1))};
    arma::Row<float> bin_coefs(m.n_elem);
    /// @todo This imbue can be improved
    bin_coefs.imbue([&, i = 0]() mutable {
      auto x = boost::math::binomial_coefficient<float>(0+m.at(i)+1, m.at(i)+1);
      i++;
      return x;
    });
    arma::Row<float> tmp(m.n_elem);
    tmp.imbue([&, i = 0]() mutable {
      return pow(0.5, static_cast<int>(0 + m.at(i++) + 2));
    });
    p_k0 = 1 - arma::accu(bin_coefs % tmp);
  } //else
    // p_k0 = NA

  /// @todo Still need to report the p_k0
  return ResultType{.k0 = k0, .se_k0 = se_k0, .k_all = k + k0, .side = side, .imputed_est = imputed_est, .imputed_pval = imputed_pval};

}

namespace sam {

/*-------------------------------------------------------------------------
 * This function calculates the Kendall correlation tau_b.
 *
 * from: https://afni.nimh.nih.gov/pub/dist/src/ktaub.c
 */
float kendallcor(const arma::Row<float> &x, const arma::Row<float> &y) {

  spdlog::debug(" → Computing Kendall Correlation...");

  int len = x.n_elem;

  int m1 = 0, m2 = 0, s = 0, nPair , i,j ;
  float cor ;

  for(i = 0; i < len; i++) {
    for(j = i + 1; j < len; j++) {
      if(y[i] > y[j]) {
        if (x[i] > x[j]) {
          s++;
        } else if(x[i] < x[j]) {
          s--;
        } else {
          m1++;
        }
      } else if(y[i] < y[j]) {
        if (x[i] > x[j]) {
          s--;
        } else if(x[i] < x[j]) {
          s++;
        } else {
          m1++;
        }
      } else {
        m2++;

        if(x[i] == x[j]) {
          m1++;
        }
      }
    }
  }

  nPair = len * (len - 1) / 2;

  if( m1 < nPair && m2 < nPair )
    cor = s / ( sqrtf((float)(nPair-m1)) * sqrtf((float)(nPair-m2)) );
  else
    cor = 0.0f;

  return cor;
}

float ckendall(int k, int n, arma::Mat<float> &w) {
  int i, u;
  float s;

  u =  (n * (n - 1) / 2);
  if ((k < 0) || (k > u))
    return(0);

  if (w.at(n, k) < 0) {
    if (n == 1)
      w.at(n, k) = (k == 0);
    else {
      s = 0;
      for (i = 0; i < n; i++)
        s += ckendall(k - i, n - 1, w);
      w.at(n, k) = s;
    }
  }
  return(w.at(n, k));
}

float pkendall(int len, int n) {

  spdlog::debug(" → Computing Kendall Probability...");

  int i, j;
  float p, q;

  p = 0;
  q = len;

  size_t u =  (n * (n - 1) / 2);
  arma::Mat<float> w(n, u); w.fill(-1);

    if (q < 0)
      p = 0;
    else if (q > (n * (n - 1) / 2))
      p = 1;
    else {
      p = 0;
      for (j = 0; j <= q; j++)
        p += ckendall(j, n, w);
      p = p / boost::math::tgamma(n + 1);
    }

  spdlog::trace(" → → p = {:f}\n", p);
  return p;
}

std::pair<float, float> kendall_cor_test(const arma::Row<float> &x, const arma::Row<float> &y, const TestStrategy::TestAlternative alternative) {

  spdlog::debug(" → Running Kendall Correlation Test...");

  auto n = x.n_elem;
  auto r = kendallcor(x, y);

  auto q = round((r + 1.) * n * (n - 1.) / 4.);

  arma::Row<float> x_uqniues = arma::unique(x);
  size_t x_n_uqniues = x_uqniues.n_elem;
  arma::Row<float> y_uqniues = arma::unique(y);
  size_t y_n_uqniues = y_uqniues.n_elem;

  bool ties = (min(x_n_uqniues, y_n_uqniues) < n);

  float p{0};
  float statistic;

  if (!ties) {

    statistic = q;
    spdlog::trace(" → → Statistic: {}", q);

    switch (alternative) {
      case TestStrategy::TestAlternative::TwoSided: {
        if(q > n * (n - 1) / 4){
          p = 1 - pkendall(q - 1, n);
        }else{
          p = pkendall(q, n);
        }
        p = std::min(2. * p, 1.);
      } break;
      case TestStrategy::TestAlternative::Greater: {
        p = 1. - pkendall(q - 1, n);
      } break;
      case TestStrategy::TestAlternative::Less: {
        p = pkendall(q, n);
      } break;
    }

  }else{
    /// @note I'm not 100% sure if this is a good replacement for `table` but it seems to
    /// be working!
    spdlog::trace("Found ties...");
    spdlog::warn("Cannot compute exact p-value with ties!");

    /// xties <- table(x[duplicated(x)]) + 1;
    arma::urowvec xties;
    if (x_n_uqniues > 0) {
      xties = arma::hist(x, arma::sort(arma::unique(x))) - 1;
      xties = arma::nonzeros(xties).as_row() + 1;
    }else
      xties = arma::urowvec({0});

    /// yties <- table(y[duplicated(y)]) + 1;
    arma::urowvec yties;
    if (y_n_uqniues) {
      yties = arma::hist(y, arma::sort(arma::unique(y))) - 1;
      yties = arma::nonzeros(yties).as_row() + 1;
    }else
      yties = arma::urowvec({0});

    float T0 = n * (n - 1)/2;

    float T1 = arma::accu(xties % (xties - 1))/2;

    float T2 = arma::accu(yties % (yties - 1))/2;

    float S = r * sqrt((T0 - T1) * (T0 - T2));

    float v0 = n * (n - 1) * (2 * n + 5);

    float vt = arma::accu(xties % (xties - 1) % (2 * xties + 5));

    float vu = arma::accu(yties % (yties - 1) % (2 * yties + 5));

    float v1 = arma::accu((xties % (xties - 1))) * arma::accu(yties % (yties - 1));

    float v2 = arma::accu((xties % (xties - 1)) % (xties - 2)) * arma::accu(yties % (yties - 1) % (yties - 2));

    float var_S = (v0 - vt - vu) / 18. + v1 / (2. * n * (n - 1.)) + v2 / (9. * n * (n - 1.) * (n - 2.));

    statistic = S / sqrt(var_S);

    using boost::math::normal;
    normal norm;

    /// @todo check if these are what I want
    switch (alternative) {
      case TestStrategy::TestAlternative::TwoSided: {
        p = 2 * min(cdf(norm, statistic), cdf(complement(norm, statistic)));
      } break;
      case TestStrategy::TestAlternative::Greater: {
        p = cdf(complement(norm, statistic));
      } break;
      case TestStrategy::TestAlternative::Less: {
        p = cdf(norm, statistic);
      } break;
    }

  }

  return std::make_pair(r, p);
}

}

RankCorrelation::ResultType RankCorrelation::RankCor(arma::Row<float> yi, arma::Row<float> vi, const Parameters &params) {

  auto res  = FixedEffectEstimator::FixedEffect(yi, vi);
  auto beta = res.est;
  auto vb = pow(res.se, 2);

  arma::Row<float> vi_star = vi - vb;
  arma::Row<float> yi_star = (yi - beta) / arma::sqrt(vi_star);

//  vi_star.replace(arma::datum::nan, 0.);
//  yi_star.replace(arma::datum::nan, 0.);
  auto ken_res = kendall_cor_test(yi_star, vi, params.alternative);

  auto tau  = ken_res.first;
  auto pval = ken_res.second;
  spdlog::trace("Kendal Correlation Test: tau: {}, p: {}", tau, pval);

  return {.est = tau, .pval = pval, .sig = pval < params.alpha};

}

void RankCorrelation::estimate(sam::Journal *journal) {
  journal->storeMetaAnalysisResult(RankCorrelation::RankCor(journal->yi, journal->vi, params));
}