//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

//==================================================================================================
//== Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of cyl_bessel_kn"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
 using i_t = eve::as_integer_t<v_t>;
 using I_t = eve::wide<i_t, eve::cardinal_t<T>>;
  TTS_EXPR_IS( eve::cyl_bessel_kn(T(), T())  ,  T);
  TTS_EXPR_IS( eve::cyl_bessel_kn(v_t(),v_t()), v_t);
  TTS_EXPR_IS( eve::cyl_bessel_kn(i_t(),T()),   T);
  TTS_EXPR_IS( eve::cyl_bessel_kn(I_t(),T()),   T);
  TTS_EXPR_IS( eve::cyl_bessel_kn(i_t(),v_t()), v_t);
  TTS_EXPR_IS( eve::cyl_bessel_kn(I_t(),v_t()), T);
};

//==================================================================================================
//== integral orders
//==================================================================================================
EVE_TEST( "Check behavior of cyl_bessel_kn on wide with integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::ramp(0), eve::test::randoms(0.0, 10.0))
        )
  <typename T>(T n , T a0 )
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_kn =  [](auto n, auto x) { return eve::cyl_bessel_kn(n, x); };
  auto std__cyl_bessel_kn =  [](auto n, auto x)->v_t { return boost::math::cyl_bessel_k(n, x); };

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(0, eve::minf(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(2, eve::inf(eve::as<v_t>())), eve::inf(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(3, eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
  }
  //scalar large x
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(3, v_t(1500)),  eve::zero(eve::as<v_t>()), 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(2, v_t(50)), std__cyl_bessel_kn(2, v_t(50)), 5.0);
  //scalar forward
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(0, v_t(10)), std__cyl_bessel_kn(0, v_t(10))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(1, v_t(5)),  std__cyl_bessel_kn(1, v_t(5))   , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(2, v_t(10)), std__cyl_bessel_kn(2, v_t(10))  , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(3, v_t(5)),  std__cyl_bessel_kn(3, v_t(5))   , 35.0);
  //scalar small
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(0, v_t(0.1)), std__cyl_bessel_kn(0, v_t(0.1))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(1, v_t(0.2)),  std__cyl_bessel_kn(1, v_t(0.2))   , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(2, v_t(0.1)), std__cyl_bessel_kn(2, v_t(0.1))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(3, v_t(0.2)),  std__cyl_bessel_kn(3, v_t(0.2))   , 5.0);
  //scalar medium

  TTS_ULP_EQUAL(eve__cyl_bessel_kn(10, v_t(8)), std__cyl_bessel_kn(10, v_t(8))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(20, v_t(8)), std__cyl_bessel_kn(20, v_t(8))   , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(2 , v_t(1.9010021686554)), std__cyl_bessel_kn(2, v_t(1.9010021686554))   , 15.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_kn(2,  v_t(244.708321520116)), std__cyl_bessel_kn(2, v_t(244.708321520116)) , 15.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(3,  v_t(517.048069608611)), std__cyl_bessel_kn(3,  v_t(517.048069608611)), 15.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(0, eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(2, eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(3, eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  //simd large x
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(3, T(1500)),  eve::zero(eve::as<T>()),  5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(2, T(50)),   T(std__cyl_bessel_kn(2, v_t(50))),   5.0);
  //simd forward
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(2, T(10)),    T(std__cyl_bessel_kn(2, v_t(10)))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(3, T(5)),     T(std__cyl_bessel_kn(3, v_t(5)))    , 35.0);
  //simd small
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(0, T(0.1)),   T(std__cyl_bessel_kn(0, v_t(0.1)))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(1, T(0.2)),   T(std__cyl_bessel_kn(1, v_t(0.2)))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(2, T(0.1)),   T(std__cyl_bessel_kn(2, v_t(0.1)))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(3, T(0.2)),   T(std__cyl_bessel_kn(3, v_t(0.2)))  , 5.0);
  //simd medium
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(10, T(8)),   T(std__cyl_bessel_kn(10, v_t(8)))   , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(20, T(8)),   T(std__cyl_bessel_kn(20, v_t(8)))   , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(2 , T(1.9010021686554)), T(std__cyl_bessel_kn(2, v_t(1.9010021686554)))   , 15.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(2,  T(244.708321520116)),T(std__cyl_bessel_kn(2, v_t(244.708321520116))) , 15.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(3,  T(517.048069608611)),T(std__cyl_bessel_kn(3,  v_t(517.048069608611))), 15.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(0), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(2), eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(3), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(3), T(1500)),   eve::zero(eve::as<T>()),  5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(2), T(50)),   T(std__cyl_bessel_kn(2, v_t(50))),   5.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(2), T(10)),    T(std__cyl_bessel_kn(2, v_t(10)))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(3), T(5)),     T(std__cyl_bessel_kn(3, v_t(5)))    , 35.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(2), T(0.1)),   T(std__cyl_bessel_kn(2, v_t(0.1)))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(3), T(0.2)),   T(std__cyl_bessel_kn(3, v_t(0.2)))  , 5.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(10), T(8)),   T(std__cyl_bessel_kn(10, v_t(8)))   , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(20), T(8)),   T(std__cyl_bessel_kn(20, v_t(8)))   , 5.0);

  using i_t = eve::as_integer_t<v_t>;
  using I_t = eve::wide<i_t, eve::cardinal_t<T>>;

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(0), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(2), eve::inf(eve::as<T>())), eve::inf(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(3), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(3), T(1500)),  eve::zero(eve::as<T>()),  5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(2), T(50)),   T(std__cyl_bessel_kn(2, v_t(50))),   5.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(2), T(10)),    T(std__cyl_bessel_kn(2, v_t(10)))   , 35.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(3), T(5)),     T(std__cyl_bessel_kn(3, v_t(5)))    , 35.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(2), T(0.1)),   T(std__cyl_bessel_kn(2, v_t(0.1)))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(3), T(0.2)),   T(std__cyl_bessel_kn(3, v_t(0.2)))  , 5.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(10), T(8)),   T(std__cyl_bessel_kn(10, v_t(8)))   , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(I_t(20), T(8)),   T(std__cyl_bessel_kn(20, v_t(8)))   , 5.0);

  TTS_RELATIVE_EQUAL(eve__cyl_bessel_kn(n, a0),   map(std__cyl_bessel_kn, n, a0)   , 1.0e-4);
  TTS_RELATIVE_EQUAL(eve__cyl_bessel_kn(-n, a0),   map(std__cyl_bessel_kn, -n, a0)   , 1.0e-4);

};

//==================================================================================================
//== non integral orders
//==================================================================================================
EVE_TEST( "Check behavior of cyl_bessel_kn on wide with non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 10.0)
        , eve::test::randoms(0.0, 200.0))
        )
  <typename T>(T n, T a0 )
{
   using v_t = eve::element_type_t<T>;
   auto eve__cyl_bessel_kn =  [](auto n, auto x) { return eve::cyl_bessel_kn(n, x); };
   auto std__cyl_bessel_kn =  [](auto n, auto x)->v_t { return boost::math::cyl_bessel_k(n, x); };

   if constexpr( eve::platform::supports_invalids )
   {
     TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(0.5), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
     TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(2.5), eve::inf(eve::as<T>())), eve::zero(eve::as<T>()), 0);
     TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(3.5), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
   }
   // large x
   TTS_ULP_EQUAL(eve__cyl_bessel_kn(v_t(3.5), v_t(1500)),  eve::zero(eve::as<v_t>()),  10.0);
   TTS_ULP_EQUAL(eve__cyl_bessel_kn(v_t(2.5), v_t(50)),   std__cyl_bessel_kn(v_t(2.5), v_t(50)),   10.0);
   // forward
   TTS_ULP_EQUAL(eve__cyl_bessel_kn(v_t(2.5), v_t(10)),    std__cyl_bessel_kn(v_t(2.5), v_t(10))   , 10.0);
   TTS_ULP_EQUAL(eve__cyl_bessel_kn(v_t(3.5), v_t(5)),     std__cyl_bessel_kn(v_t(3.5), v_t(5))    , 10.0);
   // serie
   TTS_ULP_EQUAL(eve__cyl_bessel_kn(v_t(2.5), v_t(0.1)),   std__cyl_bessel_kn(v_t(2.5), v_t(0.1))  , 10.0);
   TTS_ULP_EQUAL(eve__cyl_bessel_kn(v_t(3.5), v_t(0.2)),   std__cyl_bessel_kn(v_t(3.5), v_t(0.2))  , 10.0);
   // besseljy
   TTS_ULP_EQUAL(eve__cyl_bessel_kn(v_t(10.5), v_t(8)),    std__cyl_bessel_kn(v_t(10.5), v_t(8))   , 10.0);
   TTS_ULP_EQUAL(eve__cyl_bessel_kn(v_t(10.5), v_t(8)),    std__cyl_bessel_kn(v_t(10.5), v_t(8))   , 10.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(0.5), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(2.5), eve::inf(eve::as<T>())), eve::zero(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(3.5), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(3.5), T(1500)),  eve::zero(eve::as<T>()),  10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(2.5), T(50)),   T(std__cyl_bessel_kn(v_t(2.5), v_t(50))),   10.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(2.5), T(10)),    T(std__cyl_bessel_kn(v_t(2.5), v_t(10)))   , 310.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(3.5), T(5)),     T(std__cyl_bessel_kn(v_t(3.5), v_t(5)))    , 310.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(2.5), T(0.1)),   T(std__cyl_bessel_kn(v_t(2.5), v_t(0.1)))  , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(3.5), T(0.2)),   T(std__cyl_bessel_kn(v_t(3.5), v_t(0.2)))  , 10.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(10.5), T(8)),   T(std__cyl_bessel_kn(v_t(10.5), v_t(8)))   , 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_kn(T(10.5), T(8)),   T(std__cyl_bessel_kn(v_t(10.5), v_t(8)))   , 10.0);

  TTS_RELATIVE_EQUAL(eve__cyl_bessel_kn(n, a0),   map(std__cyl_bessel_kn, n, a0)   , 1.0e-3);
};

EVE_TEST( "Check behavior of diff cyl_bessel_kn on wide "
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 10.0)
                             , eve::test::randoms(0.0, 10.0))
        )
  <typename T>(T n, T a0 )
{
  using v_t = eve::element_type_t<T>;
  auto eve__diff_bessel_kn =  [](auto n, auto x) { return eve::diff(eve::cyl_bessel_kn)(n, x); };
  auto std__diff_bessel_kn =  [](auto n, auto x)->v_t { return boost::math::cyl_bessel_k_prime(double(n), double(x)); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_kn(n, a0),   map(std__diff_bessel_kn, n, a0)   , 1.0e-3);

};


/**
 ** Isaac Genome Alignment Software
 ** Copyright (c) 2010-2017 Illumina, Inc.
 ** All rights reserved.
 **
 ** This software is provided under the terms and conditions of the
 ** GNU GENERAL PUBLIC LICENSE Version 3
 **
 ** You should have received a copy of the GNU GENERAL PUBLIC LICENSE Version 3
 ** along with this program. If not, see
 ** <https://github.com/illumina/licenses/>.
 **
 ** \file MathCompatibility.hh
 **
 ** \brief  Compatibility layer for math-related constructs.
 **
 ** \author Come Raczy
 **/

#ifndef iSAACCOMMON_MATH_COMPATIBILITY_HH
#define iSAACCOMMON_MATH_COMPATIBILITY_HH

#include <cmath>

#include "config.h"

#ifndef HAVE_FLOORF
inline float floorf(float x)
{
    return static_cast<float> (floor(x));
}
#define HAVE_FLOORF
#endif

#ifndef HAVE_ROUND
inline double round(double x)
{
    return (x - floor(x) < 0.5) ? floor(x) : ceil(x);
}
#define HAVE_ROUND
#endif

#ifndef HAVE_ROUNDF
inline float roundf(float x)
{
    return (x - floorf(x) < 0.5f ? floorf(x) : ceil(x));
}
#define HAVE_ROUNDF
#endif

#ifndef HAVE_POWF
inline float powf(float x, float y)
{
    return static_cast<float> (pow(x, y));
}
#define HAVE_POWF
#endif

#ifndef HAVE_ERF
#include <boost/math/special_functions/erf.hpp>
inline double erf(double x)
{
    return boost::math::erf(x);
}
#define HAVE_ERF
#endif

#ifndef HAVE_ERFF
#include <boost/math/special_functions/erf.hpp>
inline float erff(float x)
{
    return boost::math::erf(x);
}
#define HAVE_ERFF
#endif

