/*
 Copyright (C) 2019 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/indexes/equityindex.cpp
\brief equity index class for holding equity fixing histories and forwarding.
\ingroup indexes
*/

#include <qle/indexes/commodityindex.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

CommodityIndex::CommodityIndex(const std::string& underlyingName, const Date& expiryDate,
                               const Calendar& fixingCalendar, const Handle<QuantExt::PriceTermStructure>& curve)
    : underlyingName_(underlyingName), expiryDate_(expiryDate), fixingCalendar_(fixingCalendar), curve_(curve) {
    if (expiryDate_ == Date()) {
        // spot price index
        name_ = "COMM-" + underlyingName_;
        isFuturesIndex_ = false;
    } else {
        // futures price index
        std::ostringstream o;
        o << "COMM-" << underlyingName_ << "-" << QuantLib::io::iso_date(expiryDate_);
        name_ = o.str();
        // Remove the "-dd" portion from the expiry date
        name_.erase(name_.length() - 3);
        isFuturesIndex_ = true;
    }
    registerWith(curve_);
    registerWith(Settings::instance().evaluationDate());
    registerWith(IndexManager::instance().notifier(name()));
}

Real CommodityIndex::fixing(const Date& fixingDate, bool forecastTodaysFixing) const {
    QL_REQUIRE(isValidFixingDate(fixingDate), "Fixing date " << fixingDate << " is not valid");
    QL_REQUIRE(expiryDate_ == Date() || fixingDate <= expiryDate_, "fixing past expiry requested");
    Date today = Settings::instance().evaluationDate();

    if (fixingDate > today || (fixingDate == today && forecastTodaysFixing))
        return forecastFixing(fixingDate);

    Real result = Null<Decimal>();

    if (fixingDate < today || Settings::instance().enforcesTodaysHistoricFixings()) {
        // must have been fixed
        // do not catch exceptions
        result = pastFixing(fixingDate);
        QL_REQUIRE(result != Null<Real>(), "Missing " << name() << " fixing for " << fixingDate);
    } else {
        try {
            // might have been fixed
            result = pastFixing(fixingDate);
        } catch (Error&) {
            ; // fall through and forecast
        }
        if (result == Null<Real>())
            return forecastFixing(fixingDate);
    }

    return result;
}

Real CommodityIndex::pastFixing(const Date& fixingDate) const {
    QL_REQUIRE(isValidFixingDate(fixingDate), fixingDate << " is not a valid fixing date");
    return timeSeries()[fixingDate];
}

Real CommodityIndex::forecastFixing(const Date& fixingDate) const {
    if (isFuturesIndex_)
        return curve_->price(expiryDate_);
    else
        return curve_->price(fixingDate);
}

} // namespace QuantExt


#include <boost/python.hpp>
#include <boost/python/numpy.hpp>

#include "base_types.hpp"

namespace bp = boost::python;
namespace np = boost::python::numpy;

extern np::ndarray conebeam_reconstruct_cgls(np::ndarray pixels,
				       np::ndarray angles,
					   np::ndarray h_offsets,
					   np::ndarray v_offsets,
					   const int pixels_per_voxel,
					   const double source_x,
					   const double detector_x,
					   const double pixel_h_size,
					   const double pixel_v_size,
					   const double mask_radius,
					   const bool beam_harden,
					   np::ndarray full_vox_origin,
					   np::ndarray voxel_size,
				       int niterations,
					   int nthreads,
					   bool is_pixels_in_log);

extern np::ndarray conebeam_reconstruct_sirt(np::ndarray pixels,
				       np::ndarray angles,
					   np::ndarray h_offsets,
					   np::ndarray v_offsets,
					   const int pixels_per_voxel,
					   const double source_x,
					   const double detector_x,
					   const double pixel_h_size,
					   const double pixel_v_size,
					   const double mask_radius,
					   const bool beam_harden,
					   np::ndarray full_vox_origin,
					   np::ndarray voxel_size,
				       int niterations, int nthreads,
					   bool is_pixels_in_log);

extern np::ndarray conebeam_reconstruct_mlem(np::ndarray pixels,
				       np::ndarray angles,
					   np::ndarray h_offsets,
					   np::ndarray v_offsets,
					   const int pixels_per_voxel, const double source_x,
					   const double detector_x, const double pixel_h_size,
					   const double pixel_v_size, const double mask_radius,
					   const bool beam_harden,
					   np::ndarray full_vox_origin,
					   np::ndarray voxel_size,
				       int niterations, int nthreads,
					   bool is_pixels_in_log);

extern np::ndarray
conebeam_reconstruct_cgls_tikhonov(np::ndarray pixels,
				       np::ndarray angles,
					   np::ndarray h_offsets,
					   np::ndarray v_offsets,
					   const int pixels_per_voxel,
					   const double source_x,
					   const double detector_x,
					   const double pixel_h_size,
					   const double pixel_v_size,
					   const double mask_radius,
					   const bool beam_harden,
					   np::ndarray full_vox_origin,
					   np::ndarray voxel_size,
				       int niterations,
					   int nthreads,
					   double regularize,
					   np::ndarray norm_r,
					   bool is_pixels_in_log);

extern np::ndarray
conebeam_reconstruct_cgls_tvreg(np::ndarray pixels,
				       np::ndarray angles,
					   np::ndarray h_offsets,
					   np::ndarray v_offsets,
					   const int pixels_per_voxel,
					   const double source_x,
					   const double detector_x,
					   const double pixel_h_size,
					   const double pixel_v_size,
					   const double mask_radius,
					   const bool beam_harden,
					   np::ndarray full_vox_origin,
					   np::ndarray voxel_size,
				       int niterations,
					   int nthreads,
					   double regularize,
					   np::ndarray norm_r,
					   bool is_pixels_in_log);
extern np::ndarray conebeam_reconstruct_cgls2(np::ndarray pixels,
				       np::ndarray angles,
					   np::ndarray h_offsets,
					   np::ndarray v_offsets,
					   const int pixels_per_voxel,
					   const double source_to_sample,
					   const double source_to_detector,
					   const double pixel_h_size,
					   const double pixel_v_size,
					   const double mask_radius,
					   const bool beam_harden,
					   np::ndarray full_vox_origin,
					   np::ndarray voxel_size,
				       int niterations, int nthreads,
					   np::ndarray norm_r,
					   bool is_pixels_in_log);

extern boost::python::tuple conebeam_create_phantom();
extern boost::python::tuple conebeam_create_phantom_volume();
extern boost::python::tuple conebeam_load_xtek(const std::string& filename);

