// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/laplacian_setup.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>

using namespace std;

template <typename MatrixA>
void test(MatrixA& A, unsigned dim1, unsigned dim2, const char* name)
{
    const unsigned max_print_size= 25;
    cout << "\n" << name << "\n";
    laplacian_setup(A, dim1, dim2);

    unsigned size= dim1 * dim2;
    mtl::dense_vector<double> v(size);
    for (unsigned i= 0; i < num_cols(A); i++)
	v[i]= A[12][i];

    // Resulting vector has same value type as matrix
    typedef typename mtl::Collection<MatrixA>::value_type rvalue_type;
    mtl::dense_vector<rvalue_type> w(size), w2;

    w= A * v;
    //mult(A, v, w);

    if (size <= max_print_size)
	cout << "A= \n" << A << "\n\nv= " << v << "\n\nA*v= " << w << "\n";

    // Same test as in matrix product: resulting vector corresponds to column 12
    // Check for stencil below in the middle of the matrix
    //        1
    //     2 -8  2
    //  1 -8 20 -8  1
    //     2 -8  2
    //        1
    if (dim1 == 5 && dim2 == 5) {
	rvalue_type twenty(20.0), two(2.0), one(1.0), zero(0.0), minus_eight(-8.0);
	MTL_THROW_IF(w[12] != twenty, mtl::runtime_error("wrong diagonal"));
	MTL_THROW_IF(w[13] != minus_eight, mtl::runtime_error("wrong east neighbor"));
	MTL_THROW_IF(w[14] != one, mtl::runtime_error("wrong east east neighbor"));
	MTL_THROW_IF(w[15] != zero, mtl::runtime_error("wrong zero-element"));
	MTL_THROW_IF(w[17] != minus_eight, mtl::runtime_error("wrong south neighbor"));
	MTL_THROW_IF(w[18] != two, mtl::runtime_error("wrong south east neighbor"));
	MTL_THROW_IF(w[22] != one, mtl::runtime_error("wrong south south neighbor"));
    }

    w+= A * v;

    if (size <= max_print_size)
	cout << "w+= A*v= \n\n" << w << "\n";

    // Check for stencil, must be doubled now
    if (dim1 == 5 && dim2 == 5) {
	rvalue_type forty(40.0), four(4.0);
	MTL_THROW_IF(w[12] != forty, mtl::runtime_error("wrong diagonal"));
	MTL_THROW_IF(w[18] != four, mtl::runtime_error("wrong south east neighbor"));
    }

    w-= A * v;

    if (size <= max_print_size)
	cout << "w-= A*v= \n\n" << w << "\n";

    // Check for stencil, must be A*v now
    if (dim1 == 5 && dim2 == 5) {
	rvalue_type twenty(20.0), two(2.0);
	MTL_THROW_IF(w[12] != twenty, mtl::runtime_error("wrong diagonal"));
	MTL_THROW_IF(w[18] != two, mtl::runtime_error("wrong south east neighbor"));
    }

#if 0
    rvalue_type dotexp= dot(w, v), dotres;
    mtl::with_dot(w2, dotres)= A * v;

    w2-= w;
    if (two_norm(w2) > 0.001)
	throw "Vector result wrong in with_dot computation.";
    if (std::abs(dotres - dotexp) > 0.001)
	throw "Dot result wrong in with_dot computation.";
#endif
}

int main(int argc, char* argv[])
{
    using namespace mtl;

    unsigned dim1= 5, dim2= 5;

    if (argc > 2) {dim1= atoi(argv[1]); dim2= atoi(argv[2]);}
    unsigned size= dim1 * dim2;

    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);

    test(cr, dim1, dim2, "Row-major sparse");
#if 1
    test(cc, dim1, dim2, "Column-major sparse");

    test(dr, dim1, dim2, "Row-major dense");
    test(dc, dim1, dim2, "Column-major dense");
#endif
    return 0;
}


#ifndef DG_BASIS_HPP
#define DG_BASIS_HPP

#include <tuple>
#include <map>
#include <boost/math/special_functions/legendre.hpp>

namespace DGHydro {

  template<int nDim, int nDeg>
  class BasisFunctions {
  public:
    BasisFunctions() {
      total_number = 0;

      for (int i = 0; i <= nDeg; i++) {
        for (int j = 0; j <= nDeg*(nDim > 1); j++) {
          for (int k = 0; k <= nDeg*(nDim > 2); k++) {
            if (i + j + k <= nDeg) {
              basisMap.insert({total_number, std::make_tuple(i, j, k)});

              //std::cout << "Added " << i << " " << j << " " << k << std::endl;
              total_number++;
            }
          }
        }
      }
    };