#ifndef HAVE_ERFC
#include <boost/math/special_functions/erf.hpp>
inline double erfc(double x)
{
    return boost::math::erfc(x);
}
#define HAVE_ERFC
#endif

#ifndef HAVE_ERFCF
#include <boost/math/special_functions/erf.hpp>
inline float erfc(float x)
{
    return boost::math::erfc(x);
}
#define HAVE_ERFCF
#endif

#endif // #ifndef iSAACCOMMON_MATH_COMPATIBILITY_HH


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to 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 <boost/math/bindings/rr.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>

#include <boost/math/tools/test_data.hpp>
#include "ntl_rr_lanczos.hpp"

using namespace boost::math::tools;

//
// Force trunctation to float precision of input values:
// we must ensure that the input values are exactly representable
// in whatever type we are testing, or the output values will all
// be thrown off:
//
float external_f;
float force_truncate(const float* f)
{
   external_f = *f;
   return external_f;
}

float truncate_to_float(boost::math::ntl::RR r)
{
   float f = boost::math::tools::real_cast<float>(r);
   return force_truncate(&f);
}

struct gamma_inverse_generator_a
{
   std::tr1::tuple<boost::math::ntl::RR, boost::math::ntl::RR> operator()(const boost::math::ntl::RR x, const boost::math::ntl::RR p)
   {
      boost::math::ntl::RR x1 = boost::math::gamma_p_inva(x, p);
      boost::math::ntl::RR x2 = boost::math::gamma_q_inva(x, p);
      std::cout << "Inverse for " << x << " " << p << std::endl;
      return std::tr1::make_tuple(x1, x2);
   }
};

int test_main(int argc, char*argv [])
{
   boost::math::ntl::RR::SetPrecision(1000);
   boost::math::ntl::RR::SetOutputPrecision(100);

   bool cont;
   std::string line;

   parameter_info<boost::math::ntl::RR> arg1, arg2;
   test_data<boost::math::ntl::RR> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the inverse incomplete gamma function:\n"
      "  gamma_p_inva(a, p) and gamma_q_inva(a, q)\n\n";

   arg1 = make_power_param<boost::math::ntl::RR>(boost::math::ntl::RR(0), -4, 24);
   arg2 = make_random_param<boost::math::ntl::RR>(boost::math::ntl::RR(0), boost::math::ntl::RR(1), 15);
   data.insert(gamma_inverse_generator_a(), arg1, arg2);

   line = "igamma_inva_data.ipp";
   std::ofstream ofs(line.c_str());
   write_code(ofs, data, "igamma_inva_data");

   return 0;
}


// Copyright (c) 2005 - 2014 Marc de Kamps
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//    * Neither the name of the 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 OWNER 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.
//
//      If you use this software in work leading to a scientific publication, you should include a reference there to
//      the 'currently valid reference', which can be found at http://miind.sourceforge.net
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/test/execution_monitor.hpp>
#include <GeomLib.hpp>
#include <MPILib/include/AlgorithmInterface.hpp>
#include <MPILib/include/DelayedConnection.hpp>

using namespace GeomLib;
using namespace MPILib;
using MPILib::DelayedConnection;

BOOST_AUTO_TEST_CASE(MuSigmaScalarTest)
{
        MuSigmaScalarProduct<MPILib::DelayedConnection>  prod;

	vector<NodeType> vec_type;
	vec_type.push_back(EXCITATORY_GAUSSIAN);
	vec_type.push_back(INHIBITORY_GAUSSIAN);

	vector<MPILib::DelayedConnection> vec_con;
	MPILib::DelayedConnection con_1(1, 0.01,0.0);
	MPILib::DelayedConnection con_2(1,-0.01,0.0);
	vec_con.push_back(con_1);
	vec_con.push_back(con_2);

	Rate rate = 1000.0;
	vector<Rate> vec_rates;

	Time tau = 10e-3;

	vec_rates.push_back(rate);
	vec_rates.push_back(rate);

	MuSigma ms = prod.Evaluate(vec_rates, vec_con, tau);

	BOOST_CHECK(ms._mu == 0.0);
	BOOST_CHECK_CLOSE(ms._sigma, 0.0447214, 0.001);

}


#ifdef MEX

#include <igl/copyleft/cgal/outer_hull.h>

#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/validate_arg.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/C_STR.h>

#include <mex.h>
#include <Eigen/Dense>
#include <iostream>

#include <cstring>

void parse_rhs(
  const int nrhs,
  const mxArray *prhs[],
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & F,
  bool & legacy)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  mexErrMsgTxt(nrhs >= 2, "The number of input arguments must be >=2.");

  const auto & parse_mesh = [](
    const mxArray *prhs[],
    Eigen::MatrixXd & V,
    Eigen::MatrixXi & F)
  {
    const int dim = mxGetN(prhs[0]);
    mexErrMsgTxt(dim == 3,
      "Mesh vertex list must be #V by 3 list of vertex positions");
    mexErrMsgTxt(dim == mxGetN(prhs[1]),
      "Mesh \"face\" simplex size must equal dimension");
    parse_rhs_double(prhs,V);
    parse_rhs_index(prhs+1,F);
  };
  parse_mesh(prhs,V,F);
  {
    int i = 3;
    while(i<nrhs)
    {
      mexErrMsgTxt(mxIsChar(prhs[i]),"Parameter names should be strings");
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("Legacy",name) == 0)
      {
        validate_arg_logical(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        legacy = (bool)*mxGetLogicals(prhs[++i]);
      }else
      {
        mexErrMsgTxt(false,"Unknown parameter");
      }
      i++;
    }
  }
}

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;
  using namespace igl::copyleft::cgal;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  MatrixXd V,HV;
  MatrixXi F,G;
  VectorXi J,flip;
  bool legacy = false;
  parse_rhs(nrhs,prhs,V,F,legacy);
  if(legacy)
  {
    outer_hull_legacy(V,F,G,J,flip);
  }else
  {
    outer_hull(V,F,HV,G,J,flip);
  }

  int offset = (legacy?0:1);
  switch(nlhs-offset)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 3:
    {
      prepare_lhs_logical(flip,plhs+2+offset);
      // Fall through
    }
    case 2:
    {
      prepare_lhs_index(J,plhs+1+offset);
      // Fall through
    }
    case 1:
    {
      prepare_lhs_index(G,plhs+0+offset);
      // Fall through
    }
    case 0:
    {
      if(!legacy)
      {
        prepare_lhs_double(HV,plhs);
      }
      break;
    }
    case -1:
    {
      assert(!legacy);
      break;
    }
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
}

#endif


#include "refill/measurement_models/linearized_measurement_model.h"

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include "refill/distributions/gaussian_distribution.h"

namespace refill {

class LinearizedMeasurementModelClass : public LinearizedMeasurementModel {
 public:
  LinearizedMeasurementModelClass(
      const size_t& state_dim, const size_t& measurement_dim,
      const DistributionInterface& measurement_noise)
      : LinearizedMeasurementModel(state_dim, measurement_dim,
                                   measurement_noise) {}
  Eigen::VectorXd observe(const Eigen::VectorXd& state,
                          const Eigen::VectorXd& noise) const {
    return state + noise;
  }
};

TEST(LinearizedMeasurementModelTest, FullTest) {
  GaussianDistribution measurement_noise(Eigen::Vector2d::Zero(),
                                         Eigen::Matrix2d::Identity());

  LinearizedMeasurementModelClass measurement_model(2, 2, measurement_noise);

  Eigen::MatrixXd state_jacobian = measurement_model.getMeasurementJacobian(
      Eigen::Vector2d::Zero());

  ASSERT_EQ(measurement_model.getMeasurementDim(), state_jacobian.rows());
  ASSERT_EQ(measurement_model.getStateDim(), state_jacobian.cols());
  ASSERT_EQ(Eigen::Matrix2d::Identity(), state_jacobian);

  Eigen::Matrix2d noise_jacobian = measurement_model.getNoiseJacobian(
      Eigen::Vector2d::Zero());

  ASSERT_EQ(measurement_model.getMeasurementDim(), noise_jacobian.rows());
  ASSERT_EQ(measurement_model.getNoiseDim(), noise_jacobian.cols());
  ASSERT_EQ(Eigen::Matrix2d::Identity(), noise_jacobian);
}

}  // namespace refill


/*
 * @Descripttion:
 * @version:
 * @Author: li
 * @Date: 2021-02-28 11:33:51
 * @LastEditors: li
 * @LastEditTime: 2021-04-12 18:00:38
 */
#include "ros/ros.h"
// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <geometry_msgs/PoseStamped.h>
#include <opencv2/opencv.hpp>
#include "eigen3/Eigen/Dense"
#include <opencv2/core.hpp>
#include "opencv2/core/eigen.hpp"
using namespace cv;
using namespace Eigen;
using namespace std;

geometry_msgs::PoseStamped c_pos,t_pos;

void ReadCalibrationFile(std::string _choose_file,cv::Mat &out_RT)
{
    std::cout << "_choose_file: " << _choose_file << std::endl;
    cv::FileStorage fs(_choose_file, cv::FileStorage::READ);
    if (!fs.isOpened())
    {
        std::cout << "Cannot open file calibration file" << _choose_file << std::endl;
    }
    else
    {
      fs["CameraExtrinsicMat"] >> out_RT;
    }
}

void readCameraParams(std::string& file){
    std::cout << "file:" << file << std::endl;
    cv::Mat color_lidar_exRT;
    std::string color_lidar_yamlfilepath;
    ReadCalibrationFile(file, color_lidar_exRT);

    Eigen::Matrix4d transform;
    cv::cv2eigen(color_lidar_exRT, transform);
    std::cout << "Matrix4d:"<< transform.matrix() << std::endl;
    Eigen::Transform<double, 3, Eigen::Affine> a3d_transform(transform);

    std::cout <<"translation:" <<a3d_transform.translation() << std::endl;
    std::cout <<"linear:" <<a3d_transform.linear() << std::endl;
    Eigen::Quaterniond qua;
    Eigen::Matrix3d rotation;
    rotation << a3d_transform.linear();
    qua = rotation;
}

void update(){
    /*
    Eigen::Matrix3d rotMatrix;
    Eigen::Vector3d vectorBefore(x, y, z)
    Eigen::Vector3d vectorAfter(a, b, c);

    rotMatrix = Eigen::Quaterniond::FromTwoVectors(vectorBefore, vectorAfter).toRotationMatrix();
    */
   //calc angle

}