extern np::ndarray conebeam_create_sinogram(const float source_x, const float source_y, const float source_z, const float detector_x, np::ndarray ndarray_h_pixels, np::ndarray ndarray_v_pixels, np::ndarray ndarray_angles, np::ndarray ndarray_voxels,
									  np::ndarray grid_offset, np::ndarray voxel_size);


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

    @Copyright 2016 Numscale SAS

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE4_1_SIMD_FUNCTION_IF_ELSE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE4_1_SIMD_FUNCTION_IF_ELSE_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/genmask.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( if_else_
                          , (typename A0, typename A1)
                          , bs::sse4_1_
                          , bs::pack_<bs::logical_<A0>, bs::sse_>
                          , bs::pack_<bd::single_<A1>, bs::sse_>
                          , bs::pack_<bd::single_<A1>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A1 operator() ( const A0 & a0
                                    , const A1 & a1
                                    , const A1 & a2) const BOOST_NOEXCEPT
    {
      return _mm_blendv_ps(a2, a1, bitwise_cast<A1>(genmask(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( if_else_
                          , (typename A0, typename A1)
                          , bs::sse4_1_
                          , bs::pack_<bs::logical_<A0>, bs::sse_>
                          , bs::pack_<bd::double_<A1>, bs::sse_>
                          , bs::pack_<bd::double_<A1>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A1 operator() ( const A0 & a0
                                    , const A1 & a1
                                    , const A1 & a2) const BOOST_NOEXCEPT
    {
      return _mm_blendv_pd(a2, a1, bitwise_cast<A1>(genmask(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( if_else_
                          , (typename A0, typename A1)
                          , bs::sse4_1_
                          , bs::pack_<bs::logical_<A0>, bs::sse_>
                          , bs::pack_<bd::integer_<A1>, bs::sse_>
                          , bs::pack_<bd::integer_<A1>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A1 operator() ( const A0 & a0
                                    , const A1 & a1
                                    , const A1 & a2) const BOOST_NOEXCEPT
    {
      return _mm_blendv_epi8(a2, a1, bitwise_cast<A1>(genmask(a0)));
    }
  };
} } }

#endif


#include <iostream>
#include <cmath>
#include <typeinfo>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/numeric/mtl/mtl.hpp>

namespace tst {

    template <typename T>
    struct is_const
    {
	static const bool value= false;
    };

    template <typename T>
    struct is_const<const T>
    {
	static const bool value= true;
    };

    template <typename T>
    struct is_matrix
      : boost::mpl::false_ {};

    template <typename T>
    struct is_matrix<const T>
      : is_matrix<T> {};

    template <typename V, typename P>
    struct is_matrix<mtl::dense2D<V, P> >
      : boost::mpl::true_ {};

    template <bool C, typename ThenType, typename ElseType>
    struct if_c
    {
	typedef ThenType type;
    };

    template <typename ThenType, typename ElseType>
    struct if_c<false, ThenType, ElseType>
    {
	typedef ElseType type;
    };

    template <typename Cond, typename ThenType, typename ElseType>
    struct if_
      : if_c<Cond::value, ThenType, ElseType>
    {};

    template <typename Matrix>
    class transposed_view
    {
	//BOOST_STATIC_ASSERT((is_matrix<Matrix>::value)); // argument Matrix must be a known matrix
	// static_assert(is_matrix<Matrix>::value, "argument Matrix must be a known matrix");
      public:
	typedef typename mtl::Collection<Matrix>::value_type  value_type;
	typedef typename mtl::Collection<Matrix>::size_type   size_type;
	typedef typename if_<is_const<Matrix>,
			     const value_type&,
			     value_type&>::type               ref_type;

	transposed_view(Matrix& A) : ref(A) {}
#if 1
	template <typename U>
	transposed_view(typename boost::enable_if<boost::is_scalar<U>, U>::type x) { }
#endif

	ref_type operator()(size_type r, size_type c) { return ref(c, r); }
	const value_type& operator()(size_type r, size_type c) const { return ref(c, r); }

      private:
	Matrix& ref;
    };

    template <typename Matrix>
    struct is_matrix<transposed_view<Matrix> >
      : is_matrix<Matrix> {};

    template <typename Matrix>
    transposed_view<Matrix> inline trans(Matrix& A)
    {
	return transposed_view<Matrix>(A);
    }

}

template <int N>
void f()
{
    typename tst::if_c<N < 100, float, double>::type x;
    std::cout << "typeid of x = " << typeid(x).name() << '\n';
}

int main (int argc, char* argv[])
{
    typedef int T;

    mtl::dense2D<float> A(3, 3);
    A=  2, 3, 4,
	5, 6, 7,
	8, 9, 10;
    std::cout << "trans(A)(2, 0) = " << tst::trans(A)(2, 0) << '\n';
    tst::trans(A)(2, 0)= 4.5;

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

    const mtl::dense2D<float> B(A);
    std::cout << "trans(B)(2, 0) = " << tst::trans(B)(2, 0) << '\n';

    double d= 3.4;
    tst::transposed_view<double> td(d);

    return 0 ;

}


/*
Lightweight Automated Planning Toolkit
Copyright (C) 2012
Miquel Ramirez <miquel.ramirez@rmit.edu.au>
Nir Lipovetzky <nirlipo@gmail.com>

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <boost/program_options.hpp>
#include <aptk/time.hxx>
#include <strips_prob.hxx>
#include <strips_state.hxx>
#include <ff_to_aptk.hxx>
#include <fwd_search_prob.hxx>
#include <h_2.hxx>
#include <conj_comp_prob.hxx>
#include <h_C.hxx>
#include <rp_heuristic.hxx>
#include <bs_graph.hxx>

#include <iostream>
#include <fstream>

namespace po = boost::program_options;

using   aptk::STRIPS_Problem;
using	aptk::agnostic::Fwd_Search_Problem;
using   aptk::agnostic::H2_Heuristic;
using	aptk::agnostic::CC_Problem;
using	aptk::agnostic::HC_Heuristic;
using	aptk::agnostic::Relaxed_Plan_Extractor;
using	aptk::agnostic::Best_Supports_Graph;

void process_command_line_options( int ac, char** av, po::variables_map& vars ) {
	po::options_description desc( "Options:" );

	desc.add_options()
		( "help", "Show help message" )
		( "domain", po::value<std::string>(), "Input PDDL domain description" )
		( "problem", po::value<std::string>(), "Input PDDL problem description" )
	;

	try {
		po::store( po::parse_command_line( ac, av, desc ), vars );
		po::notify( vars );
	}
	catch ( std::exception& e ) {
		std::cerr << "Error: " << e.what() << std::endl;
		std::exit(1);
	}
	catch ( ... ) {
		std::cerr << "Exception of unknown type!" << std::endl;
		std::exit(1);
	}

	if ( vars.count("help") ) {
		std::cout << desc << std::endl;
		std::exit(0);
	}

}

void compute_best_supports_graph( const CC_Problem& task, const aptk::State& s, HC_Heuristic<>& h ) {

}

int main( int argc, char** argv ) {

	po::variables_map vm;

	process_command_line_options( argc, argv, vm );

	if ( !vm.count( "domain" ) ) {
		std::cerr << "No PDDL domain was specified!" << std::endl;
		std::exit(1);
	}

	if ( !vm.count( "problem" ) ) {
		std::cerr << "No PDDL problem was specified!" << std::endl;
		std::exit(1);
	}

	STRIPS_Problem	prob;

	aptk::FF_Parser::get_problem_description( vm["domain"].as<std::string>(), vm["problem"].as<std::string>(), prob );

	std::cout << "PDDL problem description loaded: " << std::endl;
	std::cout << "\tDomain: " << prob.domain_name() << std::endl;
	std::cout << "\tProblem: " << prob.problem_name() << std::endl;
	std::cout << "\t#Actions: " << prob.num_actions() << std::endl;
	std::cout << "\t#Fluents: " << prob.num_fluents() << std::endl;

	Fwd_Search_Problem	search_prob( &prob );
	aptk::State 		s0( prob );
	s0.set( prob.init() );

	float t0 = aptk::time_used();
	H2_Heuristic< Fwd_Search_Problem >  h2( search_prob );
	float tf = aptk::time_used();
	std::cout << "Time to initialize h^2: " << tf - t0 << " secs" << std::endl;

	float h;
	t0 = aptk::time_used();
	h2.eval( s0, h );
	tf = aptk::time_used();
	std::cout << "Time to evaluate h^2: " << tf - t0 << " secs" << std::endl;
	std::cout << "h^2 value = " << h << std::endl;

	std::ofstream	h2_values("h2.values");
	h2.print_values( h2_values );
	h2_values.close();

	t0 = aptk::time_used();
	CC_Problem	pi_2_ce_task( prob, 2 );
	HC_Heuristic<>	h2_ce( pi_2_ce_task );
	tf = aptk::time_used();
	std::cout << "Time to initialize h^{2}_{ce}: " << tf - t0 << " secs" << std::endl;
	std::cout << "# Fluents in \\Pi^{2}_{ce} task: " << pi_2_ce_task.num_fluents() << std::endl;

	t0 = aptk::time_used();
	h2_ce.eval( s0, h );
	tf = aptk::time_used();
	std::cout << "Time to evaluate h^{2}_{ce}: " << tf - t0 << " secs" << std::endl;
	std::cout << "h^{2}_{ce} value = " << h << std::endl;

	std::ofstream	h2_ce_values("h2_ce.values");
	h2_ce.print_values( h2_ce_values );
	h2_ce_values.close();

	t0 = aptk::time_used();
	std::vector<aptk::Fluent_Vec> conjunctions;

	CC_Problem	pi_1_ce_task( prob, conjunctions );
	HC_Heuristic<>	h1_ce( pi_1_ce_task );
	tf = aptk::time_used();
	std::cout << "Time to initialize h^{1}_{ce}: " << tf - t0 << " secs" << std::endl;
	std::cout << "# Fluents in \\Pi^{1}_{ce} task: " << pi_1_ce_task.num_fluents() << std::endl;

	t0 = aptk::time_used();
	h1_ce.eval( s0, h );
	tf = aptk::time_used();
	std::cout << "Time to evaluate h^{1}_{ce}: " << tf - t0 << " secs" << std::endl;
	std::cout << "h^{1}_{ce} value = " << h << std::endl;

	std::ofstream	h1_ce_values("h1_ce.values");
	h1_ce.print_values( h1_ce_values );
	h1_ce_values.close();

	Relaxed_Plan_Extractor< HC_Heuristic<> >	rp_h2_ce( prob, h2_ce );
	std::vector<aptk::Action_Idx> pref_ops;
	rp_h2_ce.compute( s0, h, pref_ops );

	std::cout << "Relaxed plan h^2_ce heuristic = " << h << std::endl;
	std::cout << "Preferred operators: " << std::endl;
	for ( auto it = pref_ops.begin(); it != pref_ops.end(); it++ )
		std::cout << "\t" << prob.actions()[*it]->signature() << std::endl;

	Relaxed_Plan_Extractor< HC_Heuristic<> >	rp_h1_ce( prob, h1_ce );
	pref_ops.clear();
	rp_h1_ce.compute( s0, h, pref_ops );

	std::cout << "Relaxed plan h^1_ce heuristic = " << h << std::endl;
	std::cout << "Preferred operators: " << std::endl;
	for ( auto it = pref_ops.begin(); it != pref_ops.end(); it++ )
		std::cout << "\t" << prob.actions()[*it]->signature() << std::endl;

	Best_Supports_Graph< HC_Heuristic<> > bs_graph( pi_2_ce_task );
	bs_graph.build( h2_ce );

	std::cout << "Actions in best supports graph = " << bs_graph.num_actions() << std::endl;
	std::cout << "Cost of actions in best supports graph = " << bs_graph.cost() << std::endl;

	std::cout << "Preferred operators: " << std::endl;
	for ( auto it = bs_graph.pref_ops().begin(); it != bs_graph.pref_ops().end(); it++ )
		std::cout << "\t" << prob.actions()[*it]->signature() << std::endl;

	std::ofstream	bs_graph_dot_file( "bs_graph_h2_ce.dot" );
	bs_graph.print( bs_graph_dot_file );
	bs_graph_dot_file.close();
	system( "dot -Tpdf -o bs_graph_h2_ce.pdf bs_graph_h2_ce.dot" );

	Best_Supports_Graph< HC_Heuristic<> > bs_graph_h1( pi_1_ce_task );
	bs_graph_h1.build( h1_ce );

	std::cout << "Actions in best supports graph = " << bs_graph_h1.num_actions() << std::endl;
	std::cout << "Cost of actions in best supports graph = " << bs_graph_h1.cost() << std::endl;

	std::cout << "Preferred operators: " << std::endl;
	for ( auto it = bs_graph_h1.pref_ops().begin(); it != bs_graph_h1.pref_ops().end(); it++ )
		std::cout << "\t" << prob.actions()[*it]->signature() << std::endl;

	std::ofstream	bs_graph_h1_dot_file( "bs_graph_h1_ce.dot" );
	bs_graph_h1.print( bs_graph_h1_dot_file );
	bs_graph_h1_dot_file.close();
	system( "dot -Tpdf -o bs_graph_h1_ce.pdf bs_graph_h1_ce.dot" );

	bs_graph_h1.find_conflicts( conjunctions );
	std::cout << "Conflicts suggest the following conjunctions (" << conjunctions.size() << "): " << std::endl;
	for ( unsigned k = 0; k < conjunctions.size(); k++ ) {
		std::cout << k+1 << ". ";
		for ( unsigned l = 0; l < conjunctions[k].size(); l++ ) {
			pi_1_ce_task.print_fluent( conjunctions[k][l], std::cout );
			if ( l < conjunctions[k].size() - 1 )
				std::cout << " & ";
		}
		std::cout << std::endl;
	}

	t0 = aptk::time_used();
	CC_Problem	pi_1_ce_task_1( prob, conjunctions );
	HC_Heuristic<>	h1_ce_1( pi_1_ce_task_1 );
	tf = aptk::time_used();
	std::cout << "Time to initialize h^{C}_{ce}, |C| = " << conjunctions.size() << " : " << tf - t0 << " secs" << std::endl;
	std::cout << "# Fluents in \\Pi^{C}_{ce} task: " << pi_1_ce_task_1.num_fluents() << std::endl;

	t0 = aptk::time_used();
	h1_ce_1.eval( s0, h );
	tf = aptk::time_used();
	std::cout << "Time to evaluate h^{C}_{ce} (1): " << tf - t0 << " secs" << std::endl;
	std::cout << "h^{C}_{ce} (1) value = " << h << std::endl;

	std::ofstream	h1_ce_1_values("hC_ce_1.values");
	h1_ce_1.print_values( h1_ce_1_values );
	h1_ce_1_values.close();

	Best_Supports_Graph< HC_Heuristic<> > bs_graph_hC_1( pi_1_ce_task_1 );
	bs_graph_hC_1.build( h1_ce_1 );

	std::cout << "Actions in best supports graph = " << bs_graph_hC_1.num_actions() << std::endl;
	std::cout << "Cost of actions in best supports graph = " << bs_graph_hC_1.cost() << std::endl;

	std::cout << "Preferred operators: " << std::endl;
	for ( auto it = bs_graph_hC_1.pref_ops().begin(); it != bs_graph_hC_1.pref_ops().end(); it++ )
		std::cout << "\t" << prob.actions()[*it]->signature() << std::endl;

	std::ofstream	bs_graph_hC_1_dot_file( "bs_graph_hC_1_ce.dot" );
	bs_graph_hC_1.print( bs_graph_hC_1_dot_file );
	bs_graph_hC_1_dot_file.close();
	system( "dot -Tpdf -o bs_graph_hC_1_ce.pdf bs_graph_hC_1_ce.dot" );

	bs_graph_hC_1.find_conflicts( conjunctions );

	std::cout << "Conflicts suggest the following conjunctions: " << std::endl;
	for ( unsigned k = 0; k < conjunctions.size(); k++ ) {
		std::cout << k+1 << ". ";
		for ( unsigned l = 0; l < conjunctions[k].size(); l++ ) {
			pi_1_ce_task_1.print_fluent( conjunctions[k][l], std::cout );
			if ( l < conjunctions[k].size() - 1 )
				std::cout << " & ";
		}

		std::cout << std::endl;
	}

	return 0;
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/serialization/single_buffered.h>
#include <scitbx/matrix/transpose_multiply.h>
#include <scitbx/math/utils.h>
#include <boost/python/make_constructor.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_arg.hpp>
#include <boost/format.hpp>
#include "flex_helpers.h"

namespace scitbx { namespace serialization { namespace single_buffered {

  inline
  char* to_string(char* start, vec3<int> const& value)
  {
    return
      to_string(to_string(to_string(start, value[0]), value[1]), value[2]);
  }

  template <>
  struct from_string<vec3<int> >
  {
    from_string(const char* start)
    {
      end = start;
      for(std::size_t i=0;i<3;i++) {
        from_string<int> proxy(end);
        value[i] = proxy.value;
        end = proxy.end;
      }
    }

    vec3<int> value;
    const char* end;
  };

}}} // namespace scitbx::serialization::single_buffered

#include <scitbx/array_family/boost_python/flex_pickle_single_buffered.h>

namespace scitbx { namespace af {
namespace {

  flex<vec3<int> >::type*
  join(
    af::const_ref<int> const& x,
    af::const_ref<int> const& y,
    af::const_ref<int> const& z)
  {
    SCITBX_ASSERT(y.size() == x.size());
    SCITBX_ASSERT(z.size() == x.size());
    af::shared<vec3<int> > result((af::reserve(x.size())));
    for(std::size_t i=0;i<x.size();i++) {
      result.push_back(vec3<int>(x[i],y[i],z[i]));
    }
    return new flex<vec3<int> >::type(result, result.size());
  }

  flex<vec3<int> >::type*
  from_int(
    af::const_ref<int> const& x)
  {
    SCITBX_ASSERT(x.size() % 3 == 0);
    std::size_t result_size = x.size() / 3;
    af::shared<vec3<int> > result((af::reserve(result_size)));
    const int* d = x.begin();
    for(std::size_t i=0;i<result_size;i++) {
      result.push_back(vec3<int>(d));
      d += 3;
    }
    return new flex<vec3<int> >::type(result, result.size());
  }

  flex_int
  as_int(flex<vec3<int> >::type const& a)
  {
    SCITBX_ASSERT(a.accessor().is_trivial_1d());
    flex_int result(a.size()*3, init_functor_null<int>());
    int* r = result.begin();
    const_ref<vec3<int> > a_ref = a.const_ref().as_1d();
    for(std::size_t i=0;i<a_ref.size();i++) {
      for(std::size_t j=0;j<3;j++) {
        *r++ = a_ref[i][j];
      }
    }
    return result;
  }

  af::shared<vec3<double> >
  as_vec3_double(flex<vec3<int> >::type const& a)
  {
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(vec3<double>(a[i]));
    }
    return result;
  }

} // namespace <anonymous>

namespace boost_python {

  template <>
  struct flex_default_element<vec3<int> >
  {
    static vec3<int>
    get() { return vec3<int>(0,0,0); }
  };

  void wrap_flex_vec3_int()
  {
    using namespace boost::python;
    using boost::python::arg;
    typedef flex_wrapper<vec3<int> > f_w;
    f_w::plain("vec3_int")
      .def_pickle(flex_pickle_single_buffered<vec3<int>,
        3*pickle_size_per_element<int>::value>())
      .def("__init__", make_constructor(join))
      .def("__init__", make_constructor(from_int))
      .def("as_int", as_int)
      .def("as_vec3_double", as_vec3_double)
      .def("add_selected",
        (object(*)(
          object const&,
          af::const_ref<std::size_t> const&,
          af::const_ref<vec3<int> > const&)) add_selected_unsigned_a, (
            arg("indices"), arg("values")))
    ;
  }

}}} // namespace scitbx::af::boost_python


// Copyright 2020-2021 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     https://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "reconstruction.h"

#include <Eigen/src/SparseCore/SparseSolverBase-mod.h>
#include <igl/cotmatrix.h>
#include <igl/invert_diag.h>
#include <igl/massmatrix.h>
#include <igl/min_quad_with_fixed.h>
#include <image/imageUtils.h>
#include <ir3d-utils/MeshBuilder.h>
#include <ir3d-utils/regionToMesh.h>

#include <Eigen/Core>
#include <Eigen/Sparse>

#include "loadsave.h"
#include "macros.h"

#define ENABLE_MERGING_COMMON_BOUNDARY_IN_PREPROCESS

using namespace std;
using namespace Eigen;
using namespace igl;

template <typename T>
int sgn(T val) {
  return (T(0) < val) - (val < T(0));
}

bool reconstruction(
    const RecData &recData, const ImgData &imgData,
    const std::vector<Imguc> &outlineImgs, const std::vector<Imguc> &regionImgs,
    const std::string &triangleOpts, Eigen::MatrixXd &Vout,
    Eigen::MatrixXi &Fout, Eigen::MatrixXd &VPreinfOut,
    std::vector<std::vector<int>> &verticesOfParts,
    Eigen::SparseMatrix<double> &I, Eigen::SparseMatrix<double> &LFlat,
    Eigen::SparseMatrix<double> &LPreinf, Eigen::SparseMatrix<double> &LFinal,
    Eigen::SparseMatrix<double> &MFinal, Eigen::SparseMatrix<double> &MinvFinal,
    Eigen::SparseMatrix<double> &Aeq, Eigen::VectorXd &Beq,
    Eigen::SparseMatrix<double> &Aieq, Eigen::VectorXd &Bieq,
    std::vector<std::tuple<int, int, int>> &ineqRegionConds,
    std::vector<std::vector<int>> &bnds, std::vector<int> &mergeBnd,
    std::vector<std::tuple<int, int, int>> &mergeArmpitsCorrs,
    std::vector<TriData> &triData, const int smoothFactor,
    const double defaultInflationAmount, const bool armpitsStitching,
    const bool armpitsStitchingInJointOptimization,
    std::set<int> &mergeBothSides) {
  auto &regionInflationAmount = recData.regionInflationAmount;
  auto &layers = imgData.layers;

  const int N = regionImgs.size();
  const int numRegions = 2 * N;

  // convert regions into flat meshes
  vector<MatrixXd> VsFront, VsBack;
  vector<MatrixXi> FsFront, FsBack;
  vector<vector<vector<Vector2f>>> regionsBnds;
  const bool interiorMergingPtsOnly = true;
  outlineToMesh(outlineImgs, regionImgs, true, false, interiorMergingPtsOnly,
                triangleOpts, mergeBothSides, VsFront, FsFront, smoothFactor,
                regionsBnds, triData);
  outlineToMesh(outlineImgs, regionImgs, false, true, interiorMergingPtsOnly,
                triangleOpts, mergeBothSides, VsBack, FsBack, smoothFactor);

  // create vectors of indices of boundary points (customBnds) from regionsBnds
  vector<vector<vector<int>>> customBnds;
  forlist(k, regionsBnds) {
    vector<vector<int>> bnd1;
    int c = 0;
    forlist(l, regionsBnds[k]) {
      vector<int> bnd2;
      forlist(m, regionsBnds[k][l]) {
        bnd2.push_back(c);
        c++;
      }
      bnd1.push_back(bnd2);
    }
    customBnds.push_back(bnd1);
  }

  {
    int count = 0;
    forlist(i, VsFront) count += VsFront[i].rows();
    forlist(i, VsBack) count += VsBack[i].rows();
    DEBUG_CMD_MM(cout << endl
                      << "-- TRACE: total vertices: " << count << endl
                      << endl;)
  }

  // process all planar meshes
  const int Z_COORD = 2;
  MeshBuilder mb;
  mb.graftingBndStitching = false;
  mb.graftingBndIneq = false;
  mb.flipReg.resize(2 * N, false);
  forlist(i, regionImgs) {
#ifdef ENABLE_MERGING_COMMON_BOUNDARY_IN_PREPROCESS
    mb.processTwoSidedMesh(VsFront[i], FsFront[i], VsBack[i], FsBack[i], false,
                           customBnds[i], true);
#else
    mb.processPlanarMesh(VsFront[i], FsFront[i], false, customBndsFront[i],
                         false);
    mb.processPlanarMesh(VsBack[i], FsBack[i], false, customBndsBack[i], false);
#endif
  }
  // default inflation amount
  vector<float> inflationAmount(2 * N, defaultInflationAmount);
  forlist(i, regionImgs) {
    const int regId = layers[i];
    mb.flipReg[2 * i + 1] = true;
    auto it = regionInflationAmount.find(regId);
    if (it != regionInflationAmount.end()) {
      inflationAmount[2 * i] = inflationAmount[2 * i + 1] = it->second;
    }
    inflationAmount[2 * i + 1] *= -1;
  }
  mb.createCompleteMesh();
  bnds = mb.bnds;

  // Determine relative depth conditions for a region B based on the following
  // rules:
  // - for all regions A that are behind B, set B_backBnd is in front of
  // A_frontIn
  // - for all regions C that are in front of B, set B_frontBnd is behind
  // C_backIn Determine merging of a region B to the nearest one based on the
  // following rules: (If a region B has merging boundaries, it is supposed to
  // be connected with other regions, also we assume that it is connected to the
  // nearest region.)
  // - find the nearest region A (e.g. body) that is behind B (e.g. front leg)
  // and merge B_frontConnBnd with A_frontConnIn
  // - find the nearest region C (e.g. body) that is in front of B (e.g. back
  // leg) and merge B_backConnBnd with C_backConnIn
  typedef MeshBuilder::VertexType VertexType;
  vector<bool> &flipReg = mb.flipReg;
  vector<tuple<int, int, int, int>> selectedVCondsIneq, selectedVCondsEq;
  vector<vector<pair<int, int>>> selectedMergingCorrs;
  vector<bool> reverseNs;
  unordered_map<int, bool> vertexUsedMerge;
  set<int> vertexUsed;
  vector<tuple<int, int, int>> mergingPartNum;  // this part, opposite part

  fora(shift, 1, numRegions) {
    fora(B, 0, numRegions) {
      const bool BIsFrontSide = !flipReg[B];
      int A = B - shift;
      if (A >= 0) {  // A is behind B
        if (A == (B - 1) && A % 2 == 0)
          continue;  // A is front side of B, skip it
        const bool AIsFrontSide = !flipReg[A];
        if (BIsFrontSide && AIsFrontSide) {  // MERGING: A is behind B &&
                                             // BIsFrontSide && AIsFrontSide
          const auto &mc = mb.mergingCorr[B][A];
          vector<pair<int, int>> tmp;
          forlist(i, mc) {
            const int vInd = mc[i].second;  // boundary vertex
            if (vertexUsedMerge.find(vInd) == vertexUsedMerge.end()) {
              tmp.push_back(mc[i]);
              vertexUsedMerge[vInd] = true;
            }
          }
          selectedMergingCorrs.push_back(tmp);
          reverseNs.push_back(false);

          if (mergeBothSides.find(B / 2) != mergeBothSides.end()) {
            const auto &mc = mb.mergingCorr[B + 1][A + 1];
            vector<pair<int, int>> tmp;
            forlist(i, mc) {
              const int vInd = mc[i].second;  // boundary vertex
              if (vertexUsedMerge.find(vInd) == vertexUsedMerge.end()) {
                tmp.push_back(mc[i]);
                vertexUsedMerge[vInd] = true;
              }
            }
            selectedMergingCorrs.push_back(tmp);
            reverseNs.push_back(true);
          }

          mergingPartNum.push_back(
              make_tuple(B, B + 1, 1));  // BEWARE! hardcoded
        }

        // Inequalities for joint optimization:
        // for interior to interior correspondences
        //          ineqRegionConds.push_back(make_tuple(B,A,1));

        // for boundary to interior correspondences
        if (BIsFrontSide &&
            AIsFrontSide) {  // REL_DEPTHS: A is behind B && BIsFrontSide &&
                             // AIsFrontSide: assume merged boundary, only front
                             // bnd remains
          ineqRegionConds.push_back(make_tuple(B, A, 1));
        }
      }
      int C = B + shift;
      if (C < numRegions) {  // C is in front of B
        if (C == (B + 1) && C % 2 == 1)
          continue;  // C is back side of B, skip it
        const bool CIsFrontSide = !flipReg[C];
        if (!BIsFrontSide && !CIsFrontSide) {  // MERGING: C is in front of B &&
                                               // BIsBackSide && CIsBackSide
          const auto &mc = mb.mergingCorr[B][C];
          vector<pair<int, int>> tmp;
          forlist(i, mc) {
            const int vInd = mc[i].second;  // boundary vertex
            if (vertexUsedMerge.find(vInd) == vertexUsedMerge.end()) {
              tmp.push_back(mc[i]);
              vertexUsedMerge[vInd] = true;
            }
          }
          selectedMergingCorrs.push_back(tmp);
          mergingPartNum.push_back(
              make_tuple(B, B - 1, -1));  // BEWARE! hardcoded
          reverseNs.push_back(true);
        }

        if (BIsFrontSide && !CIsFrontSide) {  // REL_DEPTHS: C is in front of B
                                              // && BIsFrontSide && CIsBackSide
          ineqRegionConds.push_back(make_tuple(B, C, -1));
        }
      }
    }
  }

  // perform exterior (front to front) side merging
  int mergingCorrsNum = 0;
  forlist(i, selectedMergingCorrs) {
    mergingCorrsNum += selectedMergingCorrs[i].size();
    mb.mergeMesh(selectedMergingCorrs[i], reverseNs[i]);
    for (auto &el : selectedMergingCorrs[i]) {
      const int bndId = el.second;
      const int meshId = el.first;
      mergeBnd.push_back(bndId);
      mergeBnd.push_back(meshId);
    }
  }
  DEBUG_CMD_MM(cout << "mergingCorrs: " << mergingCorrsNum << endl;)

  // get the final flat mesh
  MatrixXd V;
  MatrixXi F;
  vector<int> bnd, neumannBnd;
  mb.getCompleteMesh(V, F);
  mb.getCompleteBoundary(bnd, neumannBnd);

  // convert relative depth conditions into a matrix form suitable for quadratic
  // programming
  struct m2mCorr  // mesh to mesh correspondence
  {
    VertexType typea;  // type(0=bnd,1=inside)
    VertexType typeb;  // type(0=bnd,1=inside)
    int a;
    int b;
    int customNum;  // customNum
    int sign;
  };
#ifdef ENABLE_MERGING_COMMON_BOUNDARY_IN_PREPROCESS
  vector<m2mCorr> eqs;
#else
  vector<m2mCorr> eqs(N);
  fora(i, 0, N) {
    eqs.push_back(
        m2mCorr{VertexType::bnd, VertexType::bnd, 2 * i, 2 * i + 1, 0, 0});
  }
#endif

  // equalities
  int num = 0;
  int numBndA = 0, numInA = 0, numBndB = 0, numInB = 0, numSkipped = 0;
  forlist(k, eqs) {
    const auto &el = eqs[k];
    const auto &bc = mb.eqCorr[el.a][el.b];
    forlist(i, bc) {
      const int customNum = el.customNum;
      const int vertexCustomNum = bc[i].customNum;
      // only apply the condition if customNum==0 or customNum==vertexCustomNum
      if (!(customNum == 0 || customNum == vertexCustomNum)) continue;
      if ((el.typea == VertexType::all && el.typeb == VertexType::all) ||
          (el.typea == VertexType::all && el.typeb == bc[i].typeb) ||
          (el.typea == bc[i].typea && el.typeb == VertexType::all) ||
          (el.typea == bc[i].typea && el.typeb == bc[i].typeb)) {
        if (el.typea == VertexType::all || el.typea == VertexType::bnd)
          numBndA++;
        if (el.typea == VertexType::all || el.typea == VertexType::in) numInA++;
        if (el.typeb == VertexType::all || el.typeb == VertexType::bnd)
          numBndB++;
        if (el.typeb == VertexType::all || el.typeb == VertexType::in) numInB++;
        num++;
      }
    }
    DEBUG_CMD_MM(cout << "eqs: " << el.a << " " << el.b << " typea:" << el.typea
                      << " typeb:" << el.typeb << endl;)
  }
  num += selectedVCondsEq.size();
  DEBUG_CMD_MM(cout << "eqsnum: " << num << ", bndA: " << numBndA << ", inA: "
                    << numInA << ", bndB: " << numBndB << ", inB: " << numInB
                    << ", skipped: " << numSkipped << endl;)
  Beq = VectorXd(num);
  Beq.fill(0);
  Aeq = SparseMatrix<double>(num, V.rows());
  int c = 0;
  unordered_map<int, set<int>> verticesToMerge;
  vector<pair<int, int>> bnd2bnd;
  //  set<int> verticesToMergeUsed;
  forlist(k, eqs) {
    const auto &el = eqs[k];
    const auto &bc = mb.eqCorr[el.a][el.b];
    forlist(i, bc) {
      const int customNum = el.customNum;
      const int vertexCustomNum = bc[i].customNum;
      if (!(customNum == 0 || customNum == vertexCustomNum)) continue;
      if (!((el.typea == VertexType::all && el.typeb == VertexType::all) ||
            (el.typea == VertexType::all && el.typeb == bc[i].typeb) ||
            (el.typea == bc[i].typea && el.typeb == VertexType::all) ||
            (el.typea == bc[i].typea && el.typeb == bc[i].typeb)))
        continue;

      verticesToMerge[bc[i].a].insert(bc[i].b);
      bnd2bnd.push_back(make_pair(bc[i].a, bc[i].b));
      //      verticesToMergeUsed.insert(bc[i].b);
      Aeq.insert(c, bc[i].a) = 1;
      Aeq.insert(c, bc[i].b) = -1;
      c++;
    }
  }
  {
    forlist(i, selectedVCondsEq) {
      const auto &s = selectedVCondsEq[i];
      const auto &bc = mb.eqCorr[get<0>(s)][get<1>(s)][get<2>(s)];
#ifdef ENABLE_EQUALITIES
      verticesToMerge.push_back(
          make_pair(bc.a, bc.b));  // TODO: this makes duplicates and causes
                                   // crashes later in merging phase
#endif
      Aeq.insert(c, bc.a) = 1;   // boundary
      Aeq.insert(c, bc.b) = -1;  // mesh
      c++;
    }
  }

  // inequalities
  num = selectedVCondsIneq.size();
  Bieq = VectorXd(num);
  Bieq.fill(0);
  Aieq = SparseMatrix<double>(num, V.rows());
  {
    int c = 0;
    forlist(i, selectedVCondsIneq) {
      const auto &s = selectedVCondsIneq[i];
      const auto &bc = mb.ineqCorr[get<0>(s)][get<1>(s)][get<2>(s)];
      const int sign = get<3>(s);
      Aieq.insert(c, bc.a) = sign * 1;   // boundary
      Aieq.insert(c, bc.b) = sign * -1;  // mesh
      c++;
    }
  }
  DEBUG_CMD_MM(cout << "ineqsNum: " << num << endl;)

  // preinflation
  DEBUG_CMD_MM(cout << "Inflation " << endl;)
  SparseMatrix<double> M, Minv, LFlatNotMerged, LPreinfNotMerged;
  massmatrix(V, F, igl::MASSMATRIX_TYPE_VORONOI, M);

  if (M.nonZeros() - M.rows() != 0) {
    DEBUG_CMD_MM(
        cout << "error: M.nonZeros()-M.rows()=" << M.nonZeros() - M.rows()
             << " probably a problem in interconnection (non-manifold mesh?)"
             << endl;)
    return false;
  }

  invert_diag(M, Minv);
  cotmatrix(V, F, LFlatNotMerged);

  const int nBnd = bnd.size();
  const int nV = V.rows();
  // sqrt
  VectorXi b(nBnd);
  VectorXd bc(nBnd);
  VectorXd inB(nV);
  VectorXd outZ;
  fora(i, 0, nBnd) b(i) = bnd[i];
  bc.fill(0);

  fora(i, 0, mb.getMeshesCount()) {
    forlist(j, mb.parts1[i]) inB(mb.parts1[i][j]) = -inflationAmount[i];
  }

  SparseMatrix<double> Q = Minv * LFlatNotMerged;
  min_quad_with_fixed(Q, inB, b, bc, SparseMatrix<double>(), VectorXd(), false,
                      outZ);

  fora(i, 0, mb.getMeshesCount()) {
    forlist(j, mb.parts1[i]) {
      int ind = mb.parts1[i][j];
      V(ind, Z_COORD) = sgn(outZ(ind)) * sqrt(abs(outZ(ind)));
    }
  }
  MatrixXd VPreinf = V;
  DEBUG_CMD_MM(cout << "Inflation done" << endl;)

  cotmatrix(VPreinf, F, LPreinfNotMerged);

  I = SparseMatrix<double>(V.rows(), V.rows());

  // merging of interior vertices
  if (armpitsStitching || armpitsStitchingInJointOptimization) {
    mergeArmpitsCorrs.clear();
    forlist(i, selectedMergingCorrs) {
      int p1 = get<0>(mergingPartNum[i]);
      int p2 = get<1>(mergingPartNum[i]);
      int sign = get<2>(mergingPartNum[i]);
      forlist(j, selectedMergingCorrs[i]) {
        auto &el = selectedMergingCorrs[i][j];
        const int bndId = el.second;
        const int meshId = el.first;  // free
        if (j == 0 ||
            j == selectedMergingCorrs[i].size() - 1) {  // first and last
#ifdef ENABLE_MERGING_COMMON_BOUNDARY_IN_PREPROCESS
          // add first and last
          mergeArmpitsCorrs.push_back(make_tuple(bndId, meshId, sign));
          if (!armpitsStitchingInJointOptimization)
            verticesToMerge[bndId].insert(meshId);
#else
          // skip the first and last
#endif

        } else {
          const auto &mc = mb.mergingCorr[p1][p2];
          int counterpartId = -1;
          forlist(k, mc) {
            int i1 = mc[k].second;
            int i2 = mc[k].first;
            if (bndId == i1) {
              counterpartId = i2;
              //          cout << "found counterpart: " << counterpartId <<
              //          endl;
              break;
            }
          }

          if (counterpartId >= 0) {
            //        if (verticesToMergeUsed.find(counterpartId) ==
            //        verticesToMergeUsed.end()) {
            mergeArmpitsCorrs.push_back(
                make_tuple(counterpartId, meshId, sign));
            if (!armpitsStitchingInJointOptimization)
              verticesToMerge[counterpartId].insert(meshId);
            //        }
          } else {
            DEBUG_CMD_MM(cout << "counterpart wasn't found (bndId: " << bndId
                              << ", part1: " << p1 << ", part2: " << p2 << ")"
                              << endl;)
          }
        }
      }
    }
    DEBUG_CMD_MM(cout << "mergeArmpitsCorrs size: " << mergeArmpitsCorrs.size()
                      << endl;)
    if (!armpitsStitchingInJointOptimization) {
#ifdef ENABLE_MERGING_COMMON_BOUNDARY_IN_PREPROCESS
      const int n = 2 * mergeArmpitsCorrs.size();
#else
      const int n = bnd.size() + 2 * mergeArmpitsCorrs.size();
#endif
      b = VectorXi(n);
      VectorXi b2(mergeArmpitsCorrs.size());
      bc = VectorXd(n);
      bc.fill(0);
      VectorXd bc2(mergeArmpitsCorrs.size());
      bc.fill(0);
      Aeq =
          SparseMatrix<double>(bnd.size() + mergeArmpitsCorrs.size(), V.rows());
      Beq = VectorXd(bnd.size() + mergeArmpitsCorrs.size());
      Beq.fill(0);
      forlist(i, mergeArmpitsCorrs) {
        int first = get<0>(mergeArmpitsCorrs[i]);
        int second = get<1>(mergeArmpitsCorrs[i]);
        int sign = get<2>(mergeArmpitsCorrs[i]);
        Aeq.insert(i, first) = sign * 1;    // boundary
        Aeq.insert(i, second) = sign * -1;  // mesh

        b2(i) = b(i) = first;
        b(i + mergeArmpitsCorrs.size()) = second;
        //      double avg = V(first,Z_COORD);
        //      double avg = V(second,Z_COORD);
        double avg = 0.5 * (V(first, Z_COORD) + V(second, Z_COORD));
        //    V(first,Z_COORD) = avg;
        bc2(i) = bc(i) = bc(i + mergeArmpitsCorrs.size()) = avg;
      }
#ifndef ENABLE_MERGING_COMMON_BOUNDARY_IN_PREPROCESS
      if (!performQP) {
        forlist(i, bnd2bnd) {
          int j = i + mergeArmpitsCorrs.size();
          Aeq.insert(j, bnd2bnd[i].first) = 1;
          Aeq.insert(j, bnd2bnd[i].second) = -1;
        }
      }
      fora(i, 0, nBnd) { b(i + 2 * mergeArmpitsCorrs.size()) = bnd[i]; }
#endif

      fora(i, 0, bnd.size()) I.insert(bnd[i], bnd[i]) = 0.00001;
      SparseMatrix<double> Q;
      Q = -(LPreinfNotMerged - I);
      inB = LPreinfNotMerged * V.col(Z_COORD);
      DEBUG_CMD_MM(cout << "Linear solve " << flush;);
      min_quad_with_fixed(Q, inB, VectorXi(), VectorXd(), Aeq, Beq, false,
                          outZ);
      DEBUG_CMD_MM(cout << "done" << endl;)

      V.col(Z_COORD) = outZ;
      VPreinf = V;
    }
  }

#ifndef DISABLE_EQUALITY_VERTICES_MERGING
  DEBUG_CMD_MM(cout << "Merge mesh and remove duplicates ("
                    << verticesToMerge.size() << ") " << flush;)
  vector<bool> removeV;
  vector<int> reindex;
  MeshBuilder::meshMergeAndRemoveDuplicates(V, F, verticesToMerge, Vout, Fout,
                                            mb.parts1, verticesOfParts, removeV,
                                            reindex);
  VPreinfOut = Vout;
  fora(i, 0, V.rows()) if (!removeV[i]) VPreinfOut(reindex[i], Z_COORD) =
      VPreinf(i, Z_COORD);

  cotmatrix(VPreinfOut, Fout, LPreinf);
  DEBUG_CMD_MM(cout << "done" << endl;)
#else
  verticesOfParts = mb.parts1;
  // return the result
  Vout = V;
  Fout = F;
  LPreinf = LPreinfNotMerged;
#endif

  massmatrix(Vout, Fout, igl::MASSMATRIX_TYPE_VORONOI, MFinal);
  invert_diag(MFinal, MinvFinal);
  cotmatrix(Vout, Fout, LFinal);

  MatrixXd VoutFlat = Vout;
  VoutFlat.col(Z_COORD).fill(0);
  cotmatrix(VoutFlat, Fout, LFlat);

#ifndef DISABLE_EQUALITY_VERTICES_MERGING
  // recompute
  const int n = Vout.rows();

  std::vector<std::vector<int>> bnds2 = bnds;
  bnds.clear();
  forlist(i, bnds2) {
    vector<int> bnd;
    forlist(j, bnds2[i]) {
      int ind = bnds2[i][j];
      if (!removeV[ind]) bnd.push_back(reindex[ind]);
    }
    bnds.push_back(bnd);
  }

  vector<int> mergeBnd2 = mergeBnd;
  mergeBnd.clear();
  for (int ind : mergeBnd2) {
    if (!removeV[ind]) mergeBnd.push_back(reindex[ind]);
  }

  Aeq.resize(0, 0);
  Beq.resize(0);
  Aieq.resize(0, 0);
  Bieq.resize(0);
#endif

  return true;
}

bool performReconstruction(RecData &recData, DefData &defData, CPData &cpData,
                           ImgData &imgData) {
  auto &def = defData.def;
  auto &verticesOfParts = defData.verticesOfParts;
  auto &defEng = defData.defEng;
  auto &mesh = defData.mesh;
  auto &defEngMaxIter = defData.defEngMaxIter;
  auto &rigidity = defData.rigidity;

  auto &outlineImgs = imgData.outlineImgs;
  auto &regionImgs = imgData.regionImgs;
  auto &layers = imgData.layers;

  auto &cpsAnim = cpData.cpsAnim;
  auto &savedCPs = cpData.savedCPs;

  auto &triangleOpts = recData.triangleOpts;
  auto &subsFactor = recData.subsFactor;
  auto &armpitsStitching = recData.armpitsStitching;
  auto &armpitsStitchingInJointOptimization =
      recData.armpitsStitchingInJointOptimization;
  auto &smoothFactor = recData.smoothFactor;
  auto &defaultInflationAmount = recData.defaultInflationAmount;
  auto &mergeBothSides = recData.mergeBothSides;
  auto &shiftModelX = recData.shiftModelX;
  auto &shiftModelY = recData.shiftModelY;
  auto &tempSmoothingSteps = recData.tempSmoothingSteps;
  auto &searchThreshold = recData.searchThreshold;
  auto &cpOptimizeForXY = recData.cpOptimizeForXY;
  auto &cpOptimizeForZ = recData.cpOptimizeForZ;
  auto &interiorDepthConditions = recData.interiorDepthConditions;

  if (layers.empty()) {
    return false;
  }

  // 3D reconstruction
  MatrixXd V, VPreinf;
  MatrixXi F;
  const int nLayers = layers.size();

  // subsample the input drawings
  // works for binary images only!
  auto subsample = [](auto &I, int subsampleFactor, int bgColor) -> auto {
    typename remove_reference<decltype(I)>::type out(
        I.w / subsampleFactor, I.h / subsampleFactor, I.ch, I.alphaChannel);
    out.fill(bgColor);
    fora(y, 0, I.h) fora(x, 0, I.w) fora(c, 0, I.ch) {
      const auto &val = I(x, y, c);
      if (val != bgColor)
        out(x / subsampleFactor, y / subsampleFactor, c) = val;
    }
    return out;
  };
  vector<Imguc> regionImgsSubs;
  regionImgsSubs.reserve(nLayers);
  vector<Imguc> outlineImgsSubs;
  outlineImgsSubs.reserve(nLayers);
  int nRegionsToInflate = 0;
  forlist(layerId, layers) {
    const int regId = layers[layerId];
    regionImgsSubs.push_back(
        closeSquare(subsample(regionImgs[regId], subsFactor, 0), Cu{0}, 0));
    outlineImgsSubs.push_back(closeSquare(
        subsample(outlineImgs[regId], subsFactor, 255), Cu{255}, 0));
    nRegionsToInflate++;
  }

  // not used after inflation
  SparseMatrix<double> I, LFlat, LPreinf, LFinal, Aeq, Aieq, MFinal, MinvFinal;
  VectorXd Beq, Bieq;

  vector<tuple<int, int, int>> ineqRegionConds;
  vector<vector<int>> bnds;
  vector<int> mergeBnd;
  vector<tuple<int, int, int>> mergeArmpitsCorrs;
  vector<TriData> triData;
  bool success = true;
  if (nRegionsToInflate > 0) {
    success = reconstruction(
        recData, imgData, outlineImgsSubs, regionImgsSubs, triangleOpts, V, F,
        VPreinf, verticesOfParts, I, LFlat, LPreinf, LFinal, MFinal, MinvFinal,
        Aeq, Beq, Aieq, Bieq, ineqRegionConds, bnds, mergeBnd,
        mergeArmpitsCorrs, triData, smoothFactor, defaultInflationAmount,
        armpitsStitching, armpitsStitchingInJointOptimization, mergeBothSides);
  }

  if (!success) {
    return false;
  }

  fora(i, 0, V.rows()) fora(j, 0, 3) {
    V(i, j) *= subsFactor;
    VPreinf(i, j) *= subsFactor;
  }
  for (auto &el : triData) el.scaling = subsFactor;
  V.col(0).array() += shiftModelX;
  V.col(1).array() += shiftModelY;
  VPreinf.col(0).array() += shiftModelX;
  VPreinf.col(1).array() += shiftModelY;

  // prepare for deformation
  mesh = Mesh3D();
  def = Def3D();
  mesh.createFromMesh(VPreinf, F);

  defData.VCurr = mesh.VCurr;
  defData.Faces = mesh.F;
  defData.VRestOrig = mesh.VRest;

  // load control points and their animations
  cpsAnim.clear();
  istringstream iss(savedCPs);
  loadControlPointsFromStream(iss, cpData, defData, imgData);

  defEng = DefEngARAPL();
  defEng.ineqRegionConds = ineqRegionConds;
  defEng.mergeArmpitsCorrs = mergeArmpitsCorrs;
  defEng.bnds = bnds;
  defEng.partsIds = verticesOfParts;
  defEng.tempSmoothingSteps = tempSmoothingSteps;
  defEng.maxIter = defEngMaxIter;
  defEng.mergeBnd = mergeBnd;
  defEng.searchThreshold = searchThreshold;
  defEng.cpOptimizeForXY = cpOptimizeForXY;
  defEng.cpOptimizeForZ = cpOptimizeForZ;
  defEng.interiorDepthConditions = interiorDepthConditions;
  defEng.armpitsStitchingInJointOptimization =
      armpitsStitchingInJointOptimization;

  defEng.rigidity = rigidity;
  defEng.L = LFinal;
  defEng.M = MFinal;
  defEng.Minv = MinvFinal;
  cotmatrix(VPreinf, F, defEng.L);

  defEng.precompute(def, mesh);

  return true;
}


#pragma warning(push, 0)
#pragma warning(disable : ALL_CODE_ANALYSIS_WARNINGS)
#include <SDL.h>
#include <SDL_ttf.h>

#include <Eigen/Dense>
#include <Eigen/StdVector>

#include <cmath>
#include <cstdint>
#include <cstdio>
#include <cstdlib>

#include <stdexcept>
#pragma warning(pop)

#define M_PI_F 3.14159265358979323846264338327950288f

template <typename T> using AlignedVector = std::vector<T, Eigen::aligned_allocator<T>>;

#ifdef __EMSCRIPTEN__
#include <emscripten.h>
static constexpr float mouseSensitivity = 1;
#else

inline uint32_t game_running = 0;

void emscripten_cancel_main_loop() { game_running = 0; }

void emscripten_set_main_loop_arg(void (*fcn)(void*), void* const arg, uint32_t const fps, int const)
{
    uint32_t const MPF = fps > 0 ? 1000 / fps : 1000 / 60; // milliseconds per frame

    // Only render a frame if at least the MPF has been passed in time
    // If the  time has not been passed, pop and process one event from
    // the event queue.  So, for fast computers, many events will be processed
    // in a single frame.  But for slow computers, only one event will be
    // popped and processed in a single frame.  Beneficial because on a slow
    // computer, if all events were popped at once, if the W key were tapped
    // for instance, it would add to the zdiff variable and subtract from it
    // before MovePlayer would have a chance to do anything.  In this case,
    // the player might move too far, but at least the player moved.

    game_running = 1;
    while (game_running)
    {
        uint32_t const startTime = SDL_GetTicks();
        fcn(arg);
        uint32_t const endTime = SDL_GetTicks();

        uint32_t const timeDiff = endTime - startTime;

        if (timeDiff < MPF)
        {
            SDL_Delay(MPF - timeDiff); // Sleep for the remainder of the time to maintain FPS
        }
    }
}

static constexpr float mouseSensitivity = 0.1f;
#endif

struct Entity
{
    Entity(std::string_view const _name, SDL_Surface* const _texture, Eigen::Vector3f const& _pos) noexcept
        : name{_name}, texture{_texture}, pos{_pos}
    {
    }

    std::string name;
    SDL_Surface* texture;
    Eigen::Vector3f pos; // x, y, h

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

// Player location, direction, and speed
struct direction
{
    direction() : pos{0.0f, 48.0f, 0.0f}, theta{M_PI_F / 2}, c{400.0f}, speed{192.0f / 1000.0f} {}

    // Direction vector
    // Eigen::Vector2f p;

    // Normal vector (must point east in screenspace)
    // Eigen::Vector2f n;

    // Player position
    Eigen::Vector3f pos;

    // Player angle from X axis
    float theta;

    // Camera properties
    float c; // Distance from lens (TODO: move to projection matrix)

    // Player speed
    float speed; // Units per millisecond
};

// Key states
struct movement
{
    float xdiff; // Whether A(-1) or D(1) is down
    float zdiff; // Whether W(1) or S(-1) is down
    float hdiff; // Whether T(1) or G(-1) is down
    float cdiff; // Whether Y(-1) or H(1) is down
};

// Render properties
// Defaults: 640x480 at 35fps native (DOOM), vsync for emscripten

struct GameResources
{
    SDL_Window* window;
    SDL_Renderer* renderer;
    SDL_Texture* texture;
    SDL_Surface* display;
    SDL_Surface* floor;
    SDL_Surface* skyTranspose;
    SDL_Surface* wallFrontTranspose;
    SDL_Surface* wallBackTranspose;
    SDL_Surface* columnSprite;
    SDL_Surface* healthSprite;
    SDL_Surface* vialSprite;
    SDL_Surface* doomGuy[8];
    TTF_Font* font;

    AlignedVector<Entity> entities;

    direction dir;
    movement move = {};

    Eigen::Matrix4f viewInverse = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f view = Eigen::Matrix4f::Identity();

    uint32_t windowID;

    ~GameResources() noexcept
    {
        TTF_CloseFont(font);
        SDL_FreeSurface(floor);
        SDL_FreeSurface(skyTranspose);
        SDL_FreeSurface(wallFrontTranspose);
        SDL_FreeSurface(wallBackTranspose);
        SDL_FreeSurface(display);
        SDL_FreeSurface(columnSprite);
        SDL_FreeSurface(healthSprite);
        SDL_FreeSurface(vialSprite);
        for (size_t i = 0; i < 8; i++)
        {
            SDL_FreeSurface(doomGuy[i]);
        }
        SDL_DestroyTexture(texture);
        SDL_DestroyRenderer(renderer);
        SDL_DestroyWindow(window);
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void MovePlayer(direction& dir, Eigen::Matrix4f const& viewInverse, movement const& move, uint32_t const timediff)
{
    // Position vector
    dir.pos +=
        dir.speed * timediff * viewInverse.topLeftCorner<3, 3>() * Eigen::Vector3f{move.xdiff, move.hdiff, move.zdiff};

    // Adjust focal length of camera (TODO: move to projection matrix)
    dir.c += move.cdiff * dir.speed * timediff;
}

void UpdateViewMatrices(direction const& dir, Eigen::Matrix4f& viewInverse, Eigen::Matrix4f& view)
{
    // viewInverse already initialized to Identity on startup

    // Upper left corner is a rotation matrix -- playing field is the XZ plane
    viewInverse.topLeftCorner<3, 3>() = Eigen::AngleAxis<float>{dir.theta, Eigen::Vector3f{0, 1, 0}}.toRotationMatrix();

    // View inverse X should be going in scanline direction (left-handed)
    viewInverse.col(0) *= -1;

    // dir.p.x() = std::cos(dir.theta);
    // dir.p.y() = std::sin(dir.theta);

    // dir.n.x() = dir.p.y();
    // dir.n.y() = -dir.p.x();

    viewInverse.topRightCorner<3, 1>() = dir.pos;

    // Travel along normal
    // dir.pos.topRows<2>() += dir.speed * timediff * move.xdiff * dir.n;

    // Travel along direction vector
    // dir.pos.topRows<2>() += dir.speed * timediff * move.zdiff * dir.p;

    // Move height of camera
    // dir.pos(2) += move.hdiff * dir.speed * timediff;

    // Adjust focal length of camera (TODO: move to projection matrix)
    // dir.c += move.cdiff * dir.speed * timediff;

    // view = Eigen::Matrix4f::Identity(); // not required as view was already
    // initialized to I float inverseC = 1.0f / viewInverse(3, 3);

    view.topLeftCorner<3, 3>() = viewInverse.topLeftCorner<3, 3>().transpose();
    view.topRightCorner<3, 1>() = -view.topLeftCorner<3, 3>() * dir.pos; // * inverseC;
                                                                         // view(3, 3) = inverseC;
}

int ProcessEvent(direction& dir, movement& move, uint32_t windowID)
{
    char e = 0;

    static SDL_Event event;

    while (SDL_PollEvent(&event))
    {
        switch (event.type)
        {
        case SDL_WINDOWEVENT:
            if (event.window.windowID == windowID)
            {
                switch (event.window.event)
                {
                    /*case SDL_WINDOWEVENT_SIZE_CHANGED:  {
              width = event.window.data1;
              height = event.window.data2;
              break;
              }*/

                case SDL_WINDOWEVENT_CLOSE:
                    SDL_LogInfo(SDL_LOG_CATEGORY_APPLICATION, "Window closed\n");
                    event.type = SDL_QUIT;
                    SDL_PushEvent(&event);
                    break;

                default:
                    break;
                }
            }
            break;

        case SDL_QUIT:
            SDL_LogInfo(SDL_LOG_CATEGORY_APPLICATION, "SDL_QUIT!\n");
            return 0; // 0 -- signal terminate program

        case SDL_KEYUP:
        case SDL_KEYDOWN:

            if (event.key.repeat != 0)
            {
                break;
            }

            // Because I'm lazy: e is positive or negative depending
            // on whether it's a keyup or keydown
            e = SDL_KEYDOWN == event.type ? 1 : -1;

            switch (event.key.keysym.sym)
            {
            case SDLK_w: // forward
                move.zdiff += e;
                break;
            case SDLK_s: // backward
                move.zdiff -= e;
                break;
            case SDLK_a: // left
                move.xdiff -= e;
                break;
            case SDLK_d: // right
                move.xdiff += e;
                break;
            case SDLK_t: // height up
                move.hdiff += e;
                break;
            case SDLK_g: // height down
                move.hdiff -= e;
                break;
            case SDLK_y:         // viewer-to-camera distance decrease (zoom out)
                move.cdiff -= e; // slower
                break;
            case SDLK_h:         // viewer-to-camera distance increase (zoom in)
                move.cdiff += e; // slower
                break;
            default:
                break;
            }
            break;
        case SDL_MOUSEMOTION:

            if (SDL_GetRelativeMouseMode() == SDL_TRUE)
            {
                // dir.theta += event.motion.xrel;
                // xrel: west negative, east positive
                // yrel: north negative, south positive

                // Use negative to map positive to west and negative to east (right
                // handed system)
                float const worldPixelAngle = std::atan2(float(event.motion.xrel) * mouseSensitivity, dir.c);
                dir.theta -= worldPixelAngle;
                // Ugly, but it works
                dir.theta /= 2 * M_PI_F;
                dir.theta = dir.theta - floor(dir.theta);
                dir.theta *= 2 * M_PI_F;
            }

            break;

        case SDL_MOUSEBUTTONUP:
            SDL_SetRelativeMouseMode(SDL_GetRelativeMouseMode() ? SDL_FALSE : SDL_TRUE);
            break;
        }
    }
    return 1; // 1 - signal something processed
}

// Tex must be 64x64.
// 0 <= x1 < x <= surface->w.
// 0 <= y < surface->h.
void DrawSpan(SDL_Surface* surface, SDL_Surface* tex, Eigen::Matrix4f const& viewInverse, float c, int x1, int x2,
              int y)
{
    uint32_t* dst = (static_cast<uint32_t*>(surface->pixels)) + (surface->w) * y + x1;

    int count = x2 - x1;

    y = y - surface->h / 2;   // South of screen is positive
    x1 = x1 - surface->w / 2; // West of screen is negative
    x2 = x2 - surface->w / 2; // East of screen is positive

    float const cameraHeight = viewInverse(1, 3);
    Eigen::Vector2f const directionVector = Eigen::Vector2f{viewInverse(0, 2), viewInverse(2, 2)};
    Eigen::Vector2f const normalVector = Eigen::Vector2f{viewInverse(0, 0), viewInverse(2, 0)};
    Eigen::Vector2f const position2D = Eigen::Vector2f{viewInverse(0, 3), viewInverse(2, 3)};

    //(tx, ty) = (h/y)(c*d + x*n) + pos
    // float ty = (dir.hpos / y) * (dir.c * dir.p.y() + x1 * dir.n.y()) +
    // dir.pos.y(); float tx = (dir.hpos / y) * (dir.c * dir.p.x() + x1 *
    // dir.n.x()) + dir.pos.x();
    Eigen::Vector2f t = (cameraHeight / y) * (c * directionVector + x1 * normalVector) + position2D;

    //(dx, dy) = (h/y)(n)
    // float xfrac = (dir.hpos / y) * (dir.n.x());
    // float yfrac = (dir.hpos / y) * (dir.n.y());
    Eigen::Vector2f const frac = cameraHeight * normalVector / y;

    // printf("(xfrac, yfrac) = (%lf,%lf)\n", xfrac, yfrac);

    // Assumes identical 32 bit pixel format

    // Uint16 pitch = surface->pitch;

    while (count >= 0)
    {
        /*remainder by 64 division
    (this is roughly what DOOM did
    Only they used the actual decimal 63 instead of 3F.

    Description:

    As we know, a division by 64 is a shift right by 6 bits.
    And a multiplication by 64 is a shift left by 6 bits.

    So, the remainder of a division, which would be expressed by
    x = x - 64*(x/64)

    can be converted into
    x = x - (x >> 6 << 6)
    Now, since we know that the lower 6 bits have no choice but to
    be 0, we can optimize this by doing the following:
    x = x - (x & 0xFFFFFFC0)

    which will subtract x by itself without it's lower 6 bits.

    But wait, we can optimize this further.  When we're subtracting
    x by itself with it's lower 6 bits set to 0, all we're getting
    is...the lower 6 bits.  So:

    x = x & 0x3F

    now we have a remainder division, valid at least for
    positive numbers.

    But, that's not too useful to us, is it?  I mean, we have negative
    numbers in our texture coordinates.  Take (-1, 0), for instance.
    What do we do?  We'd like our result to be (63, 0) on the texture,
    because the texture is aligned to the XY axis and starting at
    (0,0).  Therefore we count from the right for the texture instead
    of the left, for negative numbers.  We, in other words, add 64
    to the negative numbers.

    Fortunately, when casting to unsigned integers, it wraps from the highest
    number, giving us the desired behaviour.  So, first cast to int32_t to
    remove fractional part (floor) while preserving sign, then cast to uint32_t
    to get 2's complement semantics on all architectures, leading to the desired
    behaviour. Note: emscripten complains if a direct conversion to uint32_t is
    attempted and it's negative.

    Now, if we only take the first 6 bits, then
    this decrease is still present, AND we're counting backwards
    from 64!  Exactly what we want!

    Example:

    -5 (int) = 0xFFFFFFFB (uint32_t)
    0xFFFFFFFB & 0x0000003F = 0x00000003B = 59

    This operation therefore accepts any XY coordinate
    and determines where in the texture it lies.

    Only caveat: Texture is rendered upside down.  Could load them
    flipped to solve this, or subtract by 64 for positive case and
    not for the negative case.
    */
        size_t uintty = size_t(ptrdiff_t(t.y())) & 0x3F;
        size_t uinttx = size_t(ptrdiff_t(t.x())) & 0x3F;

        uint32_t* src = static_cast<uint32_t*>(tex->pixels) + uintty * 64 + uinttx;

        *dst = *src;
        // Increment
        dst++;
        t += frac;
        // tx += xfrac;
        // ty += yfrac;

        --count;
    }
}

void DrawColumn(SDL_Surface* const display, SDL_Surface const* const texTranspose, size_t const dx, ptrdiff_t const dy1,
                ptrdiff_t const dy2, float const tx, float const ty1, float const ty2)
{
    uint32_t* displayPixels = static_cast<uint32_t*>(display->pixels);
    uint32_t* texTPixels = static_cast<uint32_t*>(texTranspose->pixels);

    size_t const displayWidth = static_cast<size_t>(display->w);
    size_t const texTransposeWidth = static_cast<size_t>(texTranspose->w);
    float const texTransposeWidthF = static_cast<float>(texTranspose->w);
    size_t const texTransposeHeight = static_cast<size_t>(texTranspose->h);

    size_t const tMappedX =
        static_cast<size_t>(static_cast<ptrdiff_t>(tx * static_cast<float>(texTranspose->h))) % texTransposeHeight;

    float const yFrac = (ty2 - ty1) / float(dy2 - dy1);

    uint32_t* texTCol = texTPixels + tMappedX * texTransposeWidth;

    // size_t const mappedCurrTopY = static_cast<size_t>(std::clamp(currTopY, 0.0f, float(display->h)));
    // size_t const mappedCurrBotY = static_cast<size_t>(std::clamp(currBotY, 0.0f, float(display->h)));

    ptrdiff_t const clampDy1 = std::clamp(dy1, ptrdiff_t(0), static_cast<ptrdiff_t>(display->h));
    ptrdiff_t const clampDy2 = std::clamp(dy2, ptrdiff_t(0), static_cast<ptrdiff_t>(display->h));

    float ti = ty1 + (clampDy1 - dy1) * (ty2 - ty1) / (dy2 - dy1);

    for (ptrdiff_t di = clampDy1; di < clampDy2; ++di)
    {
        size_t const tMappedY =
            static_cast<size_t>(static_cast<ptrdiff_t>(ti * texTransposeWidthF)) % texTransposeWidth;

        *(displayPixels + displayWidth * di + dx) = *(texTCol + tMappedY);

        ti += yFrac;
    }
}

void RenderSprite(SDL_Surface* display, SDL_Surface* sprite, float const c, float const x, float const h,
                  float const z) noexcept
{

    auto const halfWidth = sprite->w / 2;

    auto const sCentre = x;
    auto const sMin = sCentre - halfWidth;
    auto const sMax = sCentre + halfWidth;

    auto const d = z;
    // auto const c = dir.c;

    // centre_x = s/d * c

    // auto const xCentre = (s / d) * c;
    auto const hTop = float(sprite->h) + h;
    auto const hBot = h;

    int const yTop = static_cast<int>((hTop * c) / d);
    int const yBot = static_cast<int>((hBot * c) / d);

    int const xMin = static_cast<int>((sMin * c) / d);
    int const xMax = static_cast<int>((sMax * c) / d);

    int const screenTop = display->h / 2 - yTop;
    // int const screenBot = display->h / 2 - yBot;
    int const screenXMin = display->w / 2 + xMin;
    // int const screenXMax = display->w / 2 + xMax;

    // TODO: replace this with custom blitting routine
    SDL_Rect finalRect;
    finalRect.h = yTop - yBot;
    finalRect.w = xMax - xMin;
    finalRect.x = screenXMin;
    finalRect.y = screenTop;

    SDL_BlitScaled(sprite, nullptr, display, &finalRect);
}

void DrawSky(GameResources* res, SDL_Surface* display)
{
    auto& dir = res->dir;

    // Render sky
    // TODO: sky texture is being rendered flipped possibly.  Need to investigate.
    for (size_t i = 0; i < static_cast<size_t>(display->w); ++i)
    {
        // Treat sky as being projected in a cylinder, with deformation happening
        // around the edges of the screen.
        // The sky is wrapped around the cylinder 4 times (every 90 degrees the
        // image repeats)

        // Recall: left side of screen corresponds to +ve
        float worldPixelAngle = dir.theta + std::atan2((display->w / 2.0f) - i, dir.c);
        if (worldPixelAngle < 0.0f)
        {
            worldPixelAngle += 2 * M_PI_F;
        }
        else if (worldPixelAngle > 2 * M_PI_F)
        { // UGLY, but it works
            worldPixelAngle /= 2 * M_PI_F;
            worldPixelAngle -= std::floor(worldPixelAngle);
            worldPixelAngle *= 2 * M_PI_F;
        }

        // float const mirroredCoord = worldPixelAngle > 180.0f ? (1.0f -
        // (worldPixelAngle - 180.0f) / 180.0f) : (worldPixelAngle / 180.0f);

        float const finalCoord = (worldPixelAngle / (M_PI_F / 2)) - std::floor(worldPixelAngle / (M_PI_F / 2));

        DrawColumn(display, res->skyTranspose, i, 0, static_cast<size_t>(display->h / 2 + 1), finalCoord, 0, 1);
    }
}

void DrawFloor(GameResources* res, SDL_Surface* display)
{
    // Render floor
    for (int i = display->h / 2 + 1; i < display->h; ++i)
    {
        DrawSpan(display, res->floor, res->viewInverse, res->dir.c, 0, display->w - 1, i);
    }
}

void DrawFPS(GameResources* res, SDL_Surface* display, uint32_t const frameTime[2])
{
    // Render FPS
    static char frameTimeString[128];

    if (frameTime[0] != 0 || frameTime[1] != 0)
    {
        static SDL_Color const colour = {255, 255, 0, 255};

        SDL_snprintf(frameTimeString, sizeof(frameTimeString), "Previous frame time: %ums",
                     frameTime[1] - frameTime[0]);

        SDL_Surface* fpsGauge = TTF_RenderText_Blended(res->font, frameTimeString, colour);

        SDL_BlitSurface(fpsGauge, nullptr, display, nullptr);

        SDL_FreeSurface(fpsGauge);
    }
}

void DrawSprites(GameResources* res, SDL_Surface* display)
{

    // Render sprites

    auto& dir = res->dir;

    // Sort them
    // std::vector<std::pair<SDL_Surface*, Eigen::Vector2f>> sprites; // get from
    // entities

    AlignedVector<std::pair<SDL_Surface*, Eigen::Vector4f>> sorted;

    for (auto const& entity : res->entities)
    {
        Eigen::Vector4f const viewSpritePos = res->view * entity.pos.homogeneous();

        sorted.emplace_back(entity.texture, viewSpritePos);
        //
    }

    // Doomguy
    // Assume doomguy is facing [-1 0]
    Eigen::Vector2f doomGuyOrient{-1, 0};
    Eigen::Vector2f playerToDoomGuy = Eigen::Vector2f{550, 50} - Eigen::Vector2f{dir.pos(0), dir.pos(2)};
    auto const dot = playerToDoomGuy.dot(doomGuyOrient);
    auto const det = playerToDoomGuy(0) * doomGuyOrient(1) - playerToDoomGuy(1) * doomGuyOrient(0);
    auto angle = std::atan2(det, dot) + (M_PI_F / 8);

    /*if (angle < 0)
    {
        angle += 2 * M_PI_F;
    }*/

    int spriteSelect = static_cast<int>(4 + std::floor((angle) / (M_PI_F / 4)));
    if (spriteSelect >= 8)
    {
        spriteSelect = 0;
    }
    if (spriteSelect < 0)
    {
        spriteSelect = 7;
    }

    // TODO: figure out the actual transformation
    Eigen::Vector4f const doomSpriteViewPos = res->view * Eigen::Vector3f{550, 0, 50}.homogeneous();

    sorted.emplace_back(res->doomGuy[spriteSelect], doomSpriteViewPos);

    // Sort back to front
    std::sort(sorted.begin(), sorted.end(),
              [](std::pair<SDL_Surface const*, Eigen::Vector4f> const& x,
                 std::pair<SDL_Surface const*, Eigen::Vector4f> const& y) { return x.second(2) > y.second(2); });

    for (auto const& [texture, viewpos] : sorted)
    {
        RenderSprite(display, texture, res->dir.c, viewpos(0), viewpos(1), viewpos(2));
    }
}

// Eigen::Vector2f clipLinePlane(Eigen::Vector2f const& p1, Eigen::Vector2f const& p2, Eigen::Vector2f const& clip) {}

std::pair<float, float> clipLine(Eigen::Vector4f const& p1, Eigen::Vector4f const& p2, float const c,
                                 float const halfWidth)
{
    Eigen::Vector4f const pH = p2 - p1;
    Eigen::Vector2f const p = Eigen::Vector2f{pH(0), pH(2)};
    Eigen::Vector2f const p1_2d = Eigen::Vector2f{p1(0), p1(2)};
    Eigen::Vector2f const p2_2d = Eigen::Vector2f{p2(0), p2(2)};

    float p1c = 0;
    float p2c = 1;

    // Eigen::Vector2f const clipLeftP = Eigen::Vector2f{-halfWidth, c};
    Eigen::Vector2f const clipLeftN = Eigen::Vector2f{c, halfWidth};
    Eigen::Vector2f const clipRightN = {-c, halfWidth}; // points INWARDS

    float const clipNSqNm = clipLeftN.squaredNorm();

    float const clipLeftInverseP = clipLeftN.dot(p) / clipNSqNm;
    float const clipLeftInverseP1 = clipLeftN.dot(p1_2d) / clipNSqNm;

    float const clipRightInverseP2 = clipRightN.dot(p2_2d) / clipNSqNm;
    float const clipRightInverseP = clipRightN.dot(p) / clipNSqNm;

    if (clipLeftInverseP1 < 0)
    {
        float const diff = -clipLeftInverseP1;
        float const t = diff / clipLeftInverseP;
        p1c = t;
    }

    if (clipRightInverseP2 < 0)
    {
        float const diff = -clipRightInverseP2;
        float const t = diff / clipRightInverseP;
        p2c = 1 + t;
    }

    // STUDY: effect of constant height, interpolation between clipped coordinates relative to unclipped
    return {p1c, p2c};
}

void DrawWall(SDL_Surface const* texture, Eigen::Vector2f const& p1, Eigen::Vector2f const& p2, float const offset,
              float const height, float const c, Eigen::Matrix4f const& view, SDL_Surface* display)
{

    // Compute p1's view space coordinates

    Eigen::Vector4f const p1EyeBot = view * Eigen::Vector4f{p1(0), offset, p1(1), 1};
    Eigen::Vector4f const p1EyeTop = view * Eigen::Vector4f{p1(0), offset + height, p1(1), 1};
    Eigen::Vector4f const p2EyeBot = view * Eigen::Vector4f{p2(0), offset, p2(1), 1};
    Eigen::Vector4f const p2EyeTop = view * Eigen::Vector4f{p2(0), offset + height, p2(1), 1};

    float const displayWidthF = static_cast<float>(display->w);

    // Eigen::Vector4f const p1ViewXClamped = clipLineViewLeft(p1ViewTop, p2ViewTop, displayWidthF);
    auto const [p1ClipTop_t, p2ClipTop_t] = clipLine(p1EyeTop, p2EyeTop, c, displayWidthF / 2);
    auto const [p1ClipBot_t, p2ClipBot_t] = clipLine(p1EyeBot, p2EyeBot, c, displayWidthF / 2);

    if (p1ClipTop_t >= p2ClipTop_t)
    {
        return;
    }

    Eigen::Vector4f const p1ClipTop = p1EyeTop + p1ClipTop_t * (p2EyeTop - p1EyeTop);
    Eigen::Vector4f const p2ClipTop = p1EyeTop + p2ClipTop_t * (p2EyeTop - p1EyeTop);
    Eigen::Vector4f const p1ClipBot = p1EyeBot + p1ClipBot_t * (p2EyeBot - p1EyeBot);
    Eigen::Vector4f const p2ClipBot = p1EyeBot + p2ClipBot_t * (p2EyeBot - p1EyeBot);

    // Perspective division
    Eigen::Vector4f const p1ProjBot = c * p1ClipBot / p1ClipBot(2);
    Eigen::Vector4f const p1ProjTop = c * p1ClipTop / p1ClipTop(2);
    Eigen::Vector4f const p2ProjBot = c * p2ClipBot / p2ClipBot(2);
    Eigen::Vector4f const p2ProjTop = c * p2ClipTop / p2ClipTop(2);

    // Select texture based on right hand rule

    // SDL_Surface const* selectedTex = p1ProjBot(0) < p2ProjBot(0) ? frontTex : backTex;

    float const p1ProjTopClampedX =
        std::clamp(std::floor(p1ProjTop(0)), -displayWidthF / 2.0f, displayWidthF / 2.0f - 1);
    float const p2ProjTopClampedX =
        std::clamp(std::floor(p2ProjTop(0)), -displayWidthF / 2.0f, displayWidthF / 2.0f - 1);

    // float const p1ProjTopXClamped = std::clamp(p1ProjTop(0), -displayWidthF, displayWidthF);
    // float const p1ProjTopXClampDiff = p1ProjTopXClamped - p1ProjTop(0);
    // float const p2ProjTopXClamped = std::clamp(p2ProjTop(0), -displayWidthF, displayWidthF);

    // Select front or back texture depending on p1View.x and p2View.x -- right
    // hand rule

    float const topXDiff = p2ProjTopClampedX - p1ProjTopClampedX;
    // float const botXDiff = p2ProjBot(0) - p1ProjBot(0);

    float const topFrac = (p2ProjTop(1) - p1ProjTop(1)) / topXDiff;
    float const botFrac = (p2ProjBot(1) - p1ProjBot(1)) / topXDiff;
    // float const texFrac = 1 / topXDiff;

    ptrdiff_t const displayWidth = static_cast<ptrdiff_t>(display->w);

    // ptrdiff_t const startColumnUnclamped = static_cast<ptrdiff_t>(p1ProjTop(0)) + displayWidth / 2;
    // ptrdiff_t const endColumnUnclamped = static_cast<ptrdiff_t>(p2ProjTop(0)) + displayWidth / 2;

    // ptrdiff_t const startColumn = std::clamp(startColumnUnclamped, ptrdiff_t(0), displayWidth);
    // ptrdiff_t const endColumn = std::clamp(endColumnUnclamped, ptrdiff_t(0), displayWidth);

    float currProjTopY = p1ProjTop(1);

    // float currProjX = p1ProjTop(0) + p1ProjTopXClampDiff;

    float currTopY = display->h / 2 - currProjTopY;

    float currBotY = display->h / 2 - p1ProjBot(1);

    // float currX = float(startColumn - startColumnUnclamped) / topXDiff;

    Eigen::Vector4f const wallDirUnnormalized4 = p2EyeTop - p1EyeTop;
    Eigen::Vector2f const wallDirUnnormalized = Eigen::Vector2f{wallDirUnnormalized4(0), wallDirUnnormalized4(2)};
    float const wallDirSqNm = wallDirUnnormalized.squaredNorm();

    for (float it = p1ProjTopClampedX; it <= p2ProjTopClampedX; it++)
    {
        float const depth_i = c * p1ClipTop(1) / currProjTopY;
        float const disp_i = depth_i * it / c;

        Eigen::Vector2f const projPtUnnormalized =
            Eigen::Vector2f{disp_i, depth_i} - Eigen::Vector2f{p1ClipBot(0), p1ClipBot(2)};

        float const texT = p1ClipTop_t + wallDirUnnormalized.dot(projPtUnnormalized) / wallDirSqNm;

        ptrdiff_t const i = static_cast<ptrdiff_t>(it) + displayWidth / 2;

        DrawColumn(display, texture, i, static_cast<ptrdiff_t>(currTopY), static_cast<ptrdiff_t>(currBotY), texT, 0, 1);

        currTopY -= topFrac;
        currProjTopY += topFrac;
        currBotY -= botFrac;
        //++currProjX;
        // currX += texFrac;
    }

    return;
}

void DrawTwoSidedWall(SDL_Surface const* frontTex, SDL_Surface const* backTex, Eigen::Vector2f const& p1,
                      Eigen::Vector2f const& p2, float const offset, float const height, float const c,
                      Eigen::Matrix4f const& view, SDL_Surface* display)
{
    // Compute p1's view space coordinates

    // Eigen::Vector4f const p1EyeBot = view * Eigen::Vector4f{p1(0), offset, p1(1), 1};
    Eigen::Vector4f const p1EyeTop = view * Eigen::Vector4f{p1(0), offset + height, p1(1), 1};
    // Eigen::Vector4f const p2EyeBot = view * Eigen::Vector4f{p2(0), offset, p2(1), 1};
    Eigen::Vector4f const p2EyeTop = view * Eigen::Vector4f{p2(0), offset + height, p2(1), 1};

    Eigen::Vector4f const P = p2EyeTop - p1EyeTop;
    Eigen::Vector2f const N = {-P(2), P(0)};
    Eigen::Vector2f const P1 = {p1EyeTop(0), p1EyeTop(2)};

    float const whichSide = P1.dot(N) / N.squaredNorm();

    if (whichSide > 0)
    {
        DrawWall(frontTex, p1, p2, offset, height, c, view, display);
    }
    else
    {
        DrawWall(backTex, p2, p1, offset, height, c, view, display);
    }
}

void Draw(GameResources* res, uint32_t const frameTime[2])
{
    SDL_Surface* display = res->display;

    DrawSky(res, display);
    DrawFloor(res, display);
    DrawTwoSidedWall(res->wallFrontTranspose, res->wallBackTranspose, Eigen::Vector2f{-300, -300},
                     Eigen::Vector2f{300, -300}, 0, 100, res->dir.c, res->view, display);
    DrawTwoSidedWall(res->wallFrontTranspose, res->wallBackTranspose, Eigen::Vector2f{300, -300},
                     Eigen::Vector2f{700, -100}, 0, 150, res->dir.c, res->view, display);
    DrawTwoSidedWall(res->wallFrontTranspose, res->wallBackTranspose, Eigen::Vector2f{700, -100},
                     Eigen::Vector2f{700, 400}, 0, 150, res->dir.c, res->view, display);
    DrawTwoSidedWall(res->wallFrontTranspose, res->wallBackTranspose, Eigen::Vector2f{700, 400},
                     Eigen::Vector2f{300, 600}, 0, 150, res->dir.c, res->view, display);
    DrawTwoSidedWall(res->wallFrontTranspose, res->wallBackTranspose, Eigen::Vector2f{300, 600},
                     Eigen::Vector2f{-300, 600}, 0, 100, res->dir.c, res->view, display);
    DrawSprites(res, display);
    DrawFPS(res, display, frameTime);
}

void GameLoop(void* const arg)
{
    GameResources* const res = static_cast<GameResources* const>(arg);

    static uint32_t prevFrameTime[2] = {};

    prevFrameTime[0] = prevFrameTime[1];
    prevFrameTime[1] = SDL_GetTicks();

    if (ProcessEvent(res->dir, res->move, res->windowID) == 0)
    {
        // FreeGameResources(res); //browser specific: it handles cleanup
        emscripten_cancel_main_loop();
        SDL_LogInfo(SDL_LOG_CATEGORY_APPLICATION, "Exiting!\n");
        SDL_Quit();
        std::exit(0); // Reason for not plain returning: emscripten won't call
                      // global destructors when main or the main loop exits.
    }
    UpdateViewMatrices(res->dir, res->viewInverse, res->view);
    MovePlayer(res->dir, res->viewInverse, res->move, prevFrameTime[1] - prevFrameTime[0]);

    Draw(res, prevFrameTime);
    SDL_UpdateTexture(res->texture, nullptr, res->display->pixels, res->display->pitch);
    // Now draw this directly to the window
    SDL_RenderClear(res->renderer);
    SDL_RenderCopy(res->renderer, res->texture, nullptr, nullptr);
    // And ask the windowing system to redraw the window
    SDL_RenderPresent(res->renderer);

    // SDL_LogInfo(SDL_LOG_CATEGORY_APPLICATION, "Prev frame time: %u, current
    // present: %u", prevFrameTime[1] - prevFrameTime[0], SDL_GetTicks() -
    // prevFrameTime[1]);

    // MovePlayer(SDL_GetTicks() - prevFrameTime[1]);
    // MovePlayer(prevFrameTime[1] - prevFrameTime[0]);
    // MovePlayer(16);
}

SDL_Surface* LoadTexture(char const* filename, SDL_PixelFormat const* const dstformat)
{
    SDL_Surface* tmp = SDL_LoadBMP(filename);

    if (tmp == nullptr)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "Could not load %s!\n", filename);
        throw std::runtime_error("Couldn't load texture");
    }

    SDL_Surface* result = SDL_ConvertSurface(tmp, dstformat, 0);
    if (result == nullptr)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "Could not convert surface for %s!\n", filename);
        throw std::runtime_error("Couldn't load texture");
    }

    SDL_FreeSurface(tmp);

    return result;
}