    ~BasisFunctions(void) {};

    double operator()(int i, double x, double y, double z) {
      int a = std::get<0>(basisMap[i]);
      int b = std::get<1>(basisMap[i]);
      int c = std::get<2>(basisMap[i]);

      return boost::math::legendre_p(a, x)*boost::math::legendre_p(b, y)*boost::math::legendre_p(c, z);
    }

    double x_derivative(int i, double x, double y, double z) {
      int a = std::get<0>(basisMap[i]);
      int b = std::get<1>(basisMap[i]);
      int c = std::get<2>(basisMap[i]);

      double p_prime = a*(x*boost::math::legendre_p(a, x) -
                          boost::math::legendre_p(a - 1, x))/(x*x - 1.0);

      return p_prime*boost::math::legendre_p(b, y)*boost::math::legendre_p(c, z);
    }

    double y_derivative(int i, double x, double y, double z) {
      int a = std::get<0>(basisMap[i]);
      int b = std::get<1>(basisMap[i]);
      int c = std::get<2>(basisMap[i]);

      double p_prime = b*(y*boost::math::legendre_p(b, y) -
                          boost::math::legendre_p(b - 1, y))/(y*y - 1.0);

      return boost::math::legendre_p(a, x)*p_prime*boost::math::legendre_p(c, z);
    }

    double z_derivative(int i, double x, double y, double z) {
      int a = std::get<0>(basisMap[i]);
      int b = std::get<1>(basisMap[i]);
      int c = std::get<2>(basisMap[i]);

      double p_prime = c*(z*boost::math::legendre_p(c, z) -
                          boost::math::legendre_p(c - 1, z))/(z*z - 1.0);

      return boost::math::legendre_p(a, x)*boost::math::legendre_p(b, y)*p_prime;
    }

    int total_number;
  private:
    std::map<int, std::tuple<int, int, int>> basisMap;

  };

} // namespace DGHydro

#endif  // DG_BASIS_HPP


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/pow.hpp>
#include <boost/simd/function/std.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/eight.hpp>
#include <boost/simd/constant/third.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_positive.hpp>