void target_callback(const geometry_msgs::PoseStampedConstPtr &msg){
    t_pos.pose.position.x = msg->pose.position.x;
    t_pos.pose.position.y = msg->pose.position.y;
    t_pos.pose.position.z = msg->pose.position.z;
    Eigen::Quaterniond q(c_pos.pose.orientation.w, c_pos.pose.orientation.x, c_pos.pose.orientation.y, c_pos.pose.orientation.z);
    //forward
    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d f = q * v;

    Eigen::Vector3d c(t_pos.pose.position.x-c_pos.pose.position.x, t_pos.pose.position.y-c_pos.pose.position.y, t_pos.pose.position.z-c_pos.pose.position.z);

    Eigen::Matrix3d rotMatrix;
    rotMatrix = Eigen::Quaterniond::FromTwoVectors(f, c).toRotationMatrix();
    std::cout << "matrix:" << rotMatrix.matrix() << std::endl;
    Eigen::Vector3d eulerAngle=rotMatrix.eulerAngles(0,1,2);
    std::cout << "roll:" << eulerAngle(0) << std::endl;
    std::cout << "pitch:" << eulerAngle(1) << std::endl;
    std::cout << "yaw:" << eulerAngle(2) << std::endl;
    std::cout << "----------------------" << std::endl;
    std::cout << "roll:" << eulerAngle(0)*180/M_PI << std::endl;
    std::cout << "pitch:" << eulerAngle(1)*180/M_PI << std::endl;
    std::cout << "yaw:" << eulerAngle(2)*180/M_PI << std::endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "eigen_demo_node");
    ros::NodeHandle nh("~");
    std::string camera_file = nh.param<std::string>("camera_file", "");
    //readCameraParams(camera_file);
    ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("/goal", 1);
    ros::Subscriber sub = nh.subscribe("/target", 1, &target_callback);
    ROS_INFO("eigen_demo_node started...");
    ros::Rate rate(10);
    /*
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << 2.4668250442e+01, 6.320669323e+00, -1.3322685606304879e+02;

    Eigen::Matrix3f rotation(3,3);
    rotation << -5.08698404e-01, 1.58019021e-01, -8.46319020e-01,
                8.60769331e-01, 1.13197252e-01, -4.96248573e-01,
                1.73842758e-02, -9.80926335e-01, -1.93601176e-01;
    transform.linear() = rotation;

    Eigen::Matrix3d rotation1(3,3);
    rotation1 << -5.08698404e-01, 1.58019021e-01, -8.46319020e-01,
                8.60769331e-01, 1.13197252e-01, -4.96248573e-01,
                1.73842758e-02, -9.80926335e-01, -1.93601176e-01;
    Eigen::Quaterniond qua;
    qua = rotation1;

    Eigen::Vector3d eulerAngle=rotation1.eulerAngles(2,1,0);
    std::cout << "roll:" << eulerAngle(0) << std::endl;
    std::cout << "pitch:" << eulerAngle(1) << std::endl;
    std::cout << "yaw:" << eulerAngle(2) << std::endl;
    std::cout << transform.matrix() << std::endl;
    std::cout << transform.translation() << std::endl;
    std::cout << transform.linear() << std::endl;
    */
    cv::Mat color_lidar_exRT;
    ReadCalibrationFile(camera_file, color_lidar_exRT);

    Eigen::Matrix4d transform;
    cv::cv2eigen(color_lidar_exRT, transform);
    std::cout << "Matrix4d:"<< transform.matrix() << std::endl;
    Eigen::Transform<double, 3, Eigen::Affine> a3d_transform(transform);

    std::cout <<"translation:" <<a3d_transform.translation() << std::endl;
    std::cout <<"linear:" <<a3d_transform.linear() << std::endl;
    Eigen::Quaterniond qua;
    Eigen::Matrix3d rotation;
    rotation << a3d_transform.linear();
    qua = rotation;

    std::cout << "qua x:" << qua.x()<< " y:" << qua.y()<< " z:" << qua.z()<< " w:" << qua.w() << std::endl;
    Eigen::Vector3d eulerAngle=rotation.eulerAngles(2,1,0);
    std::cout << "roll:" << eulerAngle(0) << std::endl;
    std::cout << "pitch:" << eulerAngle(1) << std::endl;
    std::cout << "yaw:" << eulerAngle(2) << std::endl;

    geometry_msgs::PoseStamped goal;
    goal.header.stamp = ros::Time::now();
    goal.header.frame_id = "map";
    goal.pose.position.x = a3d_transform.translation()[0];
    goal.pose.position.y = a3d_transform.translation()[1];
    goal.pose.position.z = a3d_transform.translation()[2];
    goal.pose.orientation.x = qua.x();
    goal.pose.orientation.y = qua.y();
    goal.pose.orientation.z = qua.z();
    goal.pose.orientation.w = qua.w();
    c_pos.pose.position = goal.pose.position;
    c_pos.pose.orientation = goal.pose.orientation;
    while (ros::ok())
    {
        update();
        pub.publish(goal);
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}


//#include "visualizer.hpp"
//#include "windowHandler.hpp"
#include "visa.hpp"
#include <armadillo>
#include <cmath>
#include <iostream>

using namespace std;

void mexicanHat( double freq, arma::mat &matrix );
int main( int argc, char** argv )
{
  visa::WindowHandler plots;
  try
  {
    visa::Colormaps::Colormap_t cmap = visa::Colormaps::Colormap_t::NIPY_SPECTRAL;
    plots.addPlot( "Mexican Hat" );
    plots.setActive( "Mexican Hat" );
    plots.getActive().setCmap( cmap );
    arma::mat matrix(2000,2000);
    mexicanHat( 0.1, matrix );
    plots.getActive().setImg( matrix );
    plots.show();
    char quit;
    cout << "Press any character to quit...\n";
    cin >> quit;
  }
  catch ( exception &exc )
  {
    cout << exc.what() << endl;
    return 1;
  }
  catch (...)
  {
    cout << "Unexpected exception!\n";
    return 1;
  }

  return 0;
}

// Function implementation
void mexicanHat( double freq, arma::mat &matrix )
{
  // Add 0.01 to avoid problems at i=0 and j=0
  for ( int i=0;i<matrix.n_cols;i++ )
  {
    double x = -static_cast<double>(matrix.n_cols)/2.0 + i+0.01;
    for ( int j=0;j<matrix.n_rows;j++ )
    {
      double y = -static_cast<double>(matrix.n_rows)/2.0+j+0.01;
      double r = sqrt( x*x+y*y );
      matrix(i,j) = sin(freq*r)/(freq*r);
    }
  }
}


// ===========================================================================
// Imagine++ Libraries
// Copyright (C) Imagine
// For detailed information: http://imagine.enpc.fr/software
// ===========================================================================

#include "Imagine/LinAlg/MyEigen.h"
#include <Eigen/Dense>
using namespace Eigen;

//N'existe pas dans Eigen Blas
// Pas de calcul d'inverse avec ldlt dans eigen
//corriger
//info important car utilise dans SymMatrix.h
template <typename T>
void subSymInverse(int *n,T *ap,T *t,int *info)
{
  symToMatrix(*n,ap,t);
  Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > apMap (t,*n,*n,OuterStride<>(*n));
  Matrix<T, Dynamic, Dynamic> Ai (*n,*n);
  Eigen::LDLT<Matrix<T, Dynamic, Dynamic> > ldlt(apMap);
  Matrix<T, Dynamic, Dynamic> id=Matrix<T, Dynamic, Dynamic>::Identity(*n,*n);
  for (int i=0; i<*n;i++) {
  Ai.col(i)=ldlt.solve(id.col(i));
  };
  *info=1-(apMap*Ai).isApprox(id);
  apMap=Ai;
}

void symInverse(int *n, double *ap, double *t, int *info) { subSymInverse<double>(n, ap, t, info); }
void symInverse(int *n, float *ap, float *t, int *info) { subSymInverse<float>(n, ap, t, info); }

//N'existe pas dans Eigen Blas
//pas de calcul d'inverse avec llt dans eigen
//On va donc resoudre AX=B avec B=(1,0,0,0...) puis B=(0,1,0,....) etc...
//Puis on constitue la matrice inverse avec les n colonnes
//corriger
//info important car utilise dans Matrix.h
template <typename T>
void subSymDPInverse(int *n,T *ap,T* t,int *info)
{
  symToMatrix(*n,ap,t);
  Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > apMap (t,*n,*n,OuterStride<>(*n));
  Matrix<T, Dynamic, Dynamic> Ai (*n,*n);
  Eigen::LLT<Matrix<T, Dynamic, Dynamic> > llt(apMap);
  Matrix<T, Dynamic, Dynamic> id=Matrix<T, Dynamic, Dynamic>::Identity(*n,*n);
  for (int i=0; i<*n;i++) {
  Ai.col(i)=llt.solve(id.col(i));
  };
  *info=1-(apMap*Ai).isApprox(id);
  apMap=Ai;
}

void symDPInverse(int *n, double *ap, double *t, int *info) { subSymDPInverse<double>(n, ap, t, info); }
void symDPInverse(int *n, float *ap, float *t, int *info) { subSymDPInverse<float>(n, ap, t, info); }

// System of linear equations with packed symmetric matrix
template <typename T>
void subSymSolve(int *n,T *ap,int *,T *b,int *info)
{
  T *t= new T [(*n)*(*n)];
  symToMatrix(*n,ap,t);
  Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (t,*n,*n,OuterStride<>(*n));
  Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > BMap (b,*n,1,OuterStride<>(*n));
  Eigen::LDLT<Matrix<T, Dynamic, Dynamic> > ldlt (AMap);
  Matrix<T, Dynamic, Dynamic> x=ldlt.solve(BMap);
  *info=1-(AMap*x).isApprox(BMap);
  BMap=x;
  delete [] t;
}

void symSolve(int *n, double *ap, int *ipiv, double *b, int *info) { subSymSolve<double>(n, ap, ipiv, b, info); }
void symSolve(int *n, float *ap, int *ipiv, float *b, int *info) { subSymSolve<float>(n, ap, ipiv, b, info); }

// Determinant
template <typename T>
T subDeterminant(int *m, int *n, T *A, int *info)
{
   Map<Matrix<T, Dynamic, Dynamic>,0, OuterStride<> > AMap (A,*m,*n,OuterStride<>(*m));
   Eigen::FullPivLU<Matrix<T, Dynamic, Dynamic> > lu(AMap);
   *info = 0;
   return(lu.determinant());
}

double determinant(int *m, int *n, double *A, int *info) { return subDeterminant<double>(m, n, A, info); }
float determinant(int *m, int *n, float *A, int *info) { return subDeterminant<float>(m, n, A, info); }

template <typename T>
T subSymDeterminant(int *n,T *ap,int *info)
{
  T *t= new T [(*n)*(*n)];
  symToMatrix(*n,ap,t);
   Map<Matrix<T, Dynamic, Dynamic>,0, OuterStride<> > AMap (t,*n,*n,OuterStride<>(*n));
   Eigen::FullPivLU<Matrix<T, Dynamic, Dynamic> > lu(AMap);
  *info=1-AMap.isApprox(lu.reconstructedMatrix());
  delete[] t;
  return(lu.determinant());
}

double symDeterminant(int *n, double *ap, int *info) { return subSymDeterminant<double>(n, ap, info); }
float symDeterminant(int *n, float *ap, int *info) { return subSymDeterminant<float>(n, ap, info); }


#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

#include <scitbx/math/zernike.h>
#include <scitbx/math/rotation.h>

namespace scitbx { namespace math {
namespace boost_python{

  struct dmatrix_wrapper
  {
    typedef dmatrix < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("dmatrix", no_init)
        .def( init<
                   int const& ,
                   double const&
                  >
             ((
                arg("l_max"),
                arg("beta")
             ))
            )
        .def("djmn", &w_t::djmn)
      ;
    }
  };

  void
  wrap_dmatrix()
  {
    dmatrix_wrapper::wrap();
  }

// correlation
  struct correlation_wrapper
  {
    typedef correlation < double > w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("correlation", no_init)
        .def( init<
                   scitbx::math::zernike::nlm_array<double> const&,
                   scitbx::math::zernike::nlm_array<double> const&,
                   int const& ,
                   double const&
                  >
             ((
                arg("f_nlm"),
                arg("m_nlm"),
                arg("l_max"),
                arg("beta")
             ))
            )
        .def("calc_correlation", &w_t::calc_correlation)
        .def("mm_coef", &w_t::mm_coef)
        .def("mhm_coef", &w_t::mhm_coef)
        .def("rotate_moving_obj", &w_t::rotate_moving_obj)
        .def("compare_FM", &w_t::compare_fm)
        .def("set_beta", &w_t::set_beta)
      ;
    }
  };

  void
  wrap_correlation()
  {
    correlation_wrapper::wrap();
  }

}
}}


#ifndef CIRCUMCENTER_H
#define CIRCUMCENTER_H
#include <mtao/types.h>

#include <Eigen/Dense>

namespace mtao {
namespace geometry {

template <typename Derived>
auto circumcenter_spd(const Eigen::MatrixBase<Derived>& V) {
    // 2 V.dot(C) = sum(V.colwise().squaredNorm()).transpose()

    // probably dont really need this temporary
    auto m = (V.rightCols(V.cols() - 1).colwise() - V.col(0)).eval();
    auto A = (2 * m.transpose() * m).eval();
    auto b = m.colwise().squaredNorm().transpose().eval();
    A.llt().solveInPlace(b);

    auto c = (V.col(0) + m * b).eval();
    return c;
}
template <typename Derived>
auto circumcenter_spsd(const Eigen::MatrixBase<Derived>& V) {
    mtao::MatrixX<typename Derived::Scalar> A(V.cols() + 1, V.cols() + 1);
    A.setConstant(1);
    A(V.cols(), V.cols()) = 0;
    auto m = V.transpose() * V;
    ;
    A.topLeftCorner(V.cols(), V.cols()) = 2 * m;
    mtao::VectorX<typename Derived::Scalar> b(V.cols() + 1);
    b.setConstant(1);
    b.topRows(m.cols()) = V.colwise().squaredNorm().transpose();

    auto x = A.colPivHouseholderQr().solve(b).eval();

    return (V * x.topRows(V.cols())).eval();
}
template <typename Derived>
auto circumcenter(const Eigen::MatrixBase<Derived>& V) {
    return circumcenter_spd(V);
}

template <typename VertexDerived, typename SimplexDerived>
auto circumcenters(const Eigen::MatrixBase<VertexDerived>& V,
                   const Eigen::MatrixBase<SimplexDerived>& S) {
    constexpr static int E = VertexDerived::RowsAtCompileTime;  // embed dim
    constexpr static int N =
        SimplexDerived::ColsAtCompileTime;  // number of elements
    constexpr static int D = SimplexDerived::RowsAtCompileTime;  // simplex dim
    using Scalar = typename VertexDerived::Scalar;

    Eigen::Matrix<Scalar, E, N> C(V.rows(), S.cols());

    Eigen::Matrix<Scalar, E, D> v(V.rows(), S.rows());
#pragma omp parallel for private(v)
    for (int i = 0; i < S.cols(); ++i) {
        auto s = S.col(i);
        for (int j = 0; j < S.rows(); ++j) {
            v.col(j) = V.col(s(j));
        }
        C.col(i) = circumcenter(v);
    }
    return C;
}
}  // namespace geometry
}  // namespace mtao
#endif  // CIRCUMCENTER_H