int main(int, char* [])
{
    // VERY IMPORTANT: Ensure SDL2 is initialized
    if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_TIMER | SDL_INIT_EVENTS) < 0)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "could not initialize sdl2: %s\n", SDL_GetError());
        return 1;
    }

    // VERY IMPORTANT: if using text in your program, ensure SDL2_ttf library is
    // initialized
    if (TTF_Init() < 0)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "could not initialize SDL2_ttf: %s\n", TTF_GetError());
    }

    GameResources res;

    // This creates the actual window in which graphics are displayed
    res.window = SDL_CreateWindow("Floor - SDL2 version", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, 640, 480,
                                  SDL_WINDOW_SHOWN);

    if (res.window == nullptr)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "Could not create window!\n");
        return 1;
    }

    res.windowID = SDL_GetWindowID(res.window);

    res.renderer = SDL_CreateRenderer(res.window, -1,
                                      0); // don't force hardware or software
    // accel -- let SDL2 choose what's best

    if (res.renderer == nullptr)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "could not create renderer: %s\n", SDL_GetError());
        return 1;
    }

    int xres{};
    int yres{};
    if (SDL_GetRendererOutputSize(res.renderer, &xres, &yres) < 0)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "could not get renderer output size: %s\n", SDL_GetError());
        return 1;
    }

    res.display = SDL_CreateRGBSurfaceWithFormat(0, xres, yres, 32, SDL_PIXELFORMAT_RGBA8888);
    if (res.display == nullptr)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "could not create surface: %s\n", SDL_GetError());
        return 1;
    }

    res.texture = SDL_CreateTexture(res.renderer, SDL_PIXELFORMAT_RGBA8888, SDL_TEXTUREACCESS_STREAMING, res.display->w,
                                    res.display->h);

    if (res.texture == nullptr)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "could not create texture: %s\n", SDL_GetError());
        return 1;
    }

    // Just in case you need text:
    // load iosevka-regular.ttf at a large size into font
    res.font = TTF_OpenFont("../assets/iosevka-regular.ttf", 16);
    if (res.font == nullptr)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "TTF_OpenFont: %s\n", TTF_GetError());
        return 1;
    }

    res.floor = LoadTexture("../assets/floor.bmp", res.display->format);
    res.skyTranspose = LoadTexture("../assets/sky.bmp", res.display->format);
    res.wallFrontTranspose = LoadTexture("../assets/wallFront.bmp", res.display->format);
    res.wallBackTranspose = LoadTexture("../assets/wallBack.bmp", res.display->format);
    res.healthSprite = LoadTexture("../assets/health.bmp", res.display->format);
    res.vialSprite = LoadTexture("../assets/vial.bmp", res.display->format);
    res.columnSprite = LoadTexture("../assets/column.bmp", res.display->format);

    char pathBuf[128];
    for (size_t i = 0; i < 8; i++)
    {
        snprintf(pathBuf, 128, "../assets/playa%zu.bmp", i + 1);
        res.doomGuy[i] = LoadTexture(pathBuf, res.display->format);
    }

    if (SDL_SetSurfaceBlendMode(res.display, SDL_BLENDMODE_BLEND) < 0)
    {
        SDL_LogError(SDL_LOG_CATEGORY_APPLICATION, "SDL_SetSurfaceBlendMode: %s\n", SDL_GetError());
        return 1;
    }

    // Objects
    res.entities.emplace_back("column1", res.columnSprite, Eigen::Vector3f{500, 0, 0});
    res.entities.emplace_back("vial", res.vialSprite, Eigen::Vector3f{500, 25, 100});
    res.entities.emplace_back("health", res.healthSprite, Eigen::Vector3f{500, 0, 200});
    res.entities.emplace_back("column2", res.columnSprite, Eigen::Vector3f{500, 0, 300});

    emscripten_set_main_loop_arg(GameLoop, &res, 0, 1);

    return 0;
}