STF_CASE_TPL("pow",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::pow;
  using r_t =  decltype(pow(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(pow(bs::Inf<T>(), bs::Inf<T>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Nan<T>(), bs::Nan<T>()), bs::Nan<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Minf<T>(), bs::Minf<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Inf<T>(), bs::Minf<T>()), bs::Zero<r_t>(), 0);
#endif
  STF_ULP_EQUAL(pow(T(-1),T(6)), T(1), 0);
  STF_ULP_EQUAL(pow(bs::Mone<T>(), bs::Mone<T>()), bs::Mone<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::One<T>(), bs::One<T>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Zero<T>(), bs::Zero<T>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(pow(T(-1),T(5)), T(-1), 0);
  STF_ULP_EQUAL(pow(bs::Zero<T>(), bs::One<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(pow(T(8),bs::Third<T>()), r_t(2), 0.5);
}

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

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  #ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(pow(bs::Inf<T>(),3), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Minf<T>(),3), bs::Minf<r_t>(), 0);
  STF_ULP_EQUAL(pow(bs::Nan<T>(),3), bs::Nan<r_t>(), 0);
  #endif

  STF_ULP_EQUAL(pow(bs::Two<T>(),-3), (bs::Ratio<r_t, 1, 8>()),  0);
 }

STF_CASE_TPL("powint",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::pow;
  using iT = bd::as_integer_t<T>;
  using r_t =  decltype(pow(T(), iT()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(pow(bs::Mone<T>(),3), bs::Mone<r_t>());
  STF_EQUAL(pow(bs::Mone<T>(),4), bs::One<r_t>());
  STF_EQUAL(pow(bs::One<T>(),3),  bs::One<r_t>());
  STF_EQUAL(pow(bs::Two<T>(),3),  bs::Eight<r_t>());
  STF_EQUAL(pow(bs::Zero<T>(),0), bs::One<r_t>());
  STF_EQUAL(pow(bs::Zero<T>(),3), bs::Zero<r_t>());
}

STF_CASE_TPL("pow conformity",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::pow;
  using r_t =  decltype(pow(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_ULP_EQUAL(pow(T(0), T(-1)), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(pow(-T(0), T(-1)), bs::Minf<r_t>(), 0);
  STF_ULP_EQUAL(pow(-T(0), T(-2)), bs::Inf<T>(), 0);
  STF_ULP_EQUAL(pow(T(0), T(-2)), bs::Inf<T>(), 0);
  STF_ULP_EQUAL(pow(T(0),  bs::Minf<T>()),  bs::Inf<T>(), 0);
  STF_ULP_EQUAL(pow(-T(0),  bs::Minf<T>()),  bs::Inf<T>(), 0);
  STF_ULP_EQUAL(pow(-T(1),  bs::Minf<T>()),  bs::One<T>(), 0);
  STF_ULP_EQUAL(pow(-T(1),  bs::Inf <T>()),  bs::One<T>(), 0);
  STF_ULP_EQUAL(pow(T(1), bs::Nan<T>()) ,  bs::One<T>(), 0);
  STF_ULP_EQUAL(pow(bs::Nan<T>(), T(0)) ,  bs::One<T>(), 0);
  STF_ULP_EQUAL(pow(bs::Nan<T>(), -T(0)) ,  bs::One<T>(), 0);
  STF_ULP_EQUAL(pow(T(0.5), bs::Inf<T>()),  bs::Zero<T>(), 0);
  STF_ULP_EQUAL(pow(T(2), bs::Inf<T>()),  bs::Inf<T>(), 0);
  STF_ULP_EQUAL(pow(T(0.5), bs::Minf<T>()),  bs::Inf<T>(), 0);
  STF_ULP_EQUAL(pow(T(2), bs::Minf<T>()),  bs::Zero<T>(), 0);
  STF_ULP_EQUAL(pow(-T(0.5), bs::Inf<T>()),  bs::Zero<T>(), 0);
  STF_ULP_EQUAL(pow(-T(2), bs::Inf<T>()),  bs::Inf<T>(), 0);
  STF_ULP_EQUAL(pow(-T(0.5), bs::Minf<T>()),  bs::Inf<T>(), 0);
  STF_ULP_EQUAL(pow(-T(2), bs::Minf<T>()),  bs::Zero<T>(), 0);
  STF_ULP_EQUAL(pow(bs::Minf<T>(), T(-3) ),  bs::Mzero<T>(), 0);
  STF_EXPECT(bs::is_negative(pow(bs::Minf<T>(), T(-3) )));
  STF_ULP_EQUAL(pow(bs::Minf<T>(), T(-4) ),  bs::Zero<T>(), 0);
  STF_EXPECT(bs::is_positive(pow(bs::Minf<T>(), T(-4) )));
  STF_ULP_EQUAL(pow(bs::Inf<T>(), T(4) ),  bs::Inf<T>(), 0);
  STF_ULP_EQUAL(pow(bs::Inf<T>(), T(-4) ),  bs::Zero<T>(), 0);
}


// Compile command: g++ -Wall -Wextra -std=c++17 -O2 -pthread -I/usr/include/eigen3 -I/usr/include/python3.8 -o triangular_cone_sigmax triangular_cone_sigmax.cpp -lpython3.8
#include <iostream>
#include <cmath>
#include <complex>
#include <Eigen/Dense>
#include <vector>
#include <utility>
#include <functional>
#include <thread>
#include <map>
#include <string>
#include <algorithm>
#include <ctime>
#include <iomanip>
#include <fstream>
#include <filesystem>
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;
using Eigen::MatrixXd;
using Eigen::Matrix2cd;
using Eigen::Vector2cd;
using std::vector;
using std::sqrt;
using std::cos;
using std::acos;
using std::sin;
using std::atan2;
using namespace std::complex_literals;

typedef std::complex<double> cd;
typedef vector<vector<vector<cd>>> grid_t;
typedef std::function<Matrix2cd(int, double, double)> gen_coin_t;

// Settings here
int num_steps = 10000;
int initialState = 0; // See main function for details
double eps = 0.01;
double dy = sqrt(3);
bool bugLambda = true; // Set to true to set Lambda = I_2
bool plotCone = false; // Set to true to plot the results in the cone coordinates
bool showVectField = true; // Set to true to show the vector field in original coordinates
int ymin = -50, ymax = 50;
int xmin = 2*ymin-2, xmax = 2*ymax+2;
int ntriangles_x = xmax-xmin+1;
int ntriangles_y = ymax-ymin+1;
int center[2] = {-xmin, -ymin};
std::string prefix;
bool sigmax_pattern[11];
vector<std::string> initialStateName = {"square", "shiftright", "shiftdl", "center", "bothsides", "almostcenter"};

std::time_t now = time(0);
std::tm *ltm = localtime(&now);

grid_t zerogrid() {
    return vector<vector<vector<cd>>>(ntriangles_x, vector<vector<cd>> (ntriangles_y, vector<cd> (3, 0. + 0i)));
}
grid_t nangrid() {
    return vector<vector<vector<cd>>>(ntriangles_x, vector<vector<cd>> (ntriangles_y, vector<cd> (3, std::nan("") + 0i)));
}

grid_t grid = zerogrid();

Matrix2cd H, Q;

double sumAmplitudes() {
    double ret = 0.0;
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++) {
                cd val = grid[x+center[0]][y+center[1]][side];
                ret += std::real(val*std::conj(val));
            }
    return ret;
}

void normalizeGrid() {
    double target = sumAmplitudes();
    double mul = 1/sqrt(target);
    if(target == 0)
        return;
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++)
                grid[x+center[0]][y+center[1]][side] *= mul;
}

inline int modulo(int a, int b) {
    return (a%b+b)%b;
}

void init_HQ() {
    H << 1, 1, 1, -1;
    H /= sqrt(2);
    Q << 1, -1i, 1, 1i;
    Q /= sqrt(2);
}

Matrix2cd gen_Ui(double thetai) {
    Matrix2cd ret;
    ret <<
        cos(thetai/2), sin(thetai/2),
        -sin(thetai/2), cos(thetai/2);
    return ret;
}

inline int sign(double d) {
    if(d>=0)
        return 1;
    else
        return -1;
}

inline double sq(double d) {
    return d*d;
}

inline double correct_fmod(const double a, const double b) {
    return std::fmod(std::fmod(a,b)+b, b);
}

inline double principal_measure(const double theta) {
    double ret = correct_fmod(theta, 2*M_PI);
    if(ret > M_PI)
        ret -= 2*M_PI;
    return ret;
}

Matrix2cd lamb(double rx, double ry) {
    Matrix2cd ret;
    if(bugLambda)
        ret = Matrix2cd::Identity();
    else if(rx == 0 && ry == 0) {
        ret <<
            6./5.*cos(-5.*M_PI/6.), -6./5.*sin(-5.*M_PI/6.),
            6./5.*sin(-5.*M_PI/6.), 5./5.*cos(-5.*M_PI/6.);
    }
    else {
        double r = 5./6.*sqrt(rx*rx+ry*ry);
        double thetap = atan2(ry, rx);
        double theta2 = principal_measure(thetap + 5*M_PI/6);
        double theta = 6./5.*theta2;
        double x = r*cos(theta), y = r*sin(theta);
        ret <<
            6./5.*(x/r*cos(thetap) + y/r*sin(thetap)),
            6./5.*(y/r*cos(thetap) - x/r*sin(thetap)),
            6./5.*(x/r*sin(thetap) - y/r*cos(thetap)),
            6./5.*(y/r*sin(thetap) + x/r*cos(thetap));
    }
    return ret;
}

vector<cd> l(double rx, double ry) {
    Matrix2cd lam = lamb(rx, ry);
    double sqrt3 = sqrt(3.0);
    return {lam(0,0), lam(1,0)/sqrt3, -lam(1,0)/sqrt3, lam(0,1), lam(1,1)/sqrt3, -lam(1,1)/sqrt3};
}

double gen_theta(int i, double rx, double ry) {
    return std::real(M_PI/2 + sqrt(eps)*l(rx, ry)[i]);
}

Matrix2cd gen_U(int i, double rx, double ry) {
    return gen_Ui(gen_theta(i, rx, ry));
}

Matrix2cd gen_Ubis(int i, double rx, double ry) {
    return gen_U(i+3, rx, ry);
}

Matrix2cd gen_Ustar(int i, double rx, double ry) {
    return gen_U(i, rx, ry).adjoint();
}

Matrix2cd gen_Ubisstar(int i, double rx, double ry) {
    return gen_Ustar(i+3, rx, ry);
}

grid_t shift(grid_t grid) {
    grid_t ngrid = zerogrid();
    for(int x = xmin; x <= xmax; x++) {
        for(int y = ymin; y <= ymax; y++) {
            for(int i = 0; i < 3; i++) {
                int iprec = ((i-1)%3+3)%3;
                ngrid[x+center[0]][y+center[1]][i] = grid[x+center[0]][y+center[1]][iprec];
            }
        }
    }
    return ngrid;
}

std::pair<double, double> real_coords(int iside, int x, int y, bool show = false) {
    double dec;
    if(show)
        dec = .4;
    else
        dec = .5;
    double xcoord, ycoord;
    if((x+y)%2==0) {
        if(iside == 0) {
            xcoord = x-dec;
            ycoord = (y+.5)*dy;
        }
        else if(iside == 1) {
            xcoord = x+dec;
            ycoord = (y+.5)*dy;
        }
        else {
            xcoord = x;
            ycoord = (y+.5+dec)*dy;
        }
    }
    else {
        if(iside == 0) {
            xcoord = x+dec;
            ycoord = (y+.5)*dy;
        }
        else if(iside == 1) {
            xcoord = x-dec;
            ycoord = (y+.5)*dy;
        }
        else {
            xcoord = x;
            ycoord = (y+.5-dec)*dy;
        }
    }
    return std::make_pair(sqrt(eps)*xcoord, sqrt(eps)*ycoord);
}

std::pair<double, double> cone_coords(int iside, int x, int y, bool show = false) {
    double rx, ry;
    std::tie(rx, ry) = real_coords(iside, x, y, show);
    double r = 5./6.*sqrt(rx*rx+ry*ry);
    double theta_before = atan2(ry, rx);
    double theta_rotate = principal_measure(theta_before+5*M_PI/6.);
    double theta = 6./5.*theta_rotate;
    //std::cerr << "theta " << theta_before*180/M_PI << " -> " << theta_rotate*180/M_PI << " -> " << theta*180/M_PI << std::endl;
    return std::make_pair(r*cos(theta), r*sin(theta));
}

const int DELTAS[][2] = {{-1,0}, {1,0}, {0,1}};
const int NUM_THREADS = 8;

void applyCoinsPartial(grid_t &ngrid, grid_t &grid, gen_coin_t &gen_coin, int loc_xmin, int loc_xmax, bool is_sigmax) {
    for(int x = loc_xmin; x < loc_xmax; x++) {
        for(int y = ymin; y <= ymax; y++) {
            if((x+y)%2 || (x>y && y>=0))
                continue;
            for(int iside = 0; iside < 3; iside++) {
                cd thisval = grid[x+center[0]][y+center[1]][iside];
                int xo = x + DELTAS[iside][0], yo = y + DELTAS[iside][1];
                //if(xo > xmax || xo < xmin || yo > ymax || yo < ymin || (xo > yo && yo >= 0)) { // pas de propagation aux bords
                if(xo > xmax || xo < xmin || yo > ymax || yo < ymin || (!is_sigmax && xo > yo && yo >= 0)) { // pas de propagation aux bords
                    ngrid[x+center[0]][y+center[1]][iside] = thisval;
                    continue;
                }
                else if(y == -1 && x >= 1 && iside == 2) {
                    int xo = x/2, yo = x/2;
                    ngrid[x+center[0]][y+center[1]][iside] = std::exp(1.0i * M_PI/3.) * grid[xo+center[0]][yo+center[1]][1];
                }
                else if(x == y && x >= 0 && iside == 1) {
                    int xo = 2*x+1, yo = -1;
                    ngrid[x+center[0]][y+center[1]][iside] = std::exp(-1.0i * M_PI/3.) * grid[xo+center[0]][yo+center[1]][2];
                }
                else {
                    cd otherval = grid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside];
                    Vector2cd vect;
                    vect << thisval, otherval;
                    double rx, ry;
                    std::tie(rx, ry) = real_coords(iside, x, y);
                    Matrix2cd coin = gen_coin(iside, rx, ry);
                    Vector2cd newvect = coin*vect;
                    ngrid[x+center[0]][y+center[1]][iside] = newvect(0);
                    ngrid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside] = newvect(1);
                }
            }
        }
    }
}

grid_t applyCoins(grid_t grid, gen_coin_t gen_coin, bool is_sigmax, bool multithread = true) {
    grid_t ngrid = nangrid();
    if(!multithread) {
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                if((x+y)%2 || (x>y && y>=0)) // pentagone : on enlève un bout de 60°
                    continue;
                for(int iside = 0; iside < 3; iside++) {
                    cd thisval = grid[x+center[0]][y+center[1]][iside];
                    int xo = x + DELTAS[iside][0], yo = y + DELTAS[iside][1];
                    if(xo > xmax || xo < xmin || yo > ymax || yo < ymin || (!is_sigmax && xo > yo && yo >= 0)) { // pas de propagation aux bords
                        ngrid[x+center[0]][y+center[1]][iside] = thisval;
                        continue;
                    }
                    else if(y == -1 && x >= 1 && iside == 2) {
                        int xo = x/2, yo = x/2;
                        ngrid[x+center[0]][y+center[1]][iside] = std::exp(1.0i * M_PI/3.) * grid[xo+center[0]][yo+center[1]][1];
                    }
                    else if(x == y && x >= 0 && iside == 1) {
                        int xo = 2*x+1, yo = -1;
                        ngrid[x+center[0]][y+center[1]][iside] = std::exp(-1.0i * M_PI/3.) * grid[xo+center[0]][yo+center[1]][2];
                    }
                    else {
                        cd otherval = grid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside];
                        Vector2cd vect;
                        vect << thisval, otherval;
                        double rx, ry;
                        std::tie(rx, ry) = real_coords(iside, x, y);
                        Matrix2cd coin = gen_coin(iside, rx, ry);
                        Vector2cd newvect = coin*vect;
                        ngrid[x+center[0]][y+center[1]][iside] = newvect(0);
                        ngrid[modulo(xo+center[0], ntriangles_x)][modulo(yo+center[1], ntriangles_y)][iside] = newvect(1);
                    }
                }
            }
        }
    }
    else {
        std::thread threads[NUM_THREADS];
        int delta_x = ntriangles_x/NUM_THREADS;
        for(int iThread = 0; iThread < NUM_THREADS-1; iThread++) {
            threads[iThread] = std::thread(applyCoinsPartial, std::ref(ngrid), std::ref(grid), std::ref(gen_coin), xmin+iThread*delta_x, xmin+(iThread+1)*delta_x, is_sigmax);
        }
        threads[NUM_THREADS-1] = std::thread(applyCoinsPartial, std::ref(ngrid), std::ref(grid), std::ref(gen_coin), xmin+(NUM_THREADS-1)*delta_x, xmax+1, is_sigmax);
        for(int iThread = 0; iThread < NUM_THREADS; iThread++)
            threads[iThread].join();
    }
    // We may miss some sides of 1 (tip up) triangles which are not adjacent to a valid 0 triangle
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++) {
                cd &newval = ngrid[x+center[0]][y+center[1]][side];
                if(std::isnan(std::real(newval)))
                    newval = grid[x+center[0]][y+center[1]][side];
            }
    return ngrid;
}