#ifndef FINDINTRA_CPP
#define FINDINTRA_CPP

#include <cstdlib>
#include <ctime>
#include <fstream>
#include <iostream>
#include <map>
#include <math.h>
#include <vector>

#include <gsl/gsl_cdf.h>

#include <scythestat/rng/mersenne.h>
#include <scythestat/distributions.h>
#include <scythestat/ide.h>
#include <scythestat/la.h>
#include <scythestat/matrix.h>
#include <scythestat/rng.h>
#include <scythestat/smath.h>
#include <scythestat/stat.h>
#include <scythestat/optimize.h>

#include <IRLS_glm/IRLS.h>

//#include <RInside.h>

#include <mlpack/methods/lars/lars.hpp>
#include <mlpack/methods/linear_regression/linear_regression.hpp>
#include <boost/test/unit_test.hpp>

#define INTTAG 0
#define BOOLTAG 1
#define DIETAG 2

#define TESTFREQTHRES 1
#define NEIGHBDIS 5
//#define MORANI 0.001
#define PCUTOFF 0.004
#define PRECISION 10e48
#define READBLOCK 409600

//using namespace std;
using std::abs;
using std::basic_string;
using std::cerr;
using std::cin;
using std::cout;
using std::endl;
using std::exception;
using std::flush;
using std::ifstream;
using std::istringstream;
using std::make_pair;
using std::map;
using std::ofstream;
using std::ostringstream;
using std::pair;
using std::sort;
using std::string;
using std::vector;

int reader_dif(const string&, vector< vector< vector<int> > >&);
int reader_dsm(const string&, vector< vector<int> >&);
void reader_ictm(const string&, map<int, int>&);
int findIntraDomainInteraction(map<int, int>&, string, vector< vector<int> >&, vector< vector< vector<int> > >&, map<int, int>&, map< int, map< pair<int, int>, double > >&, double, double, double);
int rmDependentCol(vector< vector<double> >&, vector<double>&, vector< pair<int, int> >&, double, int&, vector< vector<double> >&);
int calStanScores(vector< vector<double> >&);
int calPearsonCorr(vector< vector<double> >&, vector< pair<int, int> >&, const double&, int, map<int, int>&);
int writer_corrFile(map< pair<int, int>, double >&, string&);

void usage();

inline void usage()
{
    cout <<"Usage: findIntraDomainInteraction empericalDistributionFile interChrFreqFile domainSitesFile domainInteractionFreqFile chrNum MORANI CorrelationThresholdForScreen"<<endl;
}

//convtIndex is now 0 based
inline int convtIndex(int row_fun, int col_fun, int siteNum_fun)
{
    if (row_fun==col_fun || row_fun>=siteNum_fun || col_fun>=siteNum_fun || row_fun<0 || col_fun<0)
    {//cout<<"row or column exceed bound."<<endl;
        return -1;
    }
    else
    {
        int max_fun = 0, min_fun = 0;
        if (row_fun<col_fun)
        {
            //switch to 1 based
            max_fun = col_fun+1;
            min_fun = row_fun+1;
        }
        else
        {
            max_fun = row_fun+1;
            min_fun = col_fun+1;
        }
        return((2*siteNum_fun-min_fun)*(min_fun-1)/2+max_fun-min_fun) - 1;//-1 put the output to be 0 based
    }
}

//l=sigma(yi(theta*xi+offseti)-exp(theta*xi+offseti))
class PoissonModel {
    public:
    double operator() (const scythe::Matrix<double> beta){
        const int n = y_.rows();
        const int p = X_.cols();

        scythe::Matrix<double> eta = X_ * beta + offset_;
        scythe::Matrix<double> m = exp(eta);
        double loglike = 0.0;
        for (int i=0; i<n; ++i)
        loglike += y_(i) * log(m(i)) - m(i);
        return -1.0 * loglike;
    }
    scythe::Matrix<double> y_;
    scythe::Matrix<double> X_;
    scythe::Matrix<double> offset_;
};

int main(int argc, char* argv[])
{
    if (argc < 10)
    {
        usage();
        exit(1);
    }

    string empDisFile = argv[1];
    string ictmFile = argv[2];
    string dsmFile = argv[3];
    string difFile = argv[4];
    string chrNum = argv[5];
    double MORANI = atof(argv[6]);
    double corrThres = atof(argv[7]);
    double disIntcp = atof(argv[8]);
    double disCoeff = atof(argv[9]);

    map<int, int> empDis_map;
    map<int, int> ictm_map;
    vector< vector<int> > dsm_map;
    vector< vector< vector<int> > > dif_map;

    //reader_ictm(empDisFile, empDis_map);
    //cout<<"\nReading empDis file done."<<endl;
    reader_ictm(ictmFile, ictm_map);
    cout<<"\nReading ictm file done."<<endl;
    reader_dsm(dsmFile, dsm_map);
    cout<<"\nReading dsm file done."<<endl;
    reader_dif(difFile, dif_map);
    cout<<"\nReading dif file done."<<endl;

    map< int, map< pair<int, int>, double > > pVal_map;

    findIntraDomainInteraction(empDis_map, chrNum, dsm_map, dif_map, ictm_map, pVal_map, corrThres, disIntcp, disCoeff);

    return 0;
}

int reader_dif(const string& fileToread, vector< vector< vector<int> > >& dif_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        //char a;
        //cin >>a;
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char lineStr[READBLOCK];
    char freq_str[15];
    int freq_int = 0;
    int domainNum = 0;

    int column = 0;

    char* it_lineStr;
    char* it_token;

    int peakNum = 0;

    //copy data to a map, which chrNo is the key, the value is a vector of ints(read's starting point)
    //ofstream testBadFile("test");
    vector< vector<int> > tempDomn;
    while(inputFile.getline(lineStr,READBLOCK))
    {
        it_lineStr = lineStr;

        column = 0;

        if (lineStr != NULL)
        {
            vector<int> tempFreq;
            //testBadFile<<lineStr<<endl;

            //pass head white space
            if(*it_lineStr == 'd')
            {
                if (tempDomn.begin() != tempDomn.end())
                {
                    ++domainNum;
                    dif_local.push_back(tempDomn);
                    tempDomn.clear();
                }
                continue;
            }

            while(*it_lineStr != '\0' && *it_lineStr != '\n')
            {
                if (*it_lineStr == ' ' || *it_lineStr == '\t')
                {
                    //remove white space
                    while(*it_lineStr == ' ' || *it_lineStr == '\t')
                    {
                        ++it_lineStr;
                    }

                    ++column;
                }
                else
                {
                    it_token = freq_str;
                    while(*it_lineStr != ' ' && *it_lineStr != '\t' && *it_lineStr != '\0' && *it_lineStr != '\n')
                    {
                        *it_token = *it_lineStr;
                        ++it_token;
                        ++it_lineStr;
                    }
                    *it_token = '\0';
                    freq_int = atoi(freq_str);
                    tempFreq.push_back(freq_int);
                }
            }

            tempDomn.push_back(tempFreq);
            ++peakNum;
        }
    }

    if(tempDomn.begin() != tempDomn.end())
    {
        dif_local.push_back(tempDomn);
    }

    inputFile.close();

    return peakNum;
}

int reader_dsm(const string& fileToread, vector< vector<int> >& dsm_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        //char a;
        //cin >>a;
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char lineStr[READBLOCK];
    char pos_str[15];
    int pos_int = 0;

    int column = 0;

    char* it_lineStr;
    char* it_token;

    int peakNum = 0;

    //copy data to a map, which chrNo is the key, the value is a vector of ints(read's starting point)
    //ofstream testBadFile("test");
    while(inputFile.getline(lineStr,READBLOCK))
    {
        it_lineStr = lineStr;

        column = 0;

        if (lineStr != NULL)
        {
            vector<int> tempPos;
            //testBadFile<<lineStr<<endl;

            //pass head white space
            while(*it_lineStr == ' '|| *it_lineStr == '\t')
            {
                ++it_lineStr;
            }

            while(*it_lineStr != '\0' && *it_lineStr != '\n')
            {
                if (*it_lineStr == ' ' || *it_lineStr == '\t')
                {
                    //remove white space
                    while(*it_lineStr == ' ' || *it_lineStr == '\t')
                    {
                        ++it_lineStr;
                    }

                    ++column;
                }
                else
                {
                    it_token = pos_str;
                    while(*it_lineStr != ' ' && *it_lineStr != '\t' && *it_lineStr != '\0' && *it_lineStr != '\n')
                    {
                        *it_token = *it_lineStr;
                        ++it_token;
                        ++it_lineStr;
                    }
                    *it_token = '\0';
                    pos_int = atoi(pos_str);
                    tempPos.push_back(pos_int);
                }
            }

            dsm_local.push_back(tempPos);
            ++peakNum;
        }
    }

    inputFile.close();

    return peakNum;
}

void reader_ictm(const string& fileToread, map<int, int>& ictm_local)
{
    ifstream inputFile(fileToread.c_str());
    if (!inputFile)
    {
        cout <<"\n"<< "Error opening " << fileToread << "." << endl;
        exit(1);
    }

    char pos[15];
    int pos_int = 0;
    char freq[15];
    int freq_int = 0;

    char* it_lineStr;
    char* it_token;

    char lineStr[512];
    while(inputFile.getline(lineStr,512))
    {
        if (lineStr != NULL)
        {
            it_lineStr = lineStr;

            it_token = pos;
            while(*it_lineStr != '\t')
            {
                *it_token = *it_lineStr;
                ++it_token;
                ++it_lineStr;
            }
            *it_token = '\0';
            ++it_lineStr;
            pos_int = atoi(pos);

            it_token = freq;
            while(*it_lineStr != '\t' && *it_lineStr != '\n' && *it_lineStr != '\0')
            {
                *it_token = *it_lineStr;
                ++it_token;
                ++it_lineStr;
            }
            *it_token = '\0';
            freq_int = atoi(freq);

            ictm_local[pos_int] = freq_int;
        }
    }
}

int findIntraDomainInteraction(map<int, int>& empDisMap_fun, string jobChr_fun, vector< vector<int> >& domainSitesMap_fun, vector< vector< vector<int> > >& domainCSinterFreq_fun, map<int, int>& csInterChromTotalMap_fun, map< int, map< pair<int, int>, double > >& pVal_fun, double corrThres_fun, double disIntcp_fun, double disCoeff_fun)
{
    string file_effBetas =  jobChr_fun + "_domainEffBetas";
    ofstream outEffBetas(file_effBetas.c_str());

    int domainNum = domainSitesMap_fun.size();

    //#pragma omp parallel for
    for (int domainIt = 0; domainIt < domainNum; ++domainIt)
    {
        map< pair<int, int>, double>& domainPval = pVal_fun[domainIt];
        outEffBetas<<"domain "<<domainIt;
        vector<int>& sitesMap = domainSitesMap_fun[domainIt];
        vector< vector<int> >& csInterFreqMap = domainCSinterFreq_fun[domainIt];
        int siteNum = sitesMap.size();

        int size_convert = convtIndex(siteNum-2,siteNum-1,siteNum) + 1;//the convtIndex function's first two input is 0 based and the output is 0 based too
	vector<double> freq_convert;
        vector<double> offset;//distance effect
        vector<double> effVec;//efficiency of cutting site and mappability, measured by the total inter chromosome hybrid frags

        for (int rowIt=0; rowIt<siteNum; ++rowIt)
        {
            for (int colIt=rowIt+1; colIt<siteNum; ++colIt)
            {
                int index_convert=convtIndex(rowIt,colIt,siteNum);

		//log(y/ydist)=mu+betaeff*effvec
                freq_convert.push_back(csInterFreqMap[rowIt][colIt]);
		offset.push_back(log(abs(sitesMap[rowIt]-sitesMap[colIt])+1)*disCoeff_fun+disIntcp_fun);
		//the digestion and ligation efficient of a cutting site should take log because, its log should proportional to log(freq)
                effVec.push_back(log(sqrt(csInterChromTotalMap_fun[sitesMap[rowIt]])*sqrt(csInterChromTotalMap_fun[sitesMap[colIt]])+1));
            }
        }
	cout<<"construt matrix done."<<endl;

	vector< vector<double> > covMatrix;
	covMatrix.push_back(effVec);

	IRLS irls("log-link");
	bool quasi_lik = false;
	irls.link->quasi = quasi_lik;
	irls.load_data(freq_convert, covMatrix, offset);
	//outEffBetas<<"data loaded"<<endl;
	irls.fit_model();
	//outEffBetas<<"model fitted"<<endl;
	vector<double> coev = irls.get_coef();
	//outEffBetas<<"get coef done"<<endl;
	vector<double> sev = irls.get_stderr();
	//outEffBetas<<"get stderr done"<<endl;

	//outEffBetas<<"Col\tEstimate\tStd.Error\tp-value"<<endl;
	for(size_t i = 0; i < coev.size(); ++i)
	{
	    //printf("X%-9zu%12.9f%12.8f", i, coev[i], sev[i]);
	    //X0 is intercept/mu
	    double p_eff=0.0;
	    if(! irls.link->quasi)
	    {
		//printf("%15.6e\n", 2 * gsl_cdf_gaussian_P(-fabs(coev[i]/sev[i]), 1.0));
		p_eff=2 * gsl_cdf_gaussian_P(-fabs(coev[i]/sev[i]), 1.0);
	    }
	    else
	    {
		//printf("%15.6e\n", 2 * gsl_cdf_tdist_P(-fabs(coev[i]/sev[i]), size_convert-irls.get_rank_X()));
		p_eff=2 * gsl_cdf_tdist_P(-fabs(coev[i]/sev[i]), size_convert-irls.get_rank_X());
	    }
	    outEffBetas<<"\t"<<coev[i]<<"\t"<<sev[i]<<"\t"<<p_eff;
	}
	outEffBetas<<endl;
	covMatrix.clear();
	//outEffBetas.close();
	continue;

	double mu = coev[0];
	//double alpha = coev[1];
	double beta = coev[1];
	vector<double> constTerms(size_convert, 0.0);
	for (int i = 0; i < size_convert; ++i)
	{
	    //constTerms[i] = mu+dist_ij[i]*alpha+effVec[i]*beta;
	    constTerms[i] = exp(mu+effVec[i]*beta+offset[i]);
	    //constTerms[i] = mu+effVec[i]*beta+offset[i];
	}

        int counter_fit=0;
        int counter_skip=0;

        //#pragma omp parallel for
        for (int rowIt=0; rowIt<siteNum; ++rowIt)
        {
            for (int colIt=rowIt+1; colIt<siteNum; ++colIt)
            {
                if(csInterFreqMap[rowIt][colIt]>TESTFREQTHRES)
                {
                    vector<double> neighbFreq;
                    vector<double> neighbConstTerms;
                    vector<double> distVec;

                    int sideLen = 2*NEIGHBDIS + 1;
                    int colNum_mat = 0;
                    int rowNum_mat = 0;

                    vector< pair<int, int> > oriCoor;
                    for (int neighbRowIt=(rowIt-NEIGHBDIS), squareRowIt = 0; squareRowIt < sideLen; ++neighbRowIt, ++squareRowIt)
                    {
                        for (int neighbColIt=colIt-NEIGHBDIS, squareColIt = 0; squareColIt < sideLen; ++neighbColIt, ++squareColIt)
                        {
                            int distMatrix_colIt=convtIndex(neighbRowIt, neighbColIt, siteNum);
                            //if (distMatrix_colIt==-1 || neighbColIt<=neighbRowIt || colIt<=neighbRowIt || neighbColIt<=rowIt || (neighbRowIt==rowIt && neighbColIt==colIt) )
                            if (distMatrix_colIt==-1 || neighbColIt<=neighbRowIt || colIt<=neighbRowIt || neighbColIt<=rowIt)
                            {continue;}

                            //for future get back the original coordinates
                            if(freq_convert[distMatrix_colIt] > TESTFREQTHRES)
                            {
                                oriCoor.push_back(make_pair(neighbRowIt, neighbColIt));
                                ++colNum_mat;
                            }

                            neighbFreq.push_back(freq_convert[distMatrix_colIt]-constTerms[distMatrix_colIt]);
                            //neighbConstTerms.push_back(constTerms[distMatrix_colIt]);
                            distVec.push_back(log(abs(sitesMap[rowIt]-sitesMap[neighbRowIt])+abs(sitesMap[colIt]-sitesMap[neighbColIt])+1));
                            ++rowNum_mat;
                        }
                    }

		    IRLS irls("log-link");
		    bool quasi_lik = false;
		    irls.link->quasi = quasi_lik;
		    irls.load_data(freq_convert, covMatrix, offset);
		    //outEffBetas<<"data loaded"<<endl;
		    irls.fit_model();
		    //outEffBetas<<"model fitted"<<endl;
		    vector<double> coev = irls.get_coef();
		    //outEffBetas<<"get coef done"<<endl;
		    vector<double> sev = irls.get_stderr();
		    //outEffBetas<<"get stderr done"<<endl;

		    outEffBetas<<"Col\tEstimate\tStd.Error\tp-value"<<endl;
		    for(size_t i = 0; i < coev.size(); ++i)
		    {
			//printf("X%-9zu%12.9f%12.8f", i, coev[i], sev[i]);
			//X0 is intercept/mu
			outEffBetas<<"X"<<i<<"\t"<<coev[i]<<"\t"<<sev[i]<<"\t";
			if(! irls.link->quasi)
			{
			    //printf("%15.6e\n", 2 * gsl_cdf_gaussian_P(-fabs(coev[i]/sev[i]), 1.0));
			    outEffBetas<<2 * gsl_cdf_gaussian_P(-fabs(coev[i]/sev[i]), 1.0)<<endl;
			}
			else
			{
			    //printf("%15.6e\n", 2 * gsl_cdf_tdist_P(-fabs(coev[i]/sev[i]), size_convert-irls.get_rank_X()));
			    outEffBetas<<2 * gsl_cdf_tdist_P(-fabs(coev[i]/sev[i]), size_convert-irls.get_rank_X())<<endl;
			}
		    }
		    covMatrix.clear();
		    //outEffBetas.close();

                    //int neighbFreq_size = neighbFreq.size();
            arma::vec neighbFreq_lm(neighbFreq);
            //arma::vec neighbFreq_lm(neighbFreq_size);
            //for(int i=0; i<neighbFreq_size; ++i)
            //{
            //    neighbFreq_lm(i)=neighbFreq[i]-neighbConstTerms[i];
            //}

                    int distMat_size = distVec.size();
            arma::mat distMat_lm(distMat_size,2);
            for(int i=0; i<distMat_size; ++i)
            {
            distMat_lm(i,0)=1;
            distMat_lm(i,1)=distVec[i];
            }

            mlpack::regression::LinearRegression lm(distMat_lm, neighbFreq_lm);
            cout<<"construt region model done."<<endl;

            //set start values for theta
            arma::vec beta_lm = lm.Parameters();
            cout << "The region MLEs are: " << endl;
            std::cout <<beta_lm(0)<<"\t"<<beta_lm(1)<< "\n";

            //calculate std err using var(beta)=Var(error)*inv(t(X)X)
            //arma::mat<double> distMat_t = distMat_lm.t();
            //arma::mat<double> distMat_prod = distMat_t * distMat_lm;
            double var_err = lm.ComputeError(distMat_lm, neighbFreq_lm);
            arma::vec beta_stderr = sqrt(var_err * (distMat_lm.t()*distMat_lm).i());
            cout << "The stderr for betas are: " << endl;
            std::cout <<beta_stderr(0)<<"\t"<<beta_stderr(1)<< "\n";

            double fitPval=0.0;
            double beta_dis=0.0;
            exit(0);

                    if (fitPval < PCUTOFF && beta_dis < 0 && colNum_mat > 1)
                    {
                        int distNum = colNum_mat;
                        //colNum_mat++;//add const to the last column of the dist matrix
                        //colNum_mat;//add const to the last column of the dist matrix after rmIndependent cols is done
                        //vector<double> distVec_lasso(colNum_mat, 0.0);
                        vector<double> distVec_lasso((colNum_mat+1), 0.0);
                        vector< vector<double> > distMatrix_lasso;
                        for (int i=0; i<rowNum_mat; ++i)
                        {
                            distVec_lasso[colNum_mat] = neighbConstTerms[i];
                            distMatrix_lasso.push_back(distVec_lasso);
                        }

                        int testIndex = 0;
                        int matColIndex = 0;
                        for (int neighbRowIt=(rowIt-NEIGHBDIS), squareRowIt = 0; squareRowIt < sideLen; ++neighbRowIt, ++squareRowIt)
                        {
                            for (int neighbColIt=colIt-NEIGHBDIS, squareColIt = 0; squareColIt < sideLen; ++neighbColIt, ++squareColIt)
                            {
                                int distMatrix_colIt=convtIndex(neighbRowIt, neighbColIt, siteNum);
                                //if (distMatrix_colIt==-1 || neighbColIt<=neighbRowIt || colIt<=neighbRowIt || neighbColIt<=rowIt || (neighbRowIt==rowIt && neighbColIt==colIt) )
                                if (distMatrix_colIt==-1 || neighbColIt<=neighbRowIt || colIt<=neighbRowIt || neighbColIt<=rowIt || freq_convert[distMatrix_colIt] < TESTFREQTHRES)
                                {continue;}

                                int matRowIndex = 0;//inside loop is actually the row index for the final matrix
                                for (int neighbNeighbRowIt=(rowIt-NEIGHBDIS), squareNeighbRowIt = 0; squareNeighbRowIt < sideLen; ++neighbNeighbRowIt, ++squareNeighbRowIt)
                                {
                                    for (int neighbNeighbColIt=colIt-NEIGHBDIS, squareNeighbColIt = 0; squareNeighbColIt < sideLen; ++neighbNeighbColIt, ++squareNeighbColIt)
                                    {
                                        int distMatrix_rowIt=convtIndex(neighbNeighbRowIt, neighbNeighbColIt, siteNum);
                                        if (distMatrix_rowIt==-1 || neighbNeighbColIt<=neighbNeighbRowIt || colIt<=neighbNeighbRowIt || neighbNeighbColIt<=rowIt)
                                        {continue;}
                                        //distMatrix_lasso[matRowIndex][matColIndex]=exp(-MORANI*(abs(sitesMap[neighbNeighbRowIt]-sitesMap[neighbRowIt])+abs(sitesMap[neighbNeighbColIt]-sitesMap[neighbColIt])));
                                        distMatrix_lasso[matRowIndex][matColIndex]=log(abs(sitesMap[neighbNeighbRowIt]-sitesMap[neighbRowIt])+abs(sitesMap[neighbNeighbColIt]-sitesMap[neighbColIt])+1);
                                        ++matRowIndex;
                                    }
                                }
                                if (neighbRowIt==rowIt && neighbColIt==colIt)
                                {
                                    testIndex=matColIndex;
                                }
                                ++matColIndex;
                                //distMatrix[make_pair(distMatrix_rowIt, distMatrix_colIt)]=exp(-MORANI*(abs(sitesMap[rowIt]-sitesMap[neighbRowIt])+abs(sitesMap[colIt]-sitesMap[neighbColIt])));
                            }
                        }

                        //vector< vector<double> > distMatrix_lasso_rmDep;

                        int testFlag = 1;
                        bool flag_rmDep = false;
                        //if (flag_rmDep)
                        //{
                        //    testFlag = rmDependentCol(distMatrix_lasso, neighbFreq, oriCoor, corrThres_fun, testIndex, distMatrix_lasso_rmDep);
                        //}
                        //else
                        //{
                        //    distMatrix_lasso_rmDep = distMatrix_lasso;
                        //}
            //distMatrix_lasso.clear();

                        //add const to the matrix
                        if (testFlag == 1)
                        {
                            //for (int i=0; i<rowNum_mat; ++i)
                            //{
                            //    distMatrix_lasso[i].push_back(neighbConstTerms[i]);
                            //}

/*Note Y need to substract offset before doing lasso fit, may use code from IRLS_glm*/

                            //#pragma omp critical
                            {
                            outEffBetas<<rowIt<<"\t"<<colIt<<"\ttest column number: "<<testIndex<<endl;

                            //if(rowIt==414 && colIt==415){
                            const int rowNum_r = distMatrix_lasso.size();
                            const int colNum_r = (distMatrix_lasso.begin())->size();
                //!!!!!!!!!!! if remove dependents is true, this need to change
                            //Rcpp::NumericVector freq_r(neighbFreq.begin(), neighbFreq.end());

                            outEffBetas<<rowNum_r<<"\t"<<colNum_r<<endl;

                            //Rcpp::NumericMatrix distMat_r(rowNum_r, colNum_r);
                            for (int i=0; i<rowNum_r; ++i)
                            {
                                for (int j=0; j<colNum_r; ++j)
                                {
                                    //double rndNum = preMaxRnd*rand()/(RAND_MAX + 1.0) + minRnd;
                                    //distMat_r(i,j)= floor((distMatrix_lasso[i][j]+rndNum) * PRECISION + 0.5) / PRECISION;
                                    //distMat_r(i,j)= floor((distMatrix_lasso[i][j]) * PRECISION + 0.5) / PRECISION;
                    outEffBetas<<i<<"\t"<<j<<endl;
                                    //distMat_r(i,j)= distMatrix_lasso[i][j];
                                    //distMat_r(i,j)= distMatrix_lasso[i][j] + rndNum;
                                }
                            }
                //distMatrix_lasso.clear();

                            //#pragma omp critical
                            //domainPval[make_pair(sitesMap[rowIt], sitesMap[colIt])]=fitPval;
                            ++counter_fit;}//}
                            //#pragma omp end critical

                            //if (fitPval < PCUTOFF)
                            //{
                            //
                            //}
                        }
                    }
                    else if(colNum_mat==1)
                    {
                        //only 1 box has signal (freq>1), no need to use lasso to choose which one is real,or maybe could remove the ones due to the const terms, however glm should have taken care of that already
                    }
                }
                else
                {
                    #pragma omp critical
                    {++counter_skip;}
                    //#pragma omp end critical
                }
            }
        }
        //freq_convert.clear();
        //distMatrix.clear();
        //dist_ij.clear();
        //offsets.clear();
        //effVec.clear();
        //outEffBetas<<"chr "<<jobChr_fun<<" domain "<<domainIt<<" siteNum "<<siteNum<<" convert size "<<size_convert<<"fit number "<<counter_fit<<" skip number "<<counter_skip<<endl;
    }
    outEffBetas.close();

    return 0;
}