void plotVectorField(double minx, double maxx, double miny, double maxy, int gridstep = 20) {
    vector<double> xloc, yloc;
    vector<double> vectx, vecty;
    double dx = (maxx-minx)/gridstep, dy = (maxy-miny)/gridstep;
    for(double x = minx; x <= maxx; x += dx)
        for(double y = miny; y <= maxy; y += dy) {
            for(int i = 0; i < 2; i++) {
                xloc.push_back(x);
                yloc.push_back(y);
                Matrix2cd deform = lamb(x,y);
                vectx.push_back(std::real(deform(0,i)));
                vecty.push_back(std::real(deform(1,i)));
            }
        }
    plt::quiver(xloc, yloc, vectx, vecty, {{"pivot","tail"}, {"color", "grey"}});
}

void plot(int iGrid = -1) {
    std::cerr << "Plotting " << iGrid << std::endl;
    PyObject *fig;
    if(plotCone) {
        fig = plt::figure_size(1000,1000);
        plt::xlim(xmin*sqrt(eps), xmax*sqrt(eps));
        plt::ylim(ymin*sqrt(eps)*dy, ymax*sqrt(eps)*dy);
        plt::set_aspect_equal();
        vector<double> xlist, ylist, colorlist;
        vector<double> listvals;
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    listvals.push_back(col);
                }
            }
        }
        std::sort(listvals.rbegin(), listvals.rend());
        double maxi = (listvals[0]+listvals[1])/2;
        double threshold = maxi/10;
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    if(col > threshold) {
                        double rx, ry;
                        std::tie(rx, ry) = cone_coords(iside, x, y, true);
                        xlist.push_back(rx);
                        ylist.push_back(ry);
                        colorlist.push_back(col);
                    }
                }
            }
        }
        if(maxi == 0.0)
            maxi = 1.0;
        maxi *= 0.6;
        plt::scatter_colored(xlist, ylist, colorlist, 5, {{"cmap","gist_heat_r"}, {"vmin", "0"}, {"vmax", std::to_string(maxi)}});
    }
    else {
        fig = plt::figure_size(1000,1000);
        plt::set_aspect_equal();
        vector<vector<double>> imgrid(ymax-ymin+1, vector<double>(xmax-xmin+1, 0.0));
        vector<double> listvals;
        for(int y = ymin; y <= ymax; y++) {
            for(int x = xmin; x <= xmax; x++) {
                double sum = 0.0;
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    sum += col;
                }
                listvals.push_back(sum);
                imgrid[y+center[1]][x+center[0]] = sum;
            }
        }
        std::sort(listvals.rbegin(), listvals.rend());
        double maxi = (listvals[0]+listvals[1])/2;
        if(maxi == 0.0)
            maxi = 1.0;
        maxi *= .6;

        double minx = sqrt(eps)*(xmin-.5);
        double maxx = sqrt(eps)*(xmax+.5);
        double miny = sqrt(eps)*dy*(ymin-.5);
        double maxy = sqrt(eps)*dy*(ymax+.5);
        plt::imshow(imgrid, {minx, maxx, miny, maxy}, {{"origin", "lower"}, {"cmap", "gist_heat_r"}, {"vmin", "0.0"}, {"vmax", std::to_string(maxi)}});
        plt::plot({0.0, maxy/tan(M_PI/3)}, {0.0, maxy}, {{"color","red"}});
        plt::plot({0.0, xmax*sqrt(eps)}, {0.0, 0.0}, {{"color","red"}});
        if(showVectField)
            plotVectorField(xmin*sqrt(eps), xmax*sqrt(eps), ymin*sqrt(eps)*dy, ymax*sqrt(eps)*dy, 20);
    }
    std::ostringstream filename;
    filename << prefix << "_" << iGrid << ".png";
    plt::save(prefix + "/" + filename.str());
    plt::clf();
    plt::close(fig);
    Py_DECREF(fig);

    // Plot around dislocation line
    vector<double> xlist2, ylist2;
    for(int x = 0; x <= xmax; x++) {
        vector<int> klist;
        if((x+1)%2)
            klist = {1, 2, 0};
        else
            klist = {0, 2, 1};
        for(int k : klist) {
            double rx, ry;
            std::tie(rx, ry) = real_coords(k, x, -1);
            xlist2.push_back(rx);
            cd val = grid[x+center[0]][center[1]-1][k];
            double col = std::real(val*std::conj(val));
            ylist2.push_back(col);
        }
    }
    plt::plot(xlist2, ylist2);
    std::ostringstream filename_disloc;
    filename_disloc << prefix << "_dislocation_" << iGrid << ".png";
    plt::save(prefix + "/" + filename_disloc.str());
    plt::cla();
    plt::clf();
    plt::close();
    std::cerr << "Done plotting " << iGrid << std::endl;
}