int rmDependentCol(vector< vector<double> >& distMatrix_local, vector<double>& freqVec_local, vector< pair<int, int> >& oriCoor_local, double thres_local, int& testIndex_local, vector< vector<double> >& distMatRmDep_local)
{
    int rowNum_fun = distMatrix_local.size();
    if (rowNum_fun != freqVec_local.size())
    {
        cerr<<"Error: row number of the dist matrix is different from the size of the frequency vector."<<endl;
        exit(1);
    }
    int colNum_fun = (distMatrix_local.begin())->size();
    vector< vector<double> > distMatrix_std;
    for (int i = 0; i < colNum_fun; ++i)
    {
        vector<double> tempVec;
        for (int j = 0; j < rowNum_fun; ++j)
        {
            tempVec.push_back(distMatrix_local[j][i]);
        }
        distMatrix_std.push_back(tempVec);
    }
    distMatrix_std.push_back(freqVec_local);
    calStanScores(distMatrix_std);

    map< pair<int, int>, double > corrMap;
    map<int, int> rmIndex;
    int testFlag_local = calPearsonCorr(distMatrix_std, oriCoor_local, thres_local, testIndex_local, rmIndex);
    cout<<"testFlag: "<<testFlag_local<<endl;

    int oriTestIndex = testIndex_local;
    for (int m = 0; m < oriTestIndex; ++m)
    {
        if (rmIndex[m]==1)
        {
            testIndex_local--;
        }
    }

    for (int i = 0; i < rowNum_fun; ++i)
    {
        vector<double> tempVec;
        for (int j = 0; j < colNum_fun; ++j)
        {
            if (rmIndex[j]!=1)
            {
                tempVec.push_back(distMatrix_local[i][j]);
            }
        }
        distMatRmDep_local.push_back(tempVec);
    }

    return testFlag_local;
}

int calStanScores(vector< vector<double> >& distMatrix_std_fun)
{
    for (vector< vector<double> >::iterator colIt = distMatrix_std_fun.begin(), colIt_end = distMatrix_std_fun.end(); colIt != colIt_end; ++colIt) //colomn as in original matrix before transpose
    {
        int rowNum_fun = colIt->size();
        double sum = 0.0;
        for (vector<double>::const_iterator rowIt = colIt->begin(), rowIt_end = colIt->end(); rowIt != rowIt_end; ++rowIt)
        {
            sum += *rowIt;
        }
        double mean = sum / rowNum_fun;

        double var = 0.0;
        for (vector<double>::const_iterator rowIt = colIt->begin(), rowIt_end = colIt->end(); rowIt != rowIt_end; ++rowIt)
        {
            double diff = *rowIt - mean;
            var += diff * diff;
        }
        var = var / (rowNum_fun - 1);
        double std = sqrt(var);

        for (vector<double>::iterator rowIt = colIt->begin(), rowIt_end = colIt->end(); rowIt != rowIt_end; ++rowIt)
        {
            *rowIt = (*rowIt - mean) / std;
        }
    }

    return 0;
}

int calPearsonCorr(vector< vector<double> >& distMatrix_std_fun, vector< pair<int, int> >& oriCoor_fun, const double& thres_fun, int testIndex_fun, map<int, int>& rmIndex_fun)
{
    int sigNum = 0;

    map< pair<int, int>, double > corrMap_fun;
    vector<double> xyCorr;
    int colNum_fun = distMatrix_std_fun.size(); // colomn as in original matrix
    int rowNum_fun = (distMatrix_std_fun.begin())->size();
    int freedom = rowNum_fun - 1;
    for (int i = 0; i < colNum_fun; ++i)
    {
        for (int j = i+1; j < colNum_fun; ++j)
        {
            double pearsonCorr = 0.0;
            for (int m = 0; m < rowNum_fun; ++m)
            {
                pearsonCorr += distMatrix_std_fun[i][m] * distMatrix_std_fun[j][m];
            }
            pearsonCorr /= freedom;

            //if (pearsonCorr >= pos_thres || pearsonCorr <= neg_thres)

            if (j==colNum_fun-1)
            {
                xyCorr.push_back(pearsonCorr);
            }
            else if (pearsonCorr >= thres_fun || pearsonCorr <= -thres_fun)
            {
                //cout<<pearsonCorr<<endl;
                corrMap_fun[make_pair(i, j)] = pearsonCorr;
            }
        }
    }

    //string corrFile = "corrFile";
    //writer_corrFile(corrMap_fun, corrFile);

    for (map< pair<int, int>, double >::const_iterator sigIt = corrMap_fun.begin(), sigIt_end = corrMap_fun.end(); sigIt != sigIt_end; ++sigIt)
    {
        int col1 = sigIt->first.first, col2 = sigIt->first.second;
        pair<int, int>& oriCol1 = oriCoor_fun[col1], oriCol2 = oriCoor_fun[col2];
        cout<<testIndex_fun<<"\t"<<col1<<"\t"<<col2<<"\t"<<oriCol1.first<<"\t"<<oriCol1.second<<"\t"<<xyCorr[col1]<<"\t"<<oriCol2.first<<"\t"<<oriCol2.second<<"\t"<<xyCorr[col2]<<endl;

        if(col1==testIndex_fun)
        {
            //if within the 3*3 square centered on the testing spot, there is a spot with higher freq than the testing spot, and this high freq spot has high corr with the testing spot, then skip the testing spot, since distMatrix_std_fun[colNum_fun-1][col1] is not the frequency of col1 (need the origin col1), use corr instead
            if(abs(oriCol1.first-oriCol2.first)>1 || abs(oriCol1.second-oriCol2.second)>1 || xyCorr[col1]>=xyCorr[col2])
            {
                rmIndex_fun[col2] = 1;
            }
            else
            {
                return 0;
            }
        }
        else if(col2==testIndex_fun)
        {
            if(abs(oriCol1.first-oriCol2.first)>1 || abs(oriCol1.second-oriCol2.second)>1 || xyCorr[col2]>=xyCorr[col1])
            {
                rmIndex_fun[col1] = 1;
            }
            else
            {
                return 0;
            }
        }
        else
        {
            //always keep the one with higher positive correlation
            //cout<<"break 1"<<endl;
            rmIndex_fun[xyCorr[col1]>xyCorr[col2]?col2:col1] = 1;
            //cout<<"break 2"<<endl;
        }
    }

    return 1;
}

int writer_corrFile(map< pair<int, int>, double >& corrMap_local, string& fileName)
{
    ofstream outputFile(fileName.c_str());

    for (map< pair<int, int>, double>::const_iterator corrIt = corrMap_local.begin(), corrIt_end = corrMap_local.end(); corrIt != corrIt_end; ++corrIt)
    {
        outputFile<<corrIt->first.first<<"\t"<<corrIt->first.second<<"\t"<<corrIt->second<<endl;
    }
    outputFile.close();

    return 0;
}

#endif


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "opengl-precomp.h"	 // Precompiled header
//
#include <mrpt/containers/yaml.h>
#include <mrpt/math/geometry.h>	 // crossProduct3D()
#include <mrpt/math/ops_containers.h>  // dotProduct()
#include <mrpt/opengl/TRenderMatrices.h>

#include <Eigen/Dense>

using namespace mrpt::opengl;

void TRenderMatrices::computeOrthoProjectionMatrix(
	float left, float right, float bottom, float top, float znear, float zfar)
{
	ASSERT_GT_(zfar, znear);
	m_last_z_near = znear;
	m_last_z_far = zfar;

	p_matrix.setIdentity();

	p_matrix(0, 0) = 2.0f / (right - left);
	p_matrix(1, 1) = 2.0f / (top - bottom);
	p_matrix(2, 2) = -2.0f / (zfar - znear);
	p_matrix(0, 3) = -(right + left) / (right - left);
	p_matrix(1, 3) = -(top + bottom) / (top - bottom);
	p_matrix(2, 3) = -(zfar + znear) / (zfar - znear);
}

// Replacement for obsolete: gluPerspective() and glOrtho()
void TRenderMatrices::computeProjectionMatrix(float znear, float zfar)
{
	ASSERT_GT_(FOV, .0f);
	ASSERT_GT_(zfar, znear);
	ASSERT_GT_(zfar, .0f);
	ASSERT_GE_(znear, .0f);

	m_last_z_near = znear;
	m_last_z_far = zfar;

	if (pinhole_model.has_value())
	{
		const auto& phm = pinhole_model.value();

		// Equivalent to gluPerspective(), from pinhole camera intrinsic
		// parameters (cx,cy,fx,fy):
		ASSERT_EQUAL_(viewport_width, pinhole_model->ncols);
		ASSERT_EQUAL_(viewport_height, pinhole_model->nrows);

		const int W = pinhole_model->ncols, H = pinhole_model->nrows;

		// See: e.g.
		// http://ksimek.github.io/2013/06/03/calibrated_cameras_in_opengl/
		mrpt::math::CMatrixFloat44 persp;
		persp.setZero();

		persp(0, 0) = phm.fx();
		persp(1, 1) = phm.fy();

		persp(0, 2) = -phm.cx();
		persp(1, 2) = -H + phm.cy();
		persp(2, 2) = (zfar + znear);
		persp(3, 2) = -1.0f;
		persp(2, 3) = zfar * znear;

		// glOrtho(-W/2, W/2, -H/2, H/2, near, far);

		computeOrthoProjectionMatrix(
			0, W, 0 /*bottom*/, H /*top*/, znear, zfar);

		// glMultMatrix(persp);
		p_matrix.asEigen() *= persp.asEigen();
	}
	else if (is_projective)
	{
		// Was: gluPerspective()
		// Based on GLM's perspective (MIT license).

		const float aspect = viewport_width / (1.0f * viewport_height);
		ASSERT_GT_(
			std::abs(aspect - std::numeric_limits<float>::epsilon()), .0f);

		const float f = 1.0f / std::tan(mrpt::DEG2RAD(FOV) / 2.0f);
		p_matrix.setZero();

		p_matrix(0, 0) = f / aspect;
		p_matrix(1, 1) = f;
		p_matrix(2, 2) = -(zfar + znear) / (zfar - znear);
		p_matrix(3, 2) = -1.0f;
		p_matrix(2, 3) = -(2.0f * zfar * znear) / (zfar - znear);
	}
	else
	{
		// Was:
		// glOrtho(-Ax, Ax, -Ay, Ay, -0.5 * m_clip_max, 0.5 * m_clip_max);

		const float ratio = viewport_width / (1.0f * viewport_height);
		float Ax = eyeDistance * 0.5f;
		float Ay = eyeDistance * 0.5f;

		if (ratio > 1) Ax *= ratio;
		else
		{
			if (ratio != 0) Ay /= ratio;
		}

		const auto left = -.5f * Ax, right = .5f * Ax;
		const auto bottom = -.5f * Ay, top = .5f * Ay;
		computeOrthoProjectionMatrix(left, right, bottom, top, znear, zfar);
	}
}

// Replacement for deprecated OpenGL gluLookAt():
void TRenderMatrices::applyLookAt()
{
	using mrpt::math::TVector3D;

	// Note: Use double instead of float to avoid numerical innacuracies that
	// are really noticeable with the naked eye when elevation is close to 90
	// deg (!)
	TVector3D forward = TVector3D(pointing - eye);
	const double fn = forward.norm();
	ASSERT_(fn != 0);
	forward *= 1.0 / fn;

	// Side = forward x up
	TVector3D side = mrpt::math::crossProduct3D(forward, up);
	const double sn = side.norm();
	ASSERT_(sn != 0);
	side *= 1.0 / sn;

	// Recompute up as: up = side x forward
	const TVector3D up2 = mrpt::math::crossProduct3D(side, forward);

	//  s.x   s.y   s.z  -dot(s, eye)
	//  u.x   u.y   u.z  -dot(u, eye)
	// -f.x  -f.y  -f.z  dot(up, eye)
	//   0     0     0      1

	mrpt::math::CMatrixFloat44 m(mrpt::math::UNINITIALIZED_MATRIX);
	// Axis X:
	m(0, 0) = d2f(side[0]);
	m(0, 1) = d2f(side[1]);
	m(0, 2) = d2f(side[2]);
	// Axis Y:
	m(1, 0) = d2f(up2[0]);
	m(1, 1) = d2f(up2[1]);
	m(1, 2) = d2f(up2[2]);
	// Axis Z:
	m(2, 0) = d2f(-forward[0]);
	m(2, 1) = d2f(-forward[1]);
	m(2, 2) = d2f(-forward[2]);
	// Translation:
	m(0, 3) = d2f(-mrpt::math::dotProduct<3, double>(side, eye));
	m(1, 3) = d2f(-mrpt::math::dotProduct<3, double>(up2, eye));
	m(2, 3) = d2f(mrpt::math::dotProduct<3, double>(forward, eye));
	// Last row:
	m(3, 0) = .0f;
	m(3, 1) = .0f;
	m(3, 2) = .0f;
	m(3, 3) = 1.f;

	// Homogeneous matrices composition:
	// Overwrite projection matrix:
	p_matrix.asEigen() = p_matrix.asEigen() * m.asEigen();
}

void TRenderMatrices::projectPoint(
	float x, float y, float z, float& proj_u, float& proj_v,
	float& proj_z_depth) const
{
	const Eigen::Matrix<float, 4, 1, Eigen::ColMajor> proj =
		pmv_matrix.asEigen() *
		Eigen::Matrix<float, 4, 1, Eigen::ColMajor>(x, y, z, 1);
	proj_u = proj[3] ? proj[0] / proj[3] : 0;
	proj_v = proj[3] ? proj[1] / proj[3] : 0;
	proj_z_depth = proj[2];
}

void TRenderMatrices::projectPointPixels(
	float x, float y, float z, float& proj_u_px, float& proj_v_px,
	float& proj_depth) const
{
	projectPoint(x, y, z, proj_u_px, proj_v_px, proj_depth);
	proj_u_px = (proj_u_px + 1.0f) * (viewport_width * 0.5f);
	proj_v_px = (proj_v_px + 1.0f) * (viewport_height * 0.5f);
}

void TRenderMatrices::saveToYaml(mrpt::containers::yaml& c) const
{
	c = mrpt::containers::yaml::Map();

	MCP_SAVE(c, initialized);
	MCP_SAVE(c, viewport_width);
	MCP_SAVE(c, viewport_height);
	MCP_SAVE(c, FOV);
	MCP_SAVE_DEG(c, azimuth);
	MCP_SAVE_DEG(c, elev);
	MCP_SAVE(c, eyeDistance);
	MCP_SAVE(c, is_projective);

	c["eye"] = eye.asString();
	c["pointing"] = pointing.asString();
	c["up"] = up.asString();

	c["p_matrix"] = mrpt::containers::yaml::FromMatrix(p_matrix);
	c["mv_matrix"] = mrpt::containers::yaml::FromMatrix(mv_matrix);
}

void TRenderMatrices::print(std::ostream& o) const
{
	mrpt::containers::yaml c;
	saveToYaml(c);
	o << c;
}


//
// Copyright (c) 2018 CNRS
//

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/algorithm/regressor.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_static_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

  VectorXd q = randomConfiguration(model);
  computeStaticRegressor(model,data,q);

  VectorXd phi(4*(model.njoints-1));
  for(int k = 1; k < model.njoints; ++k)
  {
    const Inertia & Y = model.inertias[(size_t)k];
    phi.segment<4>(4*(k-1)) << Y.mass(), Y.mass() * Y.lever();
  }

  Vector3d com = centerOfMass(model,data_ref,q);
  Vector3d static_com_ref;
  static_com_ref <<  com;

  Vector3d static_com = data.staticRegressor * phi;

  BOOST_CHECK(static_com.isApprox(static_com_ref));
}

BOOST_AUTO_TEST_CASE(test_body_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  Inertia I(Inertia::Random());
  Motion v(Motion::Random());
  Motion a(Motion::Random());

  Force f = I*a + I.vxiv(v);

  Inertia::Vector6 f_regressor = bodyRegressor(v,a) * I.toDynamicParameters();

  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
}

BOOST_AUTO_TEST_CASE(test_joint_body_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  buildModels::manipulator(model);
  pinocchio::Data data(model);

  JointIndex JOINT_ID = JointIndex(model.njoints) - 1;

  VectorXd q = randomConfiguration(model);
  VectorXd v = Eigen::VectorXd::Random(model.nv);
  VectorXd a = Eigen::VectorXd::Random(model.nv);

  rnea(model,data,q,v,a);

  Force f = data.f[JOINT_ID];

  Inertia::Vector6 f_regressor = jointBodyRegressor(model,data,JOINT_ID) * model.inertias[JOINT_ID].toDynamicParameters();

  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
}

BOOST_AUTO_TEST_CASE(test_frame_body_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  buildModels::manipulator(model);

  JointIndex JOINT_ID = JointIndex(model.njoints) - 1;

  const SE3 & framePlacement = SE3::Random();
  FrameIndex FRAME_ID = (FrameIndex) model.addBodyFrame ("test_body", JOINT_ID, framePlacement, -1);

  pinocchio::Data data(model);

  VectorXd q = randomConfiguration(model);
  VectorXd v = Eigen::VectorXd::Random(model.nv);
  VectorXd a = Eigen::VectorXd::Random(model.nv);

  rnea(model,data,q,v,a);

  Force f = framePlacement.actInv(data.f[JOINT_ID]);
  Inertia I = framePlacement.actInv(model.inertias[JOINT_ID]);

  Inertia::Vector6 f_regressor = frameBodyRegressor(model,data,FRAME_ID) * I.toDynamicParameters();

  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
}

BOOST_AUTO_TEST_CASE(test_joint_torque_regressor)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  VectorXd q = randomConfiguration(model);
  VectorXd v = Eigen::VectorXd::Random(model.nv);
  VectorXd a = Eigen::VectorXd::Random(model.nv);

  rnea(model,data_ref,q,v,a);

  Eigen::VectorXd params(10*(model.njoints-1));
  for(JointIndex i=1; i<(Model::JointIndex)model.njoints; ++i)
      params.segment<10>((int)((i-1)*10)) = model.inertias[i].toDynamicParameters();

  computeJointTorqueRegressor(model,data,q,v,a);

  Eigen::VectorXd tau_regressor = data.jointTorqueRegressor * params;

  BOOST_CHECK(tau_regressor.isApprox(data_ref.tau));
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Eigen/Dense>
#include <cstdint>
//#include <std_msgs/Float64.h> TODO use in place of double

namespace sel_map::mesh {
    typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowArray_t;
    typedef Eigen::Array<double, Eigen::Dynamic, 4, Eigen::RowMajor> PointWithCovArray_t;
    typedef Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor> PointArray_t;
    typedef Eigen::Array<uint32_t, Eigen::Dynamic, 3, Eigen::RowMajor> IndexArray_t;
    static const int AllPoints = -1;
    static const int NoRadius = -1;
    constexpr static const double defaultOrigin[2] = {0, 0};
    static const RowArray_t NoPoints;
}


#include "SchemeAlgo.h"

#include <NTL/BasicThreadPool.h>
#include <NTL/ZZ.h>
#include <cmath>
#include <map>

#include "CZZ.h"
#include "EvaluatorUtils.h"
#include "Message.h"
#include "Params.h"
#include "SchemeAux.h"
#include "SecKey.h"

Cipher* SchemeAlgo::encryptSingleArray(CZZ*& vals, long size) {
	Cipher* res = new Cipher[size];
	for (long i = 0; i < size; ++i) {
		res[i] = scheme.encryptSingle(vals[i]);
	}
	return res;
}

CZZ* SchemeAlgo::decryptSingleArray(SecKey& secretKey, Cipher*& ciphers, long size) {
	CZZ* res = new CZZ[size];
	for (int i = 0; i < size; ++i) {
		Message msg = scheme.decryptMsg(secretKey, ciphers[i]);
		CZZ* gvals = scheme.decode(msg);
		res[i] = gvals[0];
	}
	return res;
}

Cipher SchemeAlgo::powerOf2(Cipher& cipher, const long precisionBits, const long logDegree) {
	Cipher res = cipher;
	for (long i = 0; i < logDegree; ++i) {
		scheme.squareAndEqual(res);
		scheme.modSwitchAndEqual(res, precisionBits);
	}
	return res;
}

Cipher* SchemeAlgo::powerOf2Extended(Cipher& cipher, const long precisionBits, const long logDegree) {
	Cipher* res = new Cipher[logDegree + 1];
	res[0] = cipher;
	for (long i = 1; i < logDegree + 1; ++i) {
		res[i] = scheme.square(res[i-1]);
		scheme.modSwitchAndEqual(res[i], precisionBits);
	}
	return res;
}

//-----------------------------------------

Cipher SchemeAlgo::power(Cipher& cipher, const long precisionBits, const long degree) {
	long logDegree = log2(degree);
	long po2Degree = 1 << logDegree;

	Cipher res = powerOf2(cipher, precisionBits, logDegree);
	long remDegree = degree - po2Degree;
	if(remDegree > 0) {
		Cipher tmp = power(cipher, precisionBits, remDegree);
		long bitsDown = tmp.cbits - res.cbits;
		scheme.modEmbedAndEqual(tmp, bitsDown);
		scheme.multAndEqual(res, tmp);
		scheme.modSwitchAndEqual(res, precisionBits);
	}
	return res;
}

Cipher* SchemeAlgo::powerExtended(Cipher& cipher, const long precisionBits, const long degree) {
	Cipher* res = new Cipher[degree];
	long logDegree = log2(degree);
	Cipher* cpows = powerOf2Extended(cipher, precisionBits, logDegree);
	long idx = 0;
	for (long i = 0; i < logDegree; ++i) {
		long powi = (1 << i);
		res[idx++] = cpows[i];
		for (int j = 0; j < powi-1; ++j) {
			long bitsDown = res[j].cbits - cpows[i].cbits;
			res[idx] = scheme.modEmbed(res[j], bitsDown);
			scheme.multAndEqual(res[idx], cpows[i]);
			scheme.modSwitchAndEqual(res[idx++], precisionBits);
		}
	}
	res[idx++] = cpows[logDegree];
	long degree2 = (1 << logDegree);
	for (int i = 0; i < (degree - degree2); ++i) {
		long bitsDown = res[i].cbits - cpows[logDegree].cbits;
		res[idx] = scheme.modEmbed(res[i], bitsDown);
		scheme.multAndEqual(res[idx], cpows[logDegree]);
		scheme.modSwitchAndEqual(res[idx++], precisionBits);
	}
	return res;
}