void step_walk(int step = -1) {
    std::cerr << "Begin step " << step << std::endl;
    grid = applyCoins(grid, [](int i, double rx, double ry) {
        Matrix2cd ret = H;
        return ret;
    }, sigmax_pattern[0]);
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_Ustar, sigmax_pattern[1]);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_U(((i-1)%3+3)%3, rx, ry);
            return ret;
        }, sigmax_pattern[2]);
    }
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_U, sigmax_pattern[3]);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_Ustar(((i-1)%3+3)%3, rx, ry);
            return ret;
        }, sigmax_pattern[4]);
    }
    grid = applyCoins(grid, [](int i, double rx, double ry) {
        Matrix2cd ret = Q*H;
        return ret;
    }, sigmax_pattern[5]);
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_Ubisstar, sigmax_pattern[6]);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_Ubis(((i-1)%3+3)%3, rx, ry);
            return ret;
        }, sigmax_pattern[7]);
    }
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, gen_Ubis, sigmax_pattern[8]);
        grid = shift(grid);
        grid = applyCoins(grid, [](int i, double rx, double ry) {
            Matrix2cd ret = gen_Ubisstar(((i-1)%3+3)%3, rx, ry);
            return ret;
        }, sigmax_pattern[9]);
    }
    grid = applyCoins(grid, [](int i, double rx, double ry) {
        Matrix2cd ret = Q.adjoint();
        return ret;
    }, sigmax_pattern[10]);
    std::cerr << "Total amplitude: " << sumAmplitudes() << "\n";
    //normalizeGrid();
    //std::cerr << "After normalization: " << sumAmplitudes() << "\n";
    std::cerr << "End step " << step << std::endl;
}