//-----------------------------------------

Cipher SchemeAlgo::prodOfPo2(Cipher*& ciphers, const long precisionBits, const long logDegree) {
	Cipher* res = ciphers;
	for (long i = logDegree - 1; i >= 0; --i) {
		long powih = (1 << i);
		Cipher* tmp = new Cipher[powih];
		NTL_EXEC_RANGE(powih, first, last);
		for (long j = first; j < last; ++j) {
			tmp[j] = scheme.mult(res[2 * j], res[2 * j + 1]);
			scheme.modSwitchAndEqual(tmp[j], precisionBits);
		}
		NTL_EXEC_RANGE_END;
		res = tmp;
	}
	return res[0];
}

Cipher SchemeAlgo::prod(Cipher*& ciphers, const long precisionBits, const long degree) {
	long logDegree = log2(degree) + 1;
	long idx = 0;
	bool isinit = false;
	Cipher res;
	for (long i = 0; i < logDegree; ++i) {
		if(bit(degree, i)) {
			long powi = (1 << i);
			Cipher* tmp = new Cipher[powi];
			for (long j = 0; j < powi; ++j) {
				tmp[j] = ciphers[idx + j];
			}
			Cipher iprod = prodOfPo2(tmp, precisionBits, i);
			if(isinit) {
				long bitsDown = res.cbits - iprod.cbits;
				scheme.modEmbedAndEqual(res, bitsDown);
				scheme.multAndEqual(res, iprod);
				scheme.modSwitchAndEqual(res, precisionBits);
			} else {
				res = iprod;
				isinit = true;
			}
			idx += powi;
		}
	}
	return res;
}

Cipher SchemeAlgo::sum(Cipher*& ciphers, const long size) {
	Cipher res = ciphers[0];
	for (long i = 1; i < size; ++i) {
		scheme.addAndEqual(res, ciphers[i]);
	}
	return res;
}

Cipher SchemeAlgo::distance(Cipher& cipher1, Cipher& cipher2, const long precisionBits) {
	Cipher cres = scheme.sub(cipher1, cipher2);
	scheme.squareAndEqual(cres);
	scheme.modSwitchAndEqual(cres, precisionBits);
	partialSlotsSumAndEqual(cres, cres.slots);
	return cres;
}

Cipher* SchemeAlgo::multVec(Cipher*& ciphers1, Cipher*& ciphers2, const long size) {
	Cipher* res = new Cipher[size];
	NTL_EXEC_RANGE(size, first, last);
	for (long i = first; i < last; ++i) {
		res[i] = scheme.mult(ciphers1[i], ciphers2[i]);
	}
	NTL_EXEC_RANGE_END;
	return res;
}

void SchemeAlgo::multAndEqualVec(Cipher*& ciphers1, Cipher*& ciphers2, const long size) {
	NTL_EXEC_RANGE(size, first, last);
	for (long i = first; i < last; ++i) {
		scheme.multAndEqual(ciphers1[i], ciphers2[i]);
	}
	NTL_EXEC_RANGE_END;
}

Cipher* SchemeAlgo::multAndModSwitchVec(Cipher*& ciphers1, Cipher*& ciphers2, const long precisionBits, const long size) {
	Cipher* res = new Cipher[size];
	NTL_EXEC_RANGE(size, first, last);
	for (long i = first; i < last; ++i) {
		res[i] = scheme.mult(ciphers1[i], ciphers2[i]);
		scheme.modSwitchAndEqual(res[i], precisionBits);
	}
	NTL_EXEC_RANGE_END;
	return res;
}

void SchemeAlgo::multModSwitchAndEqualVec(Cipher*& ciphers1, Cipher*& ciphers2, const long precisionBits, const long size) {
	NTL_EXEC_RANGE(size, first, last);
	for (long i = first; i < last; ++i) {
		scheme.multAndEqual(ciphers1[i], ciphers2[i]);
		scheme.modSwitchAndEqual(ciphers1[i], precisionBits);
	}
	NTL_EXEC_RANGE_END;
}

Cipher SchemeAlgo::innerProd(Cipher*& ciphers1, Cipher*& ciphers2, const long precisionBits, const long size) {
	Cipher cip = scheme.mult(ciphers1[size-1], ciphers2[size-1]);

	NTL_EXEC_RANGE(size-1, first, last);
	for (long i = first; i < last; ++i) {
		Cipher cprodi = scheme.mult(ciphers1[i], ciphers2[i]);
		scheme.addAndEqual(cip, cprodi);
	}
	NTL_EXEC_RANGE_END;

	scheme.modSwitchAndEqual(cip, precisionBits);
	return cip;
}

Cipher SchemeAlgo::partialSlotsSum(Cipher& cipher, const long slots) {
	long logslots = log2(slots);
	Cipher res = cipher;
	for (long i = 0; i < logslots; ++i) {
		Cipher rot = scheme.leftRotateByPo2(cipher, i);
		scheme.addAndEqual(res, rot);
	}
	return res;
}

void SchemeAlgo::partialSlotsSumAndEqual(Cipher& cipher, const long slots) {
	long logslots = log2(slots);
	for (long i = 0; i < logslots; ++i) {
		Cipher rot = scheme.leftRotateByPo2(cipher, i);
		scheme.addAndEqual(cipher, rot);
	}
}

//-----------------------------------------

Cipher SchemeAlgo::inverse(Cipher& cipher, const long precisionBits, const long steps) {
	ZZ precision = power2_ZZ(precisionBits);
	Cipher cpow = cipher;
	Cipher tmp = scheme.addConst(cipher, precision);
	scheme.modEmbedAndEqual(tmp, precisionBits);
	Cipher res = tmp;

	for (long i = 1; i < steps; ++i) {
		scheme.squareAndEqual(cpow);
		scheme.modSwitchAndEqual(cpow, precisionBits);
		tmp = cpow;
		scheme.addConstAndEqual(tmp, precision);
		scheme.multAndEqual(tmp, res);
		scheme.modSwitchAndEqual(tmp, precisionBits);
		res = tmp;
	}
	return res;
}

Cipher* SchemeAlgo::inverseExtended(Cipher& cipher, const long precisionBits, const long steps) {
	ZZ precision = power2_ZZ(precisionBits);

	Cipher* res = new Cipher[steps];
	Cipher cpow = cipher;
	Cipher tmp = scheme.addConst(cipher, precision);
	scheme.modEmbedAndEqual(tmp, precisionBits);
	res[0] = tmp;

	for (long i = 1; i < steps; ++i) {
		scheme.squareAndEqual(cpow);
		scheme.modSwitchAndEqual(cpow, precisionBits);
		tmp = cpow;
		scheme.addConstAndEqual(tmp, precision);
		scheme.multAndEqual(tmp, res[i - 1]);
		scheme.modSwitchAndEqual(tmp, precisionBits);
		res[i] = tmp;
	}
	return res;
}

//-----------------------------------------

Cipher SchemeAlgo::function(Cipher& cipher, string& funcName, const long precisionBits, const long degree) {
	Cipher* cpows = powerExtended(cipher, precisionBits, degree);

	long dprecisionBits = 2 * precisionBits;

	double* coeffs = scheme.aux.taylorCoeffsMap.at(funcName);

	ZZ tmp = EvaluatorUtils::evaluateVal(coeffs[1], precisionBits);
	Cipher res = scheme.multByConst(cpows[0], tmp);

	tmp = EvaluatorUtils::evaluateVal(coeffs[0], dprecisionBits);
	scheme.addConstAndEqual(res, tmp);

	for (int i = 1; i < degree; ++i) {
		if(abs(coeffs[i + 1]) > 1e-27) {
			tmp = EvaluatorUtils::evaluateVal(coeffs[i + 1], precisionBits);
			Cipher aixi = scheme.multByConst(cpows[i], tmp);
			long bitsDown = res.cbits - aixi.cbits;
			scheme.modEmbedAndEqual(res, bitsDown);
			scheme.addAndEqual(res, aixi);
		}
	}
	scheme.modSwitchAndEqual(res, precisionBits);
	return res;
}

Cipher SchemeAlgo::functionLazy(Cipher& cipher, string& funcName, const long precisionBits, const long degree) {
	Cipher* cpows = powerExtended(cipher, precisionBits, degree);

	long dprecisionBits = 2 * precisionBits;

	double* coeffs = scheme.aux.taylorCoeffsMap.at(funcName);

	ZZ tmp = EvaluatorUtils::evaluateVal(coeffs[1], precisionBits);
	Cipher res = scheme.multByConst(cpows[0], tmp);

	tmp = EvaluatorUtils::evaluateVal(coeffs[0], dprecisionBits);
	scheme.addConstAndEqual(res, tmp);

	for (int i = 1; i < degree; ++i) {
		if(abs(coeffs[i + 1]) > 1e-27) {
			tmp = EvaluatorUtils::evaluateVal(coeffs[i + 1], precisionBits);
			Cipher aixi = scheme.multByConst(cpows[i], tmp);
			long bitsDown = res.cbits - aixi.cbits;
			scheme.modEmbedAndEqual(res, bitsDown);
			scheme.addAndEqual(res, aixi);
		}
	}
	return res;
}

Cipher* SchemeAlgo::functionExtended(Cipher& cipher, string& funcName, const long precisionBits, const long degree) {
	Cipher* cpows = powerExtended(cipher, precisionBits, degree);

	long dprecisionBits = 2 * precisionBits;
	double* coeffs = scheme.aux.taylorCoeffsMap.at(funcName);

	ZZ tmp = EvaluatorUtils::evaluateVal(coeffs[1], precisionBits);
	Cipher aixi = scheme.multByConst(cpows[0], tmp);

	tmp = EvaluatorUtils::evaluateVal(coeffs[0], dprecisionBits);
	scheme.addConstAndEqual(aixi, tmp);

	Cipher* res = new Cipher[degree];
	res[0] = aixi;
	for (long i = 1; i < degree; ++i) {
		if(abs(coeffs[i + 1]) > 1e-27) {
			tmp = EvaluatorUtils::evaluateVal(coeffs[i + 1], precisionBits);
			aixi = scheme.multByConst(cpows[i], tmp);
			long bitsDown = res[i - 1].cbits - aixi.cbits;
			Cipher ctmp = scheme.modEmbed(res[i - 1], bitsDown);
			scheme.addAndEqual(aixi, ctmp);
			res[i] = aixi;
		} else {
			res[i] = res[i - 1];
		}
	}
	NTL_EXEC_RANGE(degree, first, last);
	for (long i = first; i < last; ++i) {
		scheme.modSwitchAndEqual(res[i], precisionBits);
	}
	NTL_EXEC_RANGE_END;
	return res;
}

void SchemeAlgo::fftRaw(Cipher*& ciphers, const long size, const bool isForward) {
	for (long i = 1, j = 0; i < size; ++i) {
		long bit = size >> 1;
		for (; j >= bit; bit>>=1) {
			j -= bit;
		}
		j += bit;
		if(i < j) {
			swap(ciphers[i], ciphers[j]);
		}
	}

	for (long len = 2; len <= size; len <<= 1) {
		long shift = isForward ? ((scheme.params.N / len) << 1) : ((scheme.params.N - scheme.params.N / len) << 1);
		for (long i = 0; i < size; i += len) {
			NTL_EXEC_RANGE(len / 2, first, last);
			for (long j = first; j < last; ++j) {
				Cipher u = ciphers[i + j];
				scheme.multByMonomialAndEqual(ciphers[i + j + len / 2], shift * j);
				scheme.addAndEqual(ciphers[i + j], ciphers[i + j + len / 2]);
				scheme.subAndEqual2(u, ciphers[i + j + len / 2]);
			}
			NTL_EXEC_RANGE_END;
		}
	}
}

void SchemeAlgo::fft(Cipher*& ciphers, const long size) {
	fftRaw(ciphers, size, true);
}

void SchemeAlgo::fftInv(Cipher*& ciphers, const long size) {
	fftRaw(ciphers, size, false);
	long logsize = log2(size);

	NTL_EXEC_RANGE(size, first, last);
	for (long i = first; i < last; ++i) {
		scheme.modSwitchAndEqual(ciphers[i], logsize);
	}
	NTL_EXEC_RANGE_END;
}

void SchemeAlgo::fftInvLazy(Cipher*& ciphers, const long size) {
	return fftRaw(ciphers, size, false);
}