void print_params() {
    std::ofstream ostream(prefix + "/settings.txt");
    ostream << "num_steps = " << num_steps << "\n";
    ostream << "eps = " << eps << "\n";
    ostream << "xmin = " << xmin << "\n";
    ostream << "xmax = " << xmax << "\n";
    ostream << "ymin = " << ymin << "\n";
    ostream << "ymax = " << ymax << "\n";
    ostream << "sigma_x pattern: ";
    for(int i = 0; i < 11; i++)
        ostream << (int)sigmax_pattern[i];
    ostream.close();
}

void listInitialStates() {
    for(size_t i = 0; i < initialStateName.size(); i++)
        std::cerr << "- " << i << ": " << initialStateName[i] << "\n";
}

int main(int argc, char **argv)
{
    if(argc <= 3) {
        std::cerr << "Usage: " << std::string(argv[0]) << " <initial state> <plot cone> <sigmax pattern>\n";
        std::cerr << "Initial states:\n";
        listInitialStates();
        std::cerr << "<plot cone> should be 1 if the figure should be plotted in cone coordinates, 0 otherwise.\n";
        std::cerr << "<sigmax pattern> should be a sequence of 11 ones and zeroes, a 1 at position i meaning that sigma_x should be applied on the dislocation line at coin i and 0 meaning that the identity should be applied instead.\n";
        return 1;
    }
    initialState = std::atoi(argv[1]);
    if(initialState < 0 || initialState >= (int)initialStateName.size()) {
        std::cerr << "Invalid initial state " << initialState << ". List of possible states:\n";
        listInitialStates();
        return 2;
    }
    plotCone = (bool) std::atoi(argv[2]);
    if(std::strlen(argv[3]) != 11) {
        std::cerr << "Invalid sigma_x pattern: mismatching length, should be 11 characters\n";
        return 3;
    }
    for(int i = 0; i < 11; i++) {
        if(argv[3][i] == '0')
            sigmax_pattern[i] = false;
        else if(argv[3][i] == '1')
            sigmax_pattern[i] = true;
        else {
            std::cerr << "Invalid sigma_x pattern: expected 0 or 1, got " << argv[3][i] << "\n";
            return 3;
        }
    }
    std::ostringstream str;
    str <<
        "simul_conesigmax_" << initialStateName[initialState];
    if(plotCone)
        str << "_conecoord_";
    else
        str << "_altcoord_";
    str <<
        std::setw(4) << std::setfill('0') << ltm->tm_year+1900 << "-" <<
        std::setw(2) << ltm->tm_mon+1 << "-" <<
        std::setw(2) << ltm->tm_mday << "_" <<
        std::setw(2) << ltm->tm_hour << "-" <<
        std::setw(2) << ltm->tm_min << "-" <<
        std::setw(2) << ltm->tm_sec;
    prefix = str.str();
    std::filesystem::create_directory(prefix);
    init_HQ();
    print_params();
    // Initial state
    if(initialState == 0) {
        for(int x = xmin/5; x <= xmax/5; x++)
            for(int y = ymin/5; y <= ymax/5; y++)
                for(int k = 0; k < 3; k++)
                    if(x <= y || y < 0)
                        grid[center[0]+x][center[1]+y][k] = 1;
        normalizeGrid();
    }
    // Shifted
    else if(initialState == 1) {
        for(int k = 0; k < 3; k++)
            grid[center[0]+xmax/2-1][center[1]-1][k]=1/sqrt(3);
    }
    // Shifted (in another way)
    else if(initialState == 2) {
        for(int k = 0; k < 3; k++)
            grid[center[0]+xmin/4+1][center[1]+ymin/4+1][k]=1/sqrt(3);
    }
    // Centered
    else if(initialState == 3) {
        vector<vector<int>> centercoord = {{-1,0},{0,0},{-1,-1},{0,-1},{1,-1}};
        for(vector<int> &coord : centercoord)
            for(int k = 0; k < 3; k++)
                grid[center[0]+coord[0]][center[1]+coord[1]][k]=1/sqrt(3*centercoord.size());
    }
    // Shifted, both sides of the line
    else if(initialState == 4) {
        for(int k=0; k<3; k++) {
            grid[center[0]+10][center[1]+10][k] = 1/sqrt(6);
            grid[center[0]+21][center[1]-1][k] = 1/sqrt(6);
        }
    }
    // Almost centered
    else if(initialState == 5) {
        for(int k = 0; k < 3; k++)
            grid[center[0]-1][center[1]-1][k] = 1/sqrt(3);
    }
    plot(0);
    for(int i = 0; i < num_steps; i++) {
        step_walk(i);
        if(((i+1)%10) == 0)
            plot(i+1);
    }
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, log1m_inv_logit) {
  using stan::math::log1m_inv_logit;
  using std::log;
  using stan::math::inv_logit;

  EXPECT_FLOAT_EQ(log(1 - inv_logit(-7.2)), log1m_inv_logit(-7.2));
  EXPECT_FLOAT_EQ(log(1 - inv_logit(0.0)), log1m_inv_logit(0.0));
  EXPECT_FLOAT_EQ(log(1 - inv_logit(1.9)), log1m_inv_logit(1.9));
}

TEST(MathFunctions, log1m_inv_logit_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::log1m_inv_logit(nan));
}