// Boost.Geometry

// Copyright (c) 2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP
#define BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/spherical.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The intersection of two great circles as proposed by Sjoberg.
\see See
    - [Sjoberg02] Lars E. Sjoberg, Intersections on the sphere and ellipsoid, 2002
      http://link.springer.com/article/10.1007/s00190-001-0230-9
*/
template <typename CT>
struct sjoberg_intersection_spherical_02
{
    // TODO: if it will be used as standalone formula
    //       support segments on equator and endpoints on poles

    static inline bool apply(CT const& lon1, CT const& lat1, CT const& lon_a2, CT const& lat_a2,
                             CT const& lon2, CT const& lat2, CT const& lon_b2, CT const& lat_b2,
                             CT & lon, CT & lat)
    {
        CT tan_lat = 0;
        bool res = apply_alt(lon1, lat1, lon_a2, lat_a2,
                             lon2, lat2, lon_b2, lat_b2,
                             lon, tan_lat);

        if (res)
        {
            lat = atan(tan_lat);
        }

        return res;
    }

    static inline bool apply_alt(CT const& lon1, CT const& lat1, CT const& lon_a2, CT const& lat_a2,
                                 CT const& lon2, CT const& lat2, CT const& lon_b2, CT const& lat_b2,
                                 CT & lon, CT & tan_lat)
    {
        CT const cos_lon1 = cos(lon1);
        CT const sin_lon1 = sin(lon1);
        CT const cos_lon2 = cos(lon2);
        CT const sin_lon2 = sin(lon2);
        CT const sin_lat1 = sin(lat1);
        CT const sin_lat2 = sin(lat2);
        CT const cos_lat1 = cos(lat1);
        CT const cos_lat2 = cos(lat2);

        CT const tan_lat_a2 = tan(lat_a2);
        CT const tan_lat_b2 = tan(lat_b2);

        return apply(lon1, lon_a2, lon2, lon_b2,
                     sin_lon1, cos_lon1, sin_lat1, cos_lat1,
                     sin_lon2, cos_lon2, sin_lat2, cos_lat2,
                     tan_lat_a2, tan_lat_b2,
                     lon, tan_lat);
    }

private:
    static inline bool apply(CT const& lon1, CT const& lon_a2, CT const& lon2, CT const& lon_b2,
                             CT const& sin_lon1, CT const& cos_lon1, CT const& sin_lat1, CT const& cos_lat1,
                             CT const& sin_lon2, CT const& cos_lon2, CT const& sin_lat2, CT const& cos_lat2,
                             CT const& tan_lat_a2, CT const& tan_lat_b2,
                             CT & lon, CT & tan_lat)
    {
        // NOTE:
        // cos_lat_ = 0 <=> segment on equator
        // tan_alpha_ = 0 <=> segment vertical

        CT const tan_lat1 = sin_lat1 / cos_lat1; //tan(lat1);
        CT const tan_lat2 = sin_lat2 / cos_lat2; //tan(lat2);

        CT const dlon1 = lon_a2 - lon1;
        CT const sin_dlon1 = sin(dlon1);
        CT const dlon2 = lon_b2 - lon2;
        CT const sin_dlon2 = sin(dlon2);

        CT const c0 = 0;
        bool const is_vertical1 = math::equals(sin_dlon1, c0);
        bool const is_vertical2 = math::equals(sin_dlon2, c0);

        CT tan_alpha1 = 0;
        CT tan_alpha2 = 0;

        if (is_vertical1 && is_vertical2)
        {
            // circles intersect at one of the poles or are collinear
            return false;
        }
        else if (is_vertical1)
        {
            CT const cos_dlon2 = cos(dlon2);
            CT const tan_alpha2_x = cos_lat2 * tan_lat_b2 - sin_lat2 * cos_dlon2;
            tan_alpha2 = sin_dlon2 / tan_alpha2_x;

            lon = lon1;
        }
        else if (is_vertical2)
        {
            CT const cos_dlon1 = cos(dlon1);
            CT const tan_alpha1_x = cos_lat1 * tan_lat_a2 - sin_lat1 * cos_dlon1;
            tan_alpha1 = sin_dlon1 / tan_alpha1_x;

            lon = lon2;
        }
        else
        {
            CT const cos_dlon1 = cos(dlon1);
            CT const cos_dlon2 = cos(dlon2);

            CT const tan_alpha1_x = cos_lat1 * tan_lat_a2 - sin_lat1 * cos_dlon1;
            CT const tan_alpha2_x = cos_lat2 * tan_lat_b2 - sin_lat2 * cos_dlon2;
            tan_alpha1 = sin_dlon1 / tan_alpha1_x;
            tan_alpha2 = sin_dlon2 / tan_alpha2_x;

            CT const T1 = tan_alpha1 * cos_lat1;
            CT const T2 = tan_alpha2 * cos_lat2;
            CT const T1T2 = T1*T2;
            CT const tan_lon_y = T1 * sin_lon2 - T2 * sin_lon1 + T1T2 * (tan_lat1 * cos_lon1 - tan_lat2 * cos_lon2);
            CT const tan_lon_x = T1 * cos_lon2 - T2 * cos_lon1 - T1T2 * (tan_lat1 * sin_lon1 - tan_lat2 * sin_lon2);

            lon = atan2(tan_lon_y, tan_lon_x);
        }

        // choose closer result
        CT const pi = math::pi<CT>();
        CT const lon_2 = lon > c0 ? lon - pi : lon + pi;
        CT const lon_dist1 = (std::max)((std::min)(math::longitude_difference<radian>(lon1, lon),
                                                   math::longitude_difference<radian>(lon_a2, lon)),
                                        (std::min)(math::longitude_difference<radian>(lon2, lon),
                                                   math::longitude_difference<radian>(lon_b2, lon)));
        CT const lon_dist2 = (std::max)((std::min)(math::longitude_difference<radian>(lon1, lon_2),
                                                   math::longitude_difference<radian>(lon_a2, lon_2)),
                                        (std::min)(math::longitude_difference<radian>(lon2, lon_2),
                                                   math::longitude_difference<radian>(lon_b2, lon_2)));
        if (lon_dist2 < lon_dist1)
        {
            lon = lon_2;
        }

        CT const sin_lon = sin(lon);
        CT const cos_lon = cos(lon);

        if (math::abs(tan_alpha1) >= math::abs(tan_alpha2)) // pick less vertical segment
        {
            CT const sin_dlon_1 = sin_lon * cos_lon1 - cos_lon * sin_lon1;
            CT const cos_dlon_1 = cos_lon * cos_lon1 + sin_lon * sin_lon1;
            CT const lat_y_1 = sin_dlon_1 + tan_alpha1 * sin_lat1 * cos_dlon_1;
            CT const lat_x_1 = tan_alpha1 * cos_lat1;
            tan_lat = lat_y_1 / lat_x_1;
        }
        else
        {
            CT const sin_dlon_2 = sin_lon * cos_lon2 - cos_lon * sin_lon2;
            CT const cos_dlon_2 = cos_lon * cos_lon2 + sin_lon * sin_lon2;
            CT const lat_y_2 = sin_dlon_2 + tan_alpha2 * sin_lat2 * cos_dlon_2;
            CT const lat_x_2 = tan_alpha2 * cos_lat2;
            tan_lat = lat_y_2 / lat_x_2;
        }

        return true;
    }
};

/*! Approximation of dLambda_j [Sjoberg07], expanded into taylor series in e^2
    Maxima script:
    dLI_j(c_j, sinB_j, sinB) := integrate(1 / (sqrt(1 - c_j ^ 2 - x ^ 2)*(1 + sqrt(1 - e2*(1 - x ^ 2)))), x, sinB_j, sinB);
    dL_j(c_j, B_j, B) := -e2 * c_j * dLI_j(c_j, B_j, B);
    S: taylor(dLI_j(c_j, sinB_j, sinB), e2, 0, 3);
    assume(c_j < 1);
    assume(c_j > 0);
    L1: factor(integrate(sqrt(-x ^ 2 - c_j ^ 2 + 1) / (x ^ 2 + c_j ^ 2 - 1), x));
    L2: factor(integrate(((x ^ 2 - 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
    L3: factor(integrate(((x ^ 4 - 2 * x ^ 2 + 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));
    L4: factor(integrate(((x ^ 6 - 3 * x ^ 4 + 3 * x ^ 2 - 1)*sqrt(-x ^ 2 - c_j ^ 2 + 1)) / (x ^ 2 + c_j ^ 2 - 1), x));

\see See
    - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
      http://link.springer.com/article/10.1007/s00190-007-0204-7
*/
template <unsigned int Order, typename CT>
inline CT sjoberg_d_lambda_e_sqr(CT const& sin_betaj, CT const& sin_beta,
                                 CT const& Cj, CT const& sqrt_1_Cj_sqr,
                                 CT const& e_sqr)
{
    using math::detail::bounded;

    if (Order == 0)
    {
        return 0;
    }

    CT const c1 = 1;
    CT const c2 = 2;

    CT const asin_B = asin(bounded(sin_beta / sqrt_1_Cj_sqr, -c1, c1));
    CT const asin_Bj = asin(sin_betaj / sqrt_1_Cj_sqr);
    CT const L0 = (asin_B - asin_Bj) / c2;

    if (Order == 1)
    {
        return -Cj * e_sqr * L0;
    }

    CT const c0 = 0;
    CT const c16 = 16;

    CT const X = sin_beta;
    CT const Xj = sin_betaj;
    CT const X_sqr = math::sqr(X);
    CT const Xj_sqr = math::sqr(Xj);
    CT const Cj_sqr = math::sqr(Cj);
    CT const Cj_sqr_plus_one = Cj_sqr + c1;
    CT const one_minus_Cj_sqr = c1 - Cj_sqr;
    CT const sqrt_Y = math::sqrt(bounded(-X_sqr + one_minus_Cj_sqr, c0));
    CT const sqrt_Yj = math::sqrt(-Xj_sqr + one_minus_Cj_sqr);
    CT const L1 = (Cj_sqr_plus_one * (asin_B - asin_Bj) + X * sqrt_Y - Xj * sqrt_Yj) / c16;

    if (Order == 2)
    {
        return -Cj * e_sqr * (L0 + e_sqr * L1);
    }

    CT const c3 = 3;
    CT const c5 = 5;
    CT const c128 = 128;

    CT const E = Cj_sqr * (c3 * Cj_sqr + c2) + c3;
    CT const F = X * (-c2 * X_sqr + c3 * Cj_sqr + c5);
    CT const Fj = Xj * (-c2 * Xj_sqr + c3 * Cj_sqr + c5);
    CT const L2 = (E * (asin_B - asin_Bj) + F * sqrt_Y - Fj * sqrt_Yj) / c128;

    if (Order == 3)
    {
        return -Cj * e_sqr * (L0 + e_sqr * (L1 + e_sqr * L2));
    }

    CT const c8 = 8;
    CT const c9 = 9;
    CT const c10 = 10;
    CT const c15 = 15;
    CT const c24 = 24;
    CT const c26 = 26;
    CT const c33 = 33;
    CT const c6144 = 6144;

    CT const G = Cj_sqr * (Cj_sqr * (Cj_sqr * c15 + c9) + c9) + c15;
    CT const H = -c10 * Cj_sqr - c26;
    CT const I = Cj_sqr * (Cj_sqr * c15 + c24) + c33;
    CT const J = X_sqr * (X * (c8 * X_sqr + H)) + X * I;
    CT const Jj = Xj_sqr * (Xj * (c8 * Xj_sqr + H)) + Xj * I;
    CT const L3 = (G * (asin_B - asin_Bj) + J * sqrt_Y - Jj * sqrt_Yj) / c6144;

    // Order 4 and higher
    return -Cj * e_sqr * (L0 + e_sqr * (L1 + e_sqr * (L2 + e_sqr * L3)));
}

/*!
\brief The representation of geodesic as proposed by Sjoberg.
\see See
    - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
      http://link.springer.com/article/10.1007/s00190-007-0204-7
    - [Sjoberg12] Lars E. Sjoberg, Solutions to the ellipsoidal Clairaut constant
      and the inverse geodetic problem by numerical integration, 2012
      https://www.degruyter.com/view/j/jogs.2012.2.issue-3/v10156-011-0037-4/v10156-011-0037-4.xml
*/
template <typename CT, unsigned int Order>
class sjoberg_geodesic
{
    sjoberg_geodesic() {}

    static int sign_C(CT const& alphaj)
    {
        CT const c0 = 0;
        CT const c2 = 2;
        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;

        return (pi_half < alphaj && alphaj < pi) || (-pi_half < alphaj && alphaj < c0) ? -1 : 1;
    }

public:
    sjoberg_geodesic(CT const& lon, CT const& lat, CT const& alpha, CT const& f)
        : lonj(lon)
        , latj(lat)
        , alphaj(alpha)
    {
        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;
        //CT const pi = math::pi<CT>();
        //CT const pi_half = pi / c2;

        one_minus_f = c1 - f;
        e_sqr = f * (c2 - f);

        tan_latj = tan(lat);
        tan_betaj = one_minus_f * tan_latj;
        betaj = atan(tan_betaj);
        sin_betaj = sin(betaj);

        cos_betaj = cos(betaj);
        sin_alphaj = sin(alphaj);
        // Clairaut constant (lower-case in the paper)
        Cj = sign_C(alphaj) * cos_betaj * sin_alphaj;
        Cj_sqr = math::sqr(Cj);
        sqrt_1_Cj_sqr = math::sqrt(c1 - Cj_sqr);

        sign_lon_diff = alphaj >= 0 ? 1 : -1; // || alphaj == -pi ?
        //sign_lon_diff = 1;

        is_on_equator = math::equals(sqrt_1_Cj_sqr, c0);
        is_Cj_zero = math::equals(Cj, c0);

        t0j = c0;
        asin_tj_t0j = c0;

        if (! is_Cj_zero)
        {
            t0j = sqrt_1_Cj_sqr / Cj;
        }

        if (! is_on_equator)
        {
            //asin_tj_t0j = asin(tan_betaj / t0j);
            asin_tj_t0j = asin(tan_betaj * Cj / sqrt_1_Cj_sqr);
        }
    }

    struct vertex_data
    {
        //CT beta0j;
        CT sin_beta0j;
        CT dL0j;
        CT lon0j;
    };

    vertex_data get_vertex_data() const
    {
        CT const c2 = 2;
        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;

        vertex_data res;

        if (! is_Cj_zero)
        {
            //res.beta0j = atan(t0j);
            //res.sin_beta0j = sin(res.beta0j);
            res.sin_beta0j = math::sign(t0j) * sqrt_1_Cj_sqr;
            res.dL0j = d_lambda(res.sin_beta0j);
            res.lon0j = lonj + sign_lon_diff * (pi_half - asin_tj_t0j + res.dL0j);
        }
        else
        {
            //res.beta0j = pi_half;
            //res.sin_beta0j = betaj >= 0 ? 1 : -1;
            res.sin_beta0j = 1;
            res.dL0j = 0;
            res.lon0j = lonj;
        }

        return res;
    }

    bool is_sin_beta_ok(CT const& sin_beta) const
    {
        CT const c1 = 1;
        return math::abs(sin_beta / sqrt_1_Cj_sqr) <= c1;
    }

    bool k_diff(CT const& sin_beta,
                CT & delta_k) const
    {
        if (is_Cj_zero)
        {
            delta_k = 0;
            return true;
        }

        // beta out of bounds and not close
        if (! (is_sin_beta_ok(sin_beta)
                || math::equals(math::abs(sin_beta), sqrt_1_Cj_sqr)) )
        {
            return false;
        }

        // NOTE: beta may be slightly out of bounds here but d_lambda handles that
        CT const dLj = d_lambda(sin_beta);
        delta_k = sign_lon_diff * (/*asin_t_t0j*/ - asin_tj_t0j + dLj);

        return true;
    }

    bool lon_diff(CT const& sin_beta, CT const& t,
                  CT & delta_lon) const
    {
        using math::detail::bounded;
        CT const c1 = 1;

        if (is_Cj_zero)
        {
            delta_lon = 0;
            return true;
        }

        CT delta_k = 0;
        if (! k_diff(sin_beta, delta_k))
        {
            return false;
        }

        CT const t_t0j = t / t0j;
        // NOTE: t may be slightly out of bounds here
        CT const asin_t_t0j = asin(bounded(t_t0j, -c1, c1));
        delta_lon = sign_lon_diff * asin_t_t0j + delta_k;

        return true;
    }

    bool k_diffs(CT const& sin_beta, vertex_data const& vd,
                 CT & delta_k_before, CT & delta_k_behind,
                 bool check_sin_beta = true) const
    {
        CT const pi = math::pi<CT>();

        if (is_Cj_zero)
        {
            delta_k_before = 0;
            delta_k_behind = sign_lon_diff * pi;
            return true;
        }

        // beta out of bounds and not close
        if (check_sin_beta
            && ! (is_sin_beta_ok(sin_beta)
                    || math::equals(math::abs(sin_beta), sqrt_1_Cj_sqr)) )
        {
            return false;
        }

        // NOTE: beta may be slightly out of bounds here but d_lambda handles that
        CT const dLj = d_lambda(sin_beta);
        delta_k_before = sign_lon_diff * (/*asin_t_t0j*/ - asin_tj_t0j + dLj);

        // This version require no additional dLj calculation
        delta_k_behind = sign_lon_diff * (pi /*- asin_t_t0j*/ - asin_tj_t0j + vd.dL0j + (vd.dL0j - dLj));

        // [Sjoberg12]
        //CT const dL101 = d_lambda(sin_betaj, vd.sin_beta0j);
        // WARNING: the following call might not work if beta was OoB because only the second argument is bounded
        //CT const dL_01 = d_lambda(sin_beta, vd.sin_beta0j);
        //delta_k_behind = sign_lon_diff * (pi /*- asin_t_t0j*/ - asin_tj_t0j + dL101 + dL_01);

        return true;
    }

    bool lon_diffs(CT const& sin_beta, CT const& t, vertex_data const& vd,
                   CT & delta_lon_before, CT & delta_lon_behind) const
    {
        using math::detail::bounded;
        CT const c1 = 1;
        CT const pi = math::pi<CT>();

        if (is_Cj_zero)
        {
            delta_lon_before = 0;
            delta_lon_behind = sign_lon_diff * pi;
            return true;
        }

        CT delta_k_before = 0, delta_k_behind = 0;
        if (! k_diffs(sin_beta, vd, delta_k_before, delta_k_behind))
        {
            return false;
        }

        CT const t_t0j = t / t0j;
        // NOTE: t may be slightly out of bounds here
        CT const asin_t_t0j = asin(bounded(t_t0j, -c1, c1));
        CT const sign_asin_t_t0j = sign_lon_diff * asin_t_t0j;
        delta_lon_before = sign_asin_t_t0j + delta_k_before;
        delta_lon_behind = -sign_asin_t_t0j + delta_k_behind;

        return true;
    }

    bool lon(CT const& sin_beta, CT const& t, vertex_data const& vd,
             CT & lon_before, CT & lon_behind) const
    {
        using math::detail::bounded;
        CT const c1 = 1;
        CT const pi = math::pi<CT>();

        if (is_Cj_zero)
        {
            lon_before = lonj;
            lon_behind = lonj + sign_lon_diff * pi;
            return true;
        }

        if (! (is_sin_beta_ok(sin_beta)
                || math::equals(math::abs(sin_beta), sqrt_1_Cj_sqr)) )
        {
            return false;
        }

        CT const t_t0j = t / t0j;
        CT const asin_t_t0j = asin(bounded(t_t0j, -c1, c1));
        CT const dLj = d_lambda(sin_beta);
        lon_before = lonj + sign_lon_diff * (asin_t_t0j - asin_tj_t0j + dLj);
        lon_behind = vd.lon0j + (vd.lon0j - lon_before);

        return true;
    }

    CT lon(CT const& delta_lon) const
    {
        return lonj + delta_lon;
    }

    CT lat(CT const& t) const
    {
        // t = tan(beta) = (1-f)tan(lat)
        return atan(t / one_minus_f);
    }

    void vertex(CT & lon, CT & lat) const
    {
        lon = get_vertex_data().lon0j;
        if (! is_Cj_zero)
        {
            lat = sjoberg_geodesic::lat(t0j);
        }
        else
        {
            CT const c2 = 2;
            lat = math::pi<CT>() / c2;
        }
    }

    CT lon_of_equator_intersection() const
    {
        CT const c0 = 0;
        CT const dLj = d_lambda(c0);
        CT const asin_tj_t0j = asin(Cj * tan_betaj / sqrt_1_Cj_sqr);
        return lonj - asin_tj_t0j + dLj;
    }

    CT d_lambda(CT const& sin_beta) const
    {
        return sjoberg_d_lambda_e_sqr<Order>(sin_betaj, sin_beta, Cj, sqrt_1_Cj_sqr, e_sqr);
    }

    // [Sjoberg12]
    /*CT d_lambda(CT const& sin_beta1, CT const& sin_beta2) const
    {
        return sjoberg_d_lambda_e_sqr<Order>(sin_beta1, sin_beta2, Cj, sqrt_1_Cj_sqr, e_sqr);
    }*/

    CT lonj;
    CT latj;
    CT alphaj;

    CT one_minus_f;
    CT e_sqr;

    CT tan_latj;
    CT tan_betaj;
    CT betaj;
    CT sin_betaj;
    CT cos_betaj;
    CT sin_alphaj;
    CT Cj;
    CT Cj_sqr;
    CT sqrt_1_Cj_sqr;

    int sign_lon_diff;

    bool is_on_equator;
    bool is_Cj_zero;

    CT t0j;
    CT asin_tj_t0j;
};

/*!
\brief The intersection of two geodesics as proposed by Sjoberg.
\see See
    - [Sjoberg02] Lars E. Sjoberg, Intersections on the sphere and ellipsoid, 2002
      http://link.springer.com/article/10.1007/s00190-001-0230-9
    - [Sjoberg07] Lars E. Sjoberg, Geodetic intersection on the ellipsoid, 2007
      http://link.springer.com/article/10.1007/s00190-007-0204-7
    - [Sjoberg12] Lars E. Sjoberg, Solutions to the ellipsoidal Clairaut constant
      and the inverse geodetic problem by numerical integration, 2012
      https://www.degruyter.com/view/j/jogs.2012.2.issue-3/v10156-011-0037-4/v10156-011-0037-4.xml
*/
template
<
    typename CT,
    template <typename, bool, bool, bool, bool, bool> class Inverse,
    unsigned int Order = 4
>
class sjoberg_intersection
{
    typedef sjoberg_geodesic<CT, Order> geodesic_type;
    typedef Inverse<CT, false, true, false, false, false> inverse_type;
    typedef typename inverse_type::result_type inverse_result;

    static bool const enable_02 = true;
    static int const max_iterations_02 = 10;
    static int const max_iterations_07 = 20;

public:
    template <typename T1, typename T2, typename Spheroid>
    static inline bool apply(T1 const& lona1, T1 const& lata1,
                             T1 const& lona2, T1 const& lata2,
                             T2 const& lonb1, T2 const& latb1,
                             T2 const& lonb2, T2 const& latb2,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        CT const lon_a1 = lona1;
        CT const lat_a1 = lata1;
        CT const lon_a2 = lona2;
        CT const lat_a2 = lata2;
        CT const lon_b1 = lonb1;
        CT const lat_b1 = latb1;
        CT const lon_b2 = lonb2;
        CT const lat_b2 = latb2;

        inverse_result const res1 = inverse_type::apply(lon_a1, lat_a1, lon_a2, lat_a2, spheroid);
        inverse_result const res2 = inverse_type::apply(lon_b1, lat_b1, lon_b2, lat_b2, spheroid);

        return apply(lon_a1, lat_a1, lon_a2, lat_a2, res1.azimuth,
                     lon_b1, lat_b1, lon_b2, lat_b2, res2.azimuth,
                     lon, lat, spheroid);
    }

    // TODO: Currently may not work correctly if one of the endpoints is the pole
    template <typename Spheroid>
    static inline bool apply(CT const& lon_a1, CT const& lat_a1, CT const& lon_a2, CT const& lat_a2, CT const& alpha_a1,
                             CT const& lon_b1, CT const& lat_b1, CT const& lon_b2, CT const& lat_b2, CT const& alpha_b1,
                             CT & lon, CT & lat,
                             Spheroid const& spheroid)
    {
        // coordinates in radians

        CT const c0 = 0;
        CT const c1 = 1;

        CT const f = formula::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;

        geodesic_type geod1(lon_a1, lat_a1, alpha_a1, f);
        geodesic_type geod2(lon_b1, lat_b1, alpha_b1, f);

        // Cj = 1 if on equator <=> sqrt_1_Cj_sqr = 0
        // Cj = 0 if vertical <=> sqrt_1_Cj_sqr = 1

        if (geod1.is_on_equator && geod2.is_on_equator)
        {
            return false;
        }
        else if (geod1.is_on_equator)
        {
            lon = geod2.lon_of_equator_intersection();
            lat = c0;
            return true;
        }
        else if (geod2.is_on_equator)
        {
            lon = geod1.lon_of_equator_intersection();
            lat = c0;
            return true;
        }

        // (lon1 - lon2) normalized to (-180, 180]
        CT const lon1_minus_lon2 = math::longitude_distance_signed<radian>(geod2.lonj, geod1.lonj);

        // vertical segments
        if (geod1.is_Cj_zero && geod2.is_Cj_zero)
        {
            CT const pi = math::pi<CT>();

            // the geodesics are parallel, the intersection point cannot be calculated
            if ( math::equals(lon1_minus_lon2, c0)
              || math::equals(lon1_minus_lon2 + (lon1_minus_lon2 < c0 ? pi : -pi), c0) )
            {
                return false;
            }

            lon = c0;

            // the geodesics intersect at one of the poles
            CT const pi_half = pi / CT(2);
            CT const abs_lat_a1 = math::abs(lat_a1);
            CT const abs_lat_a2 = math::abs(lat_a2);
            if (math::equals(abs_lat_a1, abs_lat_a2))
            {
                lat = pi_half;
            }
            else
            {
                // pick the pole closest to one of the points of the first segment
                CT const& closer_lat = abs_lat_a1 > abs_lat_a2 ? lat_a1 : lat_a2;
                lat = closer_lat >= 0 ? pi_half : -pi_half;
            }

            return true;
        }

        CT lon_sph = 0;

        // Starting tan(beta)
        CT t = 0;

        /*if (geod1.is_Cj_zero)
        {
            CT const k_base = lon1_minus_lon2 + geod2.sign_lon_diff * geod2.asin_tj_t0j;
            t = sin(k_base) * geod2.t0j;
            lon_sph = vertical_intersection_longitude(geod1.lonj, lon_b1, lon_b2);
        }
        else if (geod2.is_Cj_zero)
        {
            CT const k_base = lon1_minus_lon2 - geod1.sign_lon_diff * geod1.asin_tj_t0j;
            t = sin(-k_base) * geod1.t0j;
            lon_sph = vertical_intersection_longitude(geod2.lonj, lon_a1, lon_a2);
        }
        else*/
        {
            // TODO: Consider using betas instead of latitudes.
            //       Some function calls might be saved this way.
            CT tan_lat_sph = 0;
            sjoberg_intersection_spherical_02<CT>::apply_alt(lon_a1, lat_a1, lon_a2, lat_a2,
                                                             lon_b1, lat_b1, lon_b2, lat_b2,
                                                             lon_sph, tan_lat_sph);
            t = one_minus_f * tan_lat_sph; // tan(beta)
        }

        // TODO: no need to calculate atan here if reduced latitudes were used
        //       instead of latitudes above, in sjoberg_intersection_spherical_02
        CT const beta = atan(t);

        if (enable_02 && newton_method(geod1, geod2, beta, t, lon1_minus_lon2, lon, lat))
        {
            return true;
        }

        return converge_07(geod1, geod2, beta, t, lon1_minus_lon2, lon_sph, lon, lat);
    }

private:
    static inline bool newton_method(geodesic_type const& geod1, geodesic_type const& geod2, // in
                                     CT beta, CT t, CT const& lon1_minus_lon2, // in
                                     CT & lon, CT & lat) // out
    {
        CT const c0 = 0;
        CT const c1 = 1;

        CT const e_sqr = geod1.e_sqr;

        CT lon1_diff = 0;
        CT lon2_diff = 0;

        CT abs_dbeta_last = 0;

        // [Sjoberg02] converges faster than solution in [Sjoberg07]
        // Newton-Raphson method
        for (int i = 0; i < max_iterations_02; ++i)
        {
            CT const sin_beta = sin(beta);
            CT const cos_beta = cos(beta);
            CT const cos_beta_sqr = math::sqr(cos_beta);
            CT const G = c1 - e_sqr * cos_beta_sqr;

            CT f1 = 0;
            CT f2 = 0;

            if (!geod1.is_Cj_zero)
            {
                bool is_beta_ok = geod1.lon_diff(sin_beta, t, lon1_diff);

                if (is_beta_ok)
                {
                    CT const H = cos_beta_sqr - geod1.Cj_sqr;
                    f1 = geod1.Cj / cos_beta * math::sqrt(G / H);
                }
                else
                {
                    return false;
                }
            }

            if (!geod2.is_Cj_zero)
            {
                bool is_beta_ok = geod2.lon_diff(sin_beta, t, lon2_diff);

                if (is_beta_ok)
                {
                    CT const H = cos_beta_sqr - geod2.Cj_sqr;
                    f2 = geod2.Cj / cos_beta * math::sqrt(G / H);
                }
                else
                {
                    return false;
                }
            }

            // NOTE: Things may go wrong if the IP is near the vertex
            //   1. May converge into the wrong direction (from the other way around).
            //      This happens when the starting point is on the other side than the vertex
            //   2. During converging may "jump" into the other side of the vertex.
            //      In this case sin_beta/sqrt_1_Cj_sqr and t/t0j is not in [-1, 1]
            //   3. f1-f2 may be 0 which means that the intermediate point is on the vertex
            //      In this case it's not possible to check if this is the correct result

            CT const dbeta_denom = f1 - f2;
            //CT const dbeta_denom = math::abs(f1) + math::abs(f2);

            if (math::equals(dbeta_denom, c0))
            {
                return false;
            }

            // The sign of dbeta is changed WRT [Sjoberg02]
            CT const dbeta = (lon1_minus_lon2 + lon1_diff - lon2_diff) / dbeta_denom;

            CT const abs_dbeta = math::abs(dbeta);
            if (i > 0 && abs_dbeta > abs_dbeta_last)
            {
                // The algorithm is not converging
                // The intersection may be on the other side of the vertex
                return false;
            }
            abs_dbeta_last = abs_dbeta;

            if (math::equals(dbeta, c0))
            {
                // Result found
                break;
            }

            // Because the sign of dbeta is changed WRT [Sjoberg02] dbeta is subtracted here
            beta = beta - dbeta;

            t = tan(beta);
        }

        lat = geod1.lat(t);
        // NOTE: if Cj is 0 then the result is lonj or lonj+180
        lon = ! geod1.is_Cj_zero
                ? geod1.lon(lon1_diff)
                : geod2.lon(lon2_diff);

        return true;
    }

    struct geodesics_type
    {
        geodesics_type(geodesic_type const& g1, geodesic_type const& g2)
            : geod1(g1)
            , geod2(g2)
            , vertex1(geod1.get_vertex_data())
            , vertex2(geod2.get_vertex_data())
        {}

        geodesic_type const& geod1;
        geodesic_type const& geod2;
        typename geodesic_type::vertex_data vertex1;
        typename geodesic_type::vertex_data vertex2;
    };

    struct converge_07_result
    {
        converge_07_result()
            : lon1(0), lon2(0), k1_diff(0), k2_diff(0), t1(0), t2(0)
        {}

        CT lon1, lon2;
        CT k1_diff, k2_diff;
        CT t1, t2;
    };

    static inline bool converge_07(geodesic_type const& geod1, geodesic_type const& geod2,
                                   CT beta, CT t,
                                   CT const& lon1_minus_lon2, CT const& lon_sph,
                                   CT & lon, CT & lat)
    {
        //CT const c0 = 0;
        //CT const c1 = 1;
        //CT const c2 = 2;
        //CT const pi = math::pi<CT>();

        geodesics_type geodesics(geod1, geod2);
        converge_07_result result;

        // calculate first pair of longitudes
        if (!converge_07_step_one(CT(sin(beta)), t, lon1_minus_lon2, geodesics, lon_sph, result, false))
        {
            return false;
        }

        int t_direction = 0;

        CT lon_diff_prev = math::longitude_difference<radian>(result.lon1, result.lon2);

        // [Sjoberg07]
        for (int i = 2; i < max_iterations_07; ++i)
        {
            // pick t candidates from previous result based on dir
            CT t_cand1 = result.t1;
            CT t_cand2 = result.t2;
            // if direction is 0 the closer one is the first
            if (t_direction < 0)
            {
                t_cand1 = (std::min)(result.t1, result.t2);
                t_cand2 = (std::max)(result.t1, result.t2);
            }
            else if (t_direction > 0)
            {
                t_cand1 = (std::max)(result.t1, result.t2);
                t_cand2 = (std::min)(result.t1, result.t2);
            }
            else
            {
                t_direction = t_cand1 < t_cand2 ? -1 : 1;
            }

            CT t1 = t;
            CT beta1 = beta;
            // check if the further calculation is needed
            if (converge_07_update(t1, beta1, t_cand1))
            {
                break;
            }

            bool try_t2 = false;
            converge_07_result result_curr;
            if (converge_07_step_one(CT(sin(beta1)), t1, lon1_minus_lon2, geodesics, lon_sph, result_curr))
            {
                CT const lon_diff1 = math::longitude_difference<radian>(result_curr.lon1, result_curr.lon2);
                if (lon_diff_prev > lon_diff1)
                {
                    t = t1;
                    beta = beta1;
                    lon_diff_prev = lon_diff1;
                    result = result_curr;
                }
                else if (t_cand1 != t_cand2)
                {
                    try_t2 = true;
                }
                else
                {
                    // the result is not fully correct but it won't be more accurate
                    break;
                }
            }
            // ! converge_07_step_one
            else
            {
                if (t_cand1 != t_cand2)
                {
                    try_t2 = true;
                }
                else
                {
                    return false;
                }
            }

            if (try_t2)
            {
                CT t2 = t;
                CT beta2 = beta;
                // check if the further calculation is needed
                if (converge_07_update(t2, beta2, t_cand2))
                {
                    break;
                }

                if (! converge_07_step_one(CT(sin(beta2)), t2, lon1_minus_lon2, geodesics, lon_sph, result_curr))
                {
                    return false;
                }

                CT const lon_diff2 = math::longitude_difference<radian>(result_curr.lon1, result_curr.lon2);
                if (lon_diff_prev > lon_diff2)
                {
                    t_direction *= -1;
                    t = t2;
                    beta = beta2;
                    lon_diff_prev = lon_diff2;
                    result = result_curr;
                }
                else
                {
                    // the result is not fully correct but it won't be more accurate
                    break;
                }
            }
        }

        lat = geod1.lat(t);
        lon = ! geod1.is_Cj_zero ? result.lon1 : result.lon2;
        math::normalize_longitude<radian>(lon);

        return true;
    }

    static inline bool converge_07_update(CT & t, CT & beta, CT const& t_new)
    {
        CT const c0 = 0;

        CT const beta_new = atan(t_new);
        CT const dbeta = beta_new - beta;
        beta = beta_new;
        t = t_new;

        return math::equals(dbeta, c0);
    }

    static inline CT const& pick_t(CT const& t1, CT const& t2, int direction)
    {
        return direction < 0 ? (std::min)(t1, t2) : (std::max)(t1, t2);
    }

    static inline bool converge_07_step_one(CT const& sin_beta,
                                            CT const& t,
                                            CT const& lon1_minus_lon2,
                                            geodesics_type const& geodesics,
                                            CT const& lon_sph,
                                            converge_07_result & result,
                                            bool check_sin_beta = true)
    {
        bool ok = converge_07_one_geod(sin_beta, t, geodesics.geod1, geodesics.vertex1, lon_sph,
                                       result.lon1, result.k1_diff, check_sin_beta)
               && converge_07_one_geod(sin_beta, t, geodesics.geod2, geodesics.vertex2, lon_sph,
                                       result.lon2, result.k2_diff, check_sin_beta);

        if (!ok)
        {
            return false;
        }

        CT const k = lon1_minus_lon2 + result.k1_diff - result.k2_diff;

        // get 2 possible ts one lesser and one greater than t
        // t1 is the closer one
        calc_ts(t, k, geodesics.geod1, geodesics.geod2, result.t1, result.t2);

        return true;
    }

    static inline bool converge_07_one_geod(CT const& sin_beta, CT const& t,
                                            geodesic_type const& geod,
                                            typename geodesic_type::vertex_data const& vertex,
                                            CT const& lon_sph,
                                            CT & lon, CT & k_diff,
                                            bool check_sin_beta)
    {
        using math::detail::bounded;
        CT const c1 = 1;

        CT k_diff_before = 0;
        CT k_diff_behind = 0;

        bool is_beta_ok = geod.k_diffs(sin_beta, vertex, k_diff_before, k_diff_behind, check_sin_beta);

        if (! is_beta_ok)
        {
            return false;
        }

        CT const asin_t_t0j = ! geod.is_Cj_zero ? asin(bounded(t / geod.t0j, -c1, c1)) : 0;
        CT const sign_asin_t_t0j = geod.sign_lon_diff * asin_t_t0j;

        CT const lon_before = geod.lonj + sign_asin_t_t0j + k_diff_before;
        CT const lon_behind = geod.lonj - sign_asin_t_t0j + k_diff_behind;

        CT const lon_dist_before = math::longitude_distance_signed<radian>(lon_before, lon_sph);
        CT const lon_dist_behind = math::longitude_distance_signed<radian>(lon_behind, lon_sph);
        if (math::abs(lon_dist_before) <= math::abs(lon_dist_behind))
        {
            k_diff = k_diff_before;
            lon = lon_before;
        }
        else
        {
            k_diff = k_diff_behind;
            lon = lon_behind;
        }

        return true;
    }

    static inline void calc_ts(CT const& t, CT const& k,
                               geodesic_type const& geod1, geodesic_type const& geod2,
                               CT & t1, CT& t2)
    {
        CT const c1 = 1;
        CT const c2 = 2;

        CT const K = sin(k);

        BOOST_GEOMETRY_ASSERT(!geod1.is_Cj_zero || !geod2.is_Cj_zero);
        if (geod1.is_Cj_zero)
        {
            t1 = K * geod2.t0j;
            t2 = -t1;
        }
        else if (geod2.is_Cj_zero)
        {
            t1 = -K * geod1.t0j;
            t2 = -t1;
        }
        else
        {
            CT const A = math::sqr(geod1.t0j) + math::sqr(geod2.t0j);
            CT const B = c2 * geod1.t0j * geod2.t0j * math::sqrt(c1 - math::sqr(K));

            CT const K_t01_t02 = K * geod1.t0j * geod2.t0j;
            CT const D1 = math::sqrt(A + B);
            CT const D2 = math::sqrt(A - B);
            CT const t_new1 = K_t01_t02 / D1;
            CT const t_new2 = K_t01_t02 / D2;
            CT const t_new3 = -t_new1;
            CT const t_new4 = -t_new2;

            // Pick 2 nearest t_new, one greater and one lesser than current t
            CT const abs_t_new1 = math::abs(t_new1);
            CT const abs_t_new2 = math::abs(t_new2);
            CT const abs_t_max = (std::max)(abs_t_new1, abs_t_new2);
            t1 = -abs_t_max; // lesser
            t2 = abs_t_max; // greater
            if (t1 < t)
            {
                if (t_new1 < t && t_new1 > t1)
                    t1 = t_new1;
                if (t_new2 < t && t_new2 > t1)
                    t1 = t_new2;
                if (t_new3 < t && t_new3 > t1)
                    t1 = t_new3;
                if (t_new4 < t && t_new4 > t1)
                    t1 = t_new4;
            }
            if (t2 > t)
            {
                if (t_new1 > t && t_new1 < t2)
                    t2 = t_new1;
                if (t_new2 > t && t_new2 < t2)
                    t2 = t_new2;
                if (t_new3 > t && t_new3 < t2)
                    t2 = t_new3;
                if (t_new4 > t && t_new4 < t2)
                    t2 = t_new4;
            }
        }

        // the first one is the closer one
        if (math::abs(t - t2) < math::abs(t - t1))
        {
            std::swap(t2, t1);
        }
    }

    static inline CT fj(CT const& cos_beta, CT const& cos2_beta, CT const& Cj, CT const& e_sqr)
    {
        CT const c1 = 1;
        CT const Cj_sqr = math::sqr(Cj);
        return Cj / cos_beta * math::sqrt((c1 - e_sqr * cos2_beta) / (cos2_beta - Cj_sqr));
    }

    /*static inline CT vertical_intersection_longitude(CT const& ip_lon, CT const& seg_lon1, CT const& seg_lon2)
    {
        CT const c0 = 0;
        CT const lon_2 = ip_lon > c0 ? ip_lon - pi : ip_lon + pi;

        return (std::min)(math::longitude_difference<radian>(ip_lon, seg_lon1),
                          math::longitude_difference<radian>(ip_lon, seg_lon2))
            <=
               (std::min)(math::longitude_difference<radian>(lon_2, seg_lon1),
                          math::longitude_difference<radian>(lon_2, seg_lon2))
            ? ip_lon : lon_2;
    }*/
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_SJOBERG_INTERSECTION_HPP


#include "parametrization/parametrization.hpp"

#include <vector>
#include <cmath>
#include <chrono>
#include <Eigen/IterativeLinearSolvers>

using namespace Eigen;
using namespace std;

typedef Triplet<double> Td;

vector<MatrixXd> parametrize(vector<const MatrixXd*> &Vs, vector<const MatrixXi*> &Ts) {
  cout << "Started parametrization:..." << flush;
  auto start = chrono::high_resolution_clock::now();

  vector<MatrixXd> Us;
  Us.reserve(Vs.size());
  for(int i = 0; i < Vs.size(); i++)
    Us.emplace_back(parametrize(*Vs[i], *Ts[i]));

  auto finish = chrono::high_resolution_clock::now();
  cout << "done: " << chrono::duration<double>(finish-start).count() << " s" << endl;

  return Us;
}

MatrixXd parametrize(const MatrixXd &V, const MatrixXi &T) {
  const int np = T.rows(), n = V.rows();

  pair<int,int> diam = approximateDiameter(V);

  // List of triplets for sparse matrix creation
  vector<Td> ta, tb, tu;
  ta.reserve(np * 12);

  // For each triangle
  for (int i = 0; i < np; i++) {
    // Calculate x,y
    // First vertex as (0,0), second as (s,0), third as t(cos(a), sin(a))
    RowVectorXd p[3];
    double x[3], y[3];

    p[0] = V.row(T(i, 0));
    p[1] = V.row(T(i, 1));
    p[2] = V.row(T(i, 2));

    double a = angleBetweenSides(p[1] - p[0], p[2] - p[0]);
    double s = (p[1] - p[0]).norm();
    double t = (p[2] - p[0]).norm();

    x[0] = y[0] = 0;
    x[1] = s, y[1] = 0;
    x[2] = t * cos(a), y[2] = t * sin(a);

    double d = sqrt((x[0] * y[1] - y[0] * x[1]) + (x[1] * y[2] - y[1] * x[2]) + (x[2] * y[0] - y[2] * x[0]));
//    d=1;

    // Calculate W
    double Wr[3], Wi[3];
    Wr[0] = (x[2] - x[1]) / d, Wi[0] = (y[2] - y[1]) / d;
    Wr[1] = (x[0] - x[2]) / d, Wi[1] = (y[0] - y[2]) / d;
    Wr[2] = (x[1] - x[0]) / d, Wi[2] = (y[1] - y[0]) / d;

    // Push values to A and B
    emplaceAB(ta, tb, i, T(i,0), n, np, Wr[0], Wi[0], diam);
    emplaceAB(ta, tb, i, T(i,1), n, np, Wr[1], Wi[1], diam);
    emplaceAB(ta, tb, i, T(i,2), n, np, Wr[2], Wi[2], diam);
  }

  // Build A and B
  SparseMatrix<double> A(2 * np, 2 * (n - 2));
  SparseMatrix<double> B(2 * np, 4);
  A.setFromTriplets(ta.begin(), ta.end());
  ta.clear();
  B.setFromTriplets(tb.begin(), tb.end());
  tb.clear();

  // Build up
  VectorXd up(4);
  up(0) = 0;
  up(1) = 1;
  up(2) = 0;
  up(3) = 1;

  VectorXd b = - B * up;

  // Solve least squares using QR decomposition
  // A.makeCompressed();
  // SparseQR<SparseMatrix<double>, COLAMDOrdering<int> > solver(A);
  // VectorXd uf = solver.solve(b);

  // Solve least squares using iterative CG
  A.makeCompressed();
  LeastSquaresConjugateGradient<SparseMatrix<double>> solver(A);
  VectorXd uf = solver.solve(b);

  // Join uf and up
  MatrixXd U(n, 2);
  for(int i = 0; i < n; i++) {
    if(i != diam.first and i != diam.second) {
      int ii = i;
      if(i > diam.second) ii--;
      if(i > diam.first) ii--;
      U(i,0) = uf(ii);
      U(i,1) = uf(ii + n - 2);
    }
    else
      U(i,0) = U(i,1) = (i==diam.second);
  }

  return U;
}

double angleBetweenSides(const RowVectorXd &a, const RowVectorXd &b) {
  return acos(a.dot(b) / a.norm() / b.norm());
}

pair<int,int> approximateDiameter(const MatrixXd &V) {
  const int n = V.rows();
  double m = 0;
  pair<int,int> best{0,1};
  for(int i = 0; i < n; i++) {
    for(int j = i+1; j < n; j++) {
      double d = (V.row(i)-V.row(j)).norm();
      if(d>m) {
        m = d;
        best = pair<int,int>{i,j};
      }
    }
  }
  return best;
}

void emplaceAB(vector<Td> &ta, vector<Td> &tb, int i, int j, int n, int np, double wr, double wi, pair<int,int> &diam) {
  if (j != diam.first and j != diam.second) {
    if(j > diam.second) j--;
    if(j > diam.first) j--;
    ta.emplace_back(i, j, wr);
    ta.emplace_back(i + np, j + n - 2, wr);
    ta.emplace_back(i, j + n - 2, -wi);
    ta.emplace_back(i + np, j, wi);
  } else {
    j = (j==diam.second);
    tb.emplace_back(i, j, wr);
    tb.emplace_back(i + np, j + 2, wr);
    tb.emplace_back(i, j + 2, -wi);
    tb.emplace_back(i + np, j, wi);
  }
}


#include <iostream>
#include <sys/time.h>
#include <Eigen/Core>

#include "celerite/celerite.h"
//#include "celerite/carma.h"
#include "celerite/utils.h"
#include "../include/KF.h"
#include "../include/dsho.h"

// This code benchmarks a single DSHO, celerite vs gpstate

// Timer for the benchmark.
double get_timestamp ()
{
  struct timeval now;
  gettimeofday (&now, NULL);
  return double(now.tv_usec) * 1.0e-6 + double(now.tv_sec);
}

int main (int argc, char* argv[])
{
  srand(42);

  size_t N_max = pow(2, 19);
  if (argc >= 2) N_max = atoi(argv[1]);
  size_t niter = 5;
  if (argc >= 3) niter = atoi(argv[2]);
  size_t niter_celerite = niter;
  if (argc >= 4) niter_celerite = atoi(argv[3]);
  std::cout << "N:" << N_max << " gpstate:" << niter << " celerite:" << niter_celerite << std::endl;

 // Generate some fake data.
  Eigen::VectorXd x = Eigen::VectorXd::Random(N_max),
                  yerr = Eigen::VectorXd::Random(N_max),
                  y, diag;
  yerr.array() *= 0.1;
  yerr.array() += 1.0;
  diag = yerr.array() * yerr.array();
  std::sort(x.data(), x.data() + x.size());
  y = sin(x.array());

  //set up the DSHO parameters
  size_t nterms=3;
  double omega0=1.0;
  double Q = 1.0;
  double varf = 1.0;
  int flag;

  // translate into corresponding CARMA parameters for use in celerite
  Eigen::VectorXd carma_arparams(nterms);
  Eigen::VectorXd carma_maparams(nterms-1);
  carma_arparams << omega0*omega0, omega0/Q, 1.0;
  carma_maparams << 1.0, 0.0;

  Eigen::VectorXd alpha_real, beta_real;
  Eigen::VectorXd alpha_complex_real(1), alpha_complex_imag(1),
                    beta_complex_real(1), beta_complex_imag(1);

  double temp = std::sqrt(4.0*Q*Q - 1.0);
  double S0 = varf* std::pow(Q,-2) * std::sqrt(M_PI) / std::sqrt(2);
  alpha_complex_real(0) = S0 * omega0 * Q;
  alpha_complex_imag(0) = S0 * omega0 * Q/temp;
  beta_complex_real(0) = 0.5*omega0 / Q;
  beta_complex_imag(0) = 0.5*temp*omega0 / Q;

//f = np.sqrt(4.0 * Q**2-1)
 //       return (
 //           S0 * w0 * Q,
 //           S0 * w0 * Q / f,
 //           0.5 * w0 / Q,
 //           0.5 * w0 / Q * f,
 //       )

  // the following pieces of code verify that the log likelihoods are
  // consistent between celerite and gpstate
  celerite::solver::CholeskySolver<double> solver;
  solver.compute(0.0, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x, diag);
  double celerite_ll = -0.5*(solver.dot_solve(y) + solver.log_determinant() + x.rows() * log(2.0 * M_PI));
  std::cout << celerite_ll << std::endl;

  gpstate::dsho::DSHOSolver dsho(x,y,yerr,omega0, Q, 1.0);
  double log_likelihood = dsho.KF_log_likelihood();
  std::cout << "logL: " << log_likelihood << std::endl;

  // now benchmark, knowing that we do get consistent log-likelohood values
  double strt;

for (size_t N = 64; N <= N_max; N *= 2) {

  double celerite_time = 0.0;
  double gpstate_time = 0.0;

  if (niter_celerite > 0) {
    //celerite::solver::CholeskySolver<double> solver;
    //solver.compute(0.0, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x, diag);
    //celerite_ll = -0.5*(solver.dot_solve(y) + solver.log_determinant() + x.rows() * log(2.0 * M_PI));
    //std::cout << "logL(celerite)" << celerite_ll << std::endl;

    for (size_t i = 0; i < niter_celerite; ++i) {
      strt = get_timestamp();
      solver.compute(0.0, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x.head(N), diag.head(N));
      celerite_ll = -0.5*(solver.dot_solve(y.head(N)) + solver.log_determinant() + x.head(N).rows() * log(2.0 * M_PI));
      celerite_time += get_timestamp()-strt;
    }
  }
  if (niter > 0) {
    //gpstate::dsho::DSHOSolver dsho1(x,y,yerr,omega0, Q, 1.0);
    //log_likelihood = dsho1.KF_log_likelihood();
    //std::cout << "logL: " << log_likelihood << std::endl;

    for (size_t i = 0; i < niter; ++i) {
      strt = get_timestamp();
      gpstate::dsho::DSHOSolver dsho(x.head(N),y.head(N),yerr.head(N),omega0, Q, 1.0);
      double log_likelihood = dsho.KF_log_likelihood();
      gpstate_time += get_timestamp()-strt;
      }
    }
   // Print the results.
   if ((niter > 0) & (niter_celerite > 0)){
     std::cout << N;
     std::cout << " ";
     std::cout << celerite_time / niter_celerite;
     std::cout << " ";
     std::cout << gpstate_time / niter;
     std::cout << "\n";
   }
}
}


//***************************************************************************************
// HW08App.cpp by Frank Luna (C) 2015 All Rights Reserved.
//***************************************************************************************

#include "../common/d3dApp.h"
#include "../common/MathHelper.h"
#include <UDX12/UploadBuffer.h>
#include "../common/GeometryGenerator.h"
#include "util.h"

#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/Dense>

using Microsoft::WRL::ComPtr;
using namespace DirectX;
using namespace DirectX::PackedVector;

const int gNumFrameResources = 3;
const size_t InterpNum = 60;

struct V;
struct F;
struct H;
using Traits_VFH = Ubpa::HEMeshTriats_EmptyE<V, F, H>;

struct V : Ubpa::TVertex<Traits_VFH> {
	Ubpa::pointf2 p; // t = 0
	Ubpa::pointf2 q; // t = 1
};

struct F : Ubpa::TPolygon<Traits_VFH> {
	float area;

	// set L, theta, S
	void Init();
	Ubpa::matf2 L;
	float theta; // -pi ~ pi
	Ubpa::matf2 S;

	// theta, S, t -> Lt
	void UpdateLt(float t);
	Ubpa::matf2 Lt;
};

struct H : Ubpa::THalfEdge<Traits_VFH> {
	float cot_theta{ 0.f }; // cotangent of opposite angle
};

void F::Init() {
	auto he01 = HalfEdge();
	auto he12 = he01->Next();
	auto he20 = he12->Next();

	auto v0 = he01->Origin();
	auto v1 = he12->Origin();
	auto v2 = he20->Origin();

	const auto& p0 = v0->p;
	const auto& p1 = v1->p;
	const auto& p2 = v2->p;

	const auto& q0 = v0->q;
	const auto& q1 = v1->q;
	const auto& q2 = v2->q;

	float y1_y2 = p1[1] - p2[1];
	float y2_y0 = p2[1] - p0[1];
	float y0_y1 = p0[1] - p1[1];

	float x2_x1 = p2[0] - p1[0];
	float x0_x2 = p0[0] - p2[0];
	float x1_x0 = p1[0] - p0[0];

	Ubpa::matf2 J;
	float inv_two_area = 1 / (2 * area);
	J(0, 0) = inv_two_area * (y1_y2 * q0[0] + y2_y0 * q1[0] + y0_y1 * q2[0]);
	J(0, 1) = inv_two_area * (x2_x1 * q0[0] + x0_x2 * q1[0] + x1_x0 * q2[0]);
	J(1, 0) = inv_two_area * (y1_y2 * q0[1] + y2_y0 * q1[1] + y0_y1 * q2[1]);
	J(1, 1) = inv_two_area * (x2_x1 * q0[1] + x0_x2 * q1[1] + x1_x0 * q2[1]);

	auto [U, S, V] = J.SVD();
	Ubpa::matf2 R = U * V.transpose();
	assert(R.det() > 0);
	this->S = V * S * V.transpose();
	L = R * S;
	float cos_theta = R(0, 0);
	float sin_theta = R(1, 0);
	float ac = std::acos(cos_theta);
	if (sin_theta > 0)
		theta = ac;
	else
		theta = - ac;
}

void F::UpdateLt(float t) {
	assert(0 <= t && t <= 1);
	float theta_t = t * theta;
	float cos_theta_t = std::cos(theta_t);
	float sin_theta_t = std::sin(theta_t);
	Ubpa::matf2 Rt = {
		cos_theta_t, -sin_theta_t,
		sin_theta_t,  cos_theta_t
	};
	Ubpa::matf2 St = Ubpa::matf2::lerp(Ubpa::matf2::eye(), S, t);
	Lt = Rt * St;
}

struct ObjectConstants
{
	DirectX::XMFLOAT4X4 World = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 TexTransform = MathHelper::Identity4x4();
};

struct PassConstants
{
	DirectX::XMFLOAT4X4 View = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 InvView = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 Proj = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 InvProj = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 ViewProj = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 InvViewProj = MathHelper::Identity4x4();
	DirectX::XMFLOAT3 EyePosW = { 0.0f, 0.0f, 0.0f };
	float cbPerObjectPad1 = 0.0f;
	DirectX::XMFLOAT2 RenderTargetSize = { 0.0f, 0.0f };
	DirectX::XMFLOAT2 InvRenderTargetSize = { 0.0f, 0.0f };
	float NearZ = 0.0f;
	float FarZ = 0.0f;
	float TotalTime = 0.0f;
	float DeltaTime = 0.0f;

	DirectX::XMFLOAT4 AmbientLight = { 0.0f, 0.0f, 0.0f, 1.0f };

	// Indices [0, NUM_DIR_LIGHTS) are directional lights;
	// indices [NUM_DIR_LIGHTS, NUM_DIR_LIGHTS+NUM_POINT_LIGHTS) are point lights;
	// indices [NUM_DIR_LIGHTS+NUM_POINT_LIGHTS, NUM_DIR_LIGHTS+NUM_POINT_LIGHT+NUM_SPOT_LIGHTS)
	// are spot lights for a maximum of MaxLights per object.
	Light Lights[MaxLights];
};

struct Vertex
{
	DirectX::XMFLOAT3 Pos;
	DirectX::XMFLOAT3 Normal;
	DirectX::XMFLOAT2 TexC;
};

// Lightweight structure stores parameters to draw a shape.  This will
// vary from app-to-app.
struct RenderItem
{
	RenderItem() = default;

    // World matrix of the shape that describes the object's local space
    // relative to the world space, which defines the position, orientation,
    // and scale of the object in the world.
    XMFLOAT4X4 World = MathHelper::Identity4x4();

	XMFLOAT4X4 TexTransform = MathHelper::Identity4x4();

	// Dirty flag indicating the object data has changed and we need to update the constant buffer.
	// Because we have an object cbuffer for each FrameResource, we have to apply the
	// update to each FrameResource.  Thus, when we modify obect data we should set
	// NumFramesDirty = gNumFrameResources so that each frame resource gets the update.
	int NumFramesDirty = gNumFrameResources;

	// Index into GPU constant buffer corresponding to the ObjectCB for this render item.
	UINT ObjCBIndex = -1;

	Material* Mat = nullptr;
	Ubpa::DX12::MeshGeometry* Geo = nullptr;
	//std::string Geo;

    // Primitive topology.
    D3D12_PRIMITIVE_TOPOLOGY PrimitiveType = D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST;

    // DrawIndexedInstanced parameters.
    UINT IndexCount = 0;
    UINT StartIndexLocation = 0;
    int BaseVertexLocation = 0;
};

class HW08App : public D3DApp
{
public:
    HW08App(HINSTANCE hInstance);
    HW08App(const HW08App& rhs) = delete;
    HW08App& operator=(const HW08App& rhs) = delete;
    ~HW08App();

    virtual bool Initialize()override;

private:
    virtual void OnResize()override;
    virtual void Update(const GameTimer& gt)override;
    virtual void Draw(const GameTimer& gt)override;

    virtual void OnMouseDown(WPARAM btnState, int x, int y)override;
    virtual void OnMouseUp(WPARAM btnState, int x, int y)override;
    virtual void OnMouseMove(WPARAM btnState, int x, int y)override;

    void OnKeyboardInput(const GameTimer& gt);
	void UpdateCamera(const GameTimer& gt);
	void AnimateMaterials(const GameTimer& gt);
	void UpdateObjectCBs(const GameTimer& gt);
	void UpdateMaterialCBs(const GameTimer& gt);
	void UpdateMainPassCB(const GameTimer& gt);

	void LoadTextures();
    void BuildRootSignature();
	void BuildDescriptorHeaps();
    void BuildShadersAndInputLayout();
    void BuildShapeGeometry();
    void BuildPSOs();
    void BuildFrameResources();
    void BuildMaterials();
    void BuildRenderItems();
    void DrawRenderItems(ID3D12GraphicsCommandList* cmdList, const std::vector<RenderItem*>& ritems);

	std::array<const CD3DX12_STATIC_SAMPLER_DESC, 6> GetStaticSamplers();

private:

	std::vector<std::unique_ptr<Ubpa::DX12::FrameResource>> mFrameResources;
	Ubpa::DX12::FrameResource* mCurrFrameResource = nullptr;
    int mCurrFrameResourceIndex = 0;

	std::unordered_map<std::string, std::unique_ptr<Material>> mMaterials;

    std::vector<D3D12_INPUT_ELEMENT_DESC> mInputLayout;

	// List of all the render items.
	std::vector<std::unique_ptr<RenderItem>> mAllRitems;

	// Render items divided by PSO.
	std::vector<RenderItem*> mOpaqueRitems;

    PassConstants mMainPassCB;

	XMFLOAT3 mEyePos = { 0.0f, 0.0f, 0.0f };
	XMFLOAT4X4 mView = MathHelper::Identity4x4();
	XMFLOAT4X4 mProj = MathHelper::Identity4x4();

	float mTheta = 1.3f*XM_PI;
	float mPhi = 0.4f*XM_PI;
	float mRadius = 2.5f;

    POINT mLastMousePos;

	std::unordered_map<std::string, std::unique_ptr<Ubpa::TriMesh>> trimeshes;

	// frame graph
	//Ubpa::DX12::FG::RsrcMngr fgRsrcMngr;
	Ubpa::DX12::FG::Executor fgExecutor;
	Ubpa::FG::Compiler fgCompiler;
	Ubpa::FG::FrameGraph fg;
};

int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE prevInstance,
    PSTR cmdLine, int showCmd)
{
    // Enable run-time memory check for debug builds.
#if defined(DEBUG) | defined(_DEBUG)
    _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);
#endif

    try
    {
        HW08App theApp(hInstance);
        if(!theApp.Initialize())
            return 0;

        int rst = theApp.Run();
		Ubpa::DXRenderer::Instance().Release();
		return rst;
    }
    catch(Ubpa::DX12::Util::Exception& e)
    {
        MessageBox(nullptr, e.ToString().c_str(), L"HR Failed", MB_OK);
        return 0;
    }

}

HW08App::HW08App(HINSTANCE hInstance)
    : D3DApp(hInstance)
{
}

HW08App::~HW08App()
{
    if(!uDevice.IsNull())
        FlushCommandQueue();
}

bool HW08App::Initialize()
{
    if(!D3DApp::Initialize())
        return false;

	Ubpa::DXRenderer::Instance().Init(uDevice.raw.Get());

	Ubpa::DX12::DescriptorHeapMngr::Instance().Init(uDevice.raw.Get(), 1024, 1024, 1024, 1024, 1024);

	//fgRsrcMngr.Init(uGCmdList, uDevice);

    // Reset the command list to prep for initialization commands.
    ThrowIfFailed(uGCmdList->Reset(mDirectCmdListAlloc.Get(), nullptr));

    // Get the increment size of a descriptor in this heap type.  This is hardware specific,
	// so we have to query this information.
    //mCbvSrvDescriptorSize = uDevice->GetDescriptorHandleIncrementSize(D3D12_DESCRIPTOR_HEAP_TYPE_CBV_SRV_UAV);

	Ubpa::DXRenderer::Instance().GetUpload().Begin();

	LoadTextures();
    BuildRootSignature();
	BuildDescriptorHeaps();
    BuildShadersAndInputLayout();
    BuildShapeGeometry();
	BuildMaterials();
    BuildRenderItems();
    BuildFrameResources();
    BuildPSOs();

    // Execute the initialization commands.
    ThrowIfFailed(uGCmdList->Close());
	uCmdQueue.Execute(uGCmdList.raw.Get());

	Ubpa::DXRenderer::Instance().GetUpload().End(uCmdQueue.raw.Get());

    // Wait until initialization is complete.
    FlushCommandQueue();

    return true;
}

void HW08App::OnResize()
{
    D3DApp::OnResize();

    // The window resized, so update the aspect ratio and recompute the projection matrix.
    XMMATRIX P = XMMatrixPerspectiveFovLH(0.25f*MathHelper::Pi, AspectRatio(), 1.0f, 1000.0f);
    XMStoreFloat4x4(&mProj, P);

	auto clearFGRsrcMngr = [](void* rsrcMngr) {
		reinterpret_cast<Ubpa::DX12::FG::RsrcMngr*>(rsrcMngr)->Clear();
	};
	for (auto& frsrc : mFrameResources)
		frsrc->DelayUpdateResource("FrameGraphRsrcMngr", clearFGRsrcMngr);
}

void HW08App::Update(const GameTimer& gt)
{
    OnKeyboardInput(gt);
	UpdateCamera(gt);

    // Cycle through the circular frame resource array.
    mCurrFrameResourceIndex = (mCurrFrameResourceIndex + 1) % gNumFrameResources;
    mCurrFrameResource = mFrameResources[mCurrFrameResourceIndex].get();

    // Has the GPU finished processing the commands of the current frame resource?
    // If not, wait until the GPU has completed commands up to this fence point.
	mCurrFrameResource->Wait();

	AnimateMaterials(gt);
	UpdateObjectCBs(gt);
	UpdateMaterialCBs(gt);
	UpdateMainPassCB(gt);
}

void HW08App::Draw(const GameTimer& gt)
{
	auto cmdListAlloc = mCurrFrameResource->GetResource<ID3D12CommandAllocator>("CommandAllocator");

    // Reuse the memory associated with command recording.
    // We can only reset when the associated command lists have finished execution on the GPU.
    ThrowIfFailed(cmdListAlloc->Reset());

    // A command list can be reset after it has been added to the command queue via ExecuteCommandList.
    // Reusing the command list reuses memory.
	ThrowIfFailed(uGCmdList->Reset(cmdListAlloc, nullptr));
	uGCmdList.SetDescriptorHeaps(Ubpa::DX12::DescriptorHeapMngr::Instance().GetCSUGpuDH()->GetDescriptorHeap());

	uGCmdList->RSSetViewports(1, &mScreenViewport);
	uGCmdList->RSSetScissorRects(1, &mScissorRect);

	fg.Clear();
	auto fgRsrcMngr = mCurrFrameResource->GetResource<Ubpa::DX12::FG::RsrcMngr>("FrameGraphRsrcMngr");
	fgRsrcMngr->NewFrame();
	fgExecutor.NewFrame();;

	auto gbuffer0 = fg.AddResourceNode("GBuffer0");
	auto gbuffer1 = fg.AddResourceNode("GBuffer1");
	auto gbuffer2 = fg.AddResourceNode("GBuffer2");
	auto backbuffer = fg.AddResourceNode("Back Buffer");
	auto depthstencil = fg.AddResourceNode("Depth Stencil");
	auto gbPass = fg.AddPassNode(
		"GBuffer Pass",
		{},
		{ gbuffer0,gbuffer1,gbuffer2,depthstencil }
	);
	/*auto debugPass = fg.AddPassNode(
		"Debug",
		{ gbuffer1 },
		{ backbuffer }
	);*/
	auto deferLightingPass = fg.AddPassNode(
		"Defer Lighting",
		{ gbuffer0,gbuffer1,gbuffer2 },
		{ backbuffer }
	);

	(*fgRsrcMngr)
		.RegisterTemporalRsrc(gbuffer0,
			Ubpa::DX12::FG::RsrcType::RT2D(DXGI_FORMAT_R32G32B32A32_FLOAT, mClientWidth, mClientHeight, Colors::Black))
		.RegisterTemporalRsrc(gbuffer1,
			Ubpa::DX12::FG::RsrcType::RT2D(DXGI_FORMAT_R32G32B32A32_FLOAT, mClientWidth, mClientHeight, Colors::Black))
		.RegisterTemporalRsrc(gbuffer2,
			Ubpa::DX12::FG::RsrcType::RT2D(DXGI_FORMAT_R32G32B32A32_FLOAT, mClientWidth, mClientHeight, Colors::Black))

		.RegisterRsrcTable({
			{gbuffer0,Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT)},
			{gbuffer1,Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT)},
			{gbuffer2,Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT)} })

		.RegisterImportedRsrc(backbuffer, { CurrentBackBuffer(), D3D12_RESOURCE_STATE_PRESENT })
		.RegisterImportedRsrc(depthstencil, { mDepthStencilBuffer.Get(), D3D12_RESOURCE_STATE_DEPTH_WRITE })

		.RegisterPassRsrcs(gbPass, gbuffer0, D3D12_RESOURCE_STATE_RENDER_TARGET,
			Ubpa::DX12::FG::RsrcImplDesc_RTV_Null{})
		.RegisterPassRsrcs(gbPass, gbuffer1, D3D12_RESOURCE_STATE_RENDER_TARGET,
			Ubpa::DX12::FG::RsrcImplDesc_RTV_Null{})
		.RegisterPassRsrcs(gbPass, gbuffer2, D3D12_RESOURCE_STATE_RENDER_TARGET,
			Ubpa::DX12::FG::RsrcImplDesc_RTV_Null{})
		.RegisterPassRsrcs(gbPass, depthstencil,
			D3D12_RESOURCE_STATE_DEPTH_WRITE, Ubpa::DX12::Desc::DSV::Basic(mDepthStencilFormat))

		/*.RegisterPassRsrcs(debugPass, gbuffer1, D3D12_RESOURCE_STATE_PIXEL_SHADER_RESOURCE,
			Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT))

		.RegisterPassRsrcs(debugPass, backbuffer, D3D12_RESOURCE_STATE_RENDER_TARGET,
			Ubpa::DX12::FG::RsrcImplDesc_RTV_Null{})*/

		.RegisterPassRsrcs(deferLightingPass, gbuffer0, D3D12_RESOURCE_STATE_PIXEL_SHADER_RESOURCE,
			Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT))
		.RegisterPassRsrcs(deferLightingPass, gbuffer1, D3D12_RESOURCE_STATE_PIXEL_SHADER_RESOURCE,
			Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT))
		.RegisterPassRsrcs(deferLightingPass, gbuffer2, D3D12_RESOURCE_STATE_PIXEL_SHADER_RESOURCE,
			Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT))

		.RegisterPassRsrcs(deferLightingPass, backbuffer, D3D12_RESOURCE_STATE_RENDER_TARGET,
			Ubpa::DX12::FG::RsrcImplDesc_RTV_Null{})
		;

	fgExecutor.RegisterPassFunc(
		gbPass,
		[&](const Ubpa::DX12::FG::PassRsrcs& rsrcs) {
			uGCmdList->SetPipelineState(Ubpa::DXRenderer::Instance().GetPSO("geometry"));
			auto gb0 = rsrcs.find(gbuffer0)->second;
			auto gb1 = rsrcs.find(gbuffer1)->second;
			auto gb2 = rsrcs.find(gbuffer2)->second;
			auto ds = rsrcs.find(depthstencil)->second;

			// Clear the render texture and depth buffer.
			uGCmdList.ClearRenderTargetView(gb0.cpuHandle, Colors::Black);
			uGCmdList.ClearRenderTargetView(gb1.cpuHandle, Colors::Black);
			uGCmdList.ClearRenderTargetView(gb2.cpuHandle, Colors::Black);
			uGCmdList.ClearDepthStencilView(ds.cpuHandle);

			// Specify the buffers we are going to render to.
			std::array rts{ gb0.cpuHandle,gb1.cpuHandle,gb2.cpuHandle };
			uGCmdList->OMSetRenderTargets(rts.size(), rts.data(), false, &ds.cpuHandle);

			uGCmdList->SetGraphicsRootSignature(Ubpa::DXRenderer::Instance().GetRootSignature("geometry"));

			auto passCB = mCurrFrameResource
				->GetResource<Ubpa::DX12::ArrayUploadBuffer<PassConstants>>("gbPass constants")
				->GetResource();
			uGCmdList->SetGraphicsRootConstantBufferView(2, passCB->GetGPUVirtualAddress());

			auto fence2interpidx = [N = InterpNum](size_t n, size_t gap) {
				size_t loop = 2 * N * gap;
				size_t i = (n % loop) / gap;
				if (i >= N)
					i = 2 * N - 1 - i;
				return 2 + i;
			};
			DrawRenderItems(uGCmdList.raw.Get(), { mAllRitems[0].get(), mAllRitems[1].get(), mAllRitems[fence2interpidx(mCurrentFence, 2)].get() });
		}
	);

	//fgExecutor.RegisterPassFunc(
	//	debugPass,
	//	[&](const Ubpa::DX12::FG::PassRsrcs& rsrcs) {
	//		uGCmdList->SetPipelineState(Ubpa::DXRenderer::Instance().GetPSO("screen"));
	//		auto img = rsrcs.find(gbuffer1)->second;
	//		auto bb = rsrcs.find(backbuffer)->second;
	//
	//		//uGCmdList->CopyResource(bb.resource, rt.resource);

	//		// Clear the render texture and depth buffer.
	//		uGCmdList.ClearRenderTargetView(bb.cpuHandle, Colors::LightSteelBlue);

	//		// Specify the buffers we are going to render to.
	//		//uGCmdList.OMSetRenderTarget(bb.cpuHandle, ds.cpuHandle);
	//		uGCmdList->OMSetRenderTargets(1, &bb.cpuHandle, false, nullptr);

	//		uGCmdList->SetGraphicsRootSignature(Ubpa::DXRenderer::Instance().GetRootSignature("screen"));

	//		uGCmdList->SetGraphicsRootDescriptorTable(0, img.gpuHandle);

	//		uGCmdList->IASetVertexBuffers(0, 0, nullptr);
	//		uGCmdList->IASetIndexBuffer(nullptr);
	//		uGCmdList->IASetPrimitiveTopology(D3D_PRIMITIVE_TOPOLOGY_TRIANGLELIST);
	//		uGCmdList->DrawInstanced(6, 1, 0, 0);
	//	}
	//);

	fgExecutor.RegisterPassFunc(
		deferLightingPass,
		[&](const Ubpa::DX12::FG::PassRsrcs& rsrcs) {
			uGCmdList->SetPipelineState(Ubpa::DXRenderer::Instance().GetPSO("defer lighting"));
			auto gb0 = rsrcs.find(gbuffer0)->second;
			auto gb1 = rsrcs.find(gbuffer1)->second;
			auto gb2 = rsrcs.find(gbuffer2)->second;

			auto bb = rsrcs.find(backbuffer)->second;

			//uGCmdList->CopyResource(bb.resource, rt.resource);

			// Clear the render texture and depth buffer.
			uGCmdList.ClearRenderTargetView(bb.cpuHandle, Colors::LightSteelBlue);

			// Specify the buffers we are going to render to.
			//uGCmdList.OMSetRenderTarget(bb.cpuHandle, ds.cpuHandle);
			uGCmdList->OMSetRenderTargets(1, &bb.cpuHandle, false, nullptr);

			uGCmdList->SetGraphicsRootSignature(Ubpa::DXRenderer::Instance().GetRootSignature("defer lighting"));

			uGCmdList->SetGraphicsRootDescriptorTable(0, gb0.gpuHandle);

			uGCmdList->IASetVertexBuffers(0, 0, nullptr);
			uGCmdList->IASetIndexBuffer(nullptr);
			uGCmdList->IASetPrimitiveTopology(D3D_PRIMITIVE_TOPOLOGY_TRIANGLELIST);
			uGCmdList->DrawInstanced(6, 1, 0, 0);
		}
	);

	static bool flag{ false };
	if (!flag) {
		OutputDebugStringA(fg.ToGraphvizGraph().Dump().c_str());
		flag = true;
	}

	auto [success, crst] = fgCompiler.Compile(fg);
	fgExecutor.Execute(crst, *fgRsrcMngr);

    // Done recording commands.
    ThrowIfFailed(uGCmdList->Close());

    // Add the command list to the queue for execution.
	uCmdQueue.Execute(uGCmdList.raw.Get());

    // Swap the back and front buffers
    ThrowIfFailed(mSwapChain->Present(0, 0));
	mCurrBackBuffer = (mCurrBackBuffer + 1) % SwapChainBufferCount;

	mCurrFrameResource->Signal(uCmdQueue.raw.Get(), ++mCurrentFence);
}

void HW08App::OnMouseDown(WPARAM btnState, int x, int y)
{
    mLastMousePos.x = x;
    mLastMousePos.y = y;

    SetCapture(mhMainWnd);
}

void HW08App::OnMouseUp(WPARAM btnState, int x, int y)
{
    ReleaseCapture();
}

void HW08App::OnMouseMove(WPARAM btnState, int x, int y)
{
    if((btnState & MK_LBUTTON) != 0)
    {
        // Make each pixel correspond to a quarter of a degree.
        float dx = XMConvertToRadians(0.25f*static_cast<float>(x - mLastMousePos.x));
        float dy = XMConvertToRadians(0.25f*static_cast<float>(y - mLastMousePos.y));

        // Update angles based on input to orbit camera around box.
        mTheta += dx;
        mPhi += dy;

        // Restrict the angle mPhi.
        mPhi = MathHelper::Clamp(mPhi, 0.1f, MathHelper::Pi - 0.1f);
    }
    else if((btnState & MK_RBUTTON) != 0)
    {
        // Make each pixel correspond to 0.2 unit in the scene.
        float dx = 0.05f*static_cast<float>(x - mLastMousePos.x);
        float dy = 0.05f*static_cast<float>(y - mLastMousePos.y);

        // Update the camera radius based on input.
        mRadius += dx - dy;

        // Restrict the radius.
        mRadius = MathHelper::Clamp(mRadius, 2.0f, 150.0f);
    }

    mLastMousePos.x = x;
    mLastMousePos.y = y;
}

void HW08App::OnKeyboardInput(const GameTimer& gt)
{
}

void HW08App::UpdateCamera(const GameTimer& gt)
{
	// Convert Spherical to Cartesian coordinates.
	mEyePos.x = mRadius*sinf(mPhi)*cosf(mTheta);
	mEyePos.z = mRadius*sinf(mPhi)*sinf(mTheta);
	mEyePos.y = mRadius*cosf(mPhi);

	// Build the view matrix.
	XMVECTOR pos = XMVectorSet(mEyePos.x, mEyePos.y, mEyePos.z, 1.0f);
	XMVECTOR target = XMVectorZero();
	XMVECTOR up = XMVectorSet(0.0f, 1.0f, 0.0f, 0.0f);

	XMMATRIX view = XMMatrixLookAtLH(pos, target, up);
	XMStoreFloat4x4(&mView, view);
}

void HW08App::AnimateMaterials(const GameTimer& gt)
{

}

void HW08App::UpdateObjectCBs(const GameTimer& gt)
{
	auto currObjectCB = mCurrFrameResource
		->GetResource<Ubpa::DX12::ArrayUploadBuffer<ObjectConstants>>("ArrayUploadBuffer<ObjectConstants>");
	for(auto& e : mAllRitems)
	{
		// Only update the cbuffer data if the constants have changed.
		// This needs to be tracked per frame resource.
		if(e->NumFramesDirty > 0)
		{
			XMMATRIX world = XMLoadFloat4x4(&e->World);
			XMMATRIX texTransform = XMLoadFloat4x4(&e->TexTransform);

			ObjectConstants objConstants;
			XMStoreFloat4x4(&objConstants.World, XMMatrixTranspose(world));
			XMStoreFloat4x4(&objConstants.TexTransform, XMMatrixTranspose(texTransform));

			currObjectCB->Set(e->ObjCBIndex, objConstants);

			// Next FrameResource need to be updated too.
			e->NumFramesDirty--;
		}
	}
}

void HW08App::UpdateMaterialCBs(const GameTimer& gt)
{
	auto currMaterialCB = mCurrFrameResource
		->GetResource<Ubpa::DX12::ArrayUploadBuffer<MaterialConstants>>("ArrayUploadBuffer<MaterialConstants>");
	for(auto& e : mMaterials)
	{
		// Only update the cbuffer data if the constants have changed.  If the cbuffer
		// data changes, it needs to be updated for each FrameResource.
		Material* mat = e.second.get();
		if(mat->NumFramesDirty > 0)
		{
			XMMATRIX matTransform = XMLoadFloat4x4(&mat->MatTransform);

			MaterialConstants matConstants;
			matConstants.DiffuseAlbedo = mat->DiffuseAlbedo;
			matConstants.FresnelR0 = mat->FresnelR0;
			matConstants.Roughness = mat->Roughness;
			XMStoreFloat4x4(&matConstants.MatTransform, XMMatrixTranspose(matTransform));

			currMaterialCB->Set(mat->MatCBIndex, matConstants);

			// Next FrameResource need to be updated too.
			mat->NumFramesDirty--;
		}
	}
}

void HW08App::UpdateMainPassCB(const GameTimer& gt)
{
	XMMATRIX view = XMLoadFloat4x4(&mView);
	XMMATRIX proj = XMLoadFloat4x4(&mProj);

	XMMATRIX viewProj = XMMatrixMultiply(view, proj);
	XMMATRIX invView = XMMatrixInverse(&XMMatrixDeterminant(view), view);
	XMMATRIX invProj = XMMatrixInverse(&XMMatrixDeterminant(proj), proj);
	XMMATRIX invViewProj = XMMatrixInverse(&XMMatrixDeterminant(viewProj), viewProj);

	XMStoreFloat4x4(&mMainPassCB.View, XMMatrixTranspose(view));
	XMStoreFloat4x4(&mMainPassCB.InvView, XMMatrixTranspose(invView));
	XMStoreFloat4x4(&mMainPassCB.Proj, XMMatrixTranspose(proj));
	XMStoreFloat4x4(&mMainPassCB.InvProj, XMMatrixTranspose(invProj));
	XMStoreFloat4x4(&mMainPassCB.ViewProj, XMMatrixTranspose(viewProj));
	XMStoreFloat4x4(&mMainPassCB.InvViewProj, XMMatrixTranspose(invViewProj));
	mMainPassCB.EyePosW = mEyePos;
	mMainPassCB.RenderTargetSize = XMFLOAT2((float)mClientWidth, (float)mClientHeight);
	mMainPassCB.InvRenderTargetSize = XMFLOAT2(1.0f / mClientWidth, 1.0f / mClientHeight);
	mMainPassCB.NearZ = 1.0f;
	mMainPassCB.FarZ = 1000.0f;
	mMainPassCB.TotalTime = gt.TotalTime();
	mMainPassCB.DeltaTime = gt.DeltaTime();
	mMainPassCB.AmbientLight = { 0.25f, 0.25f, 0.35f, 1.0f };
	mMainPassCB.Lights[0].Direction = { 0.57735f, -0.57735f, 0.57735f };
	mMainPassCB.Lights[0].Strength = { 0.6f, 0.6f, 0.6f };
	mMainPassCB.Lights[1].Direction = { -0.57735f, -0.57735f, 0.57735f };
	mMainPassCB.Lights[1].Strength = { 0.3f, 0.3f, 0.3f };
	mMainPassCB.Lights[2].Direction = { 0.0f, -0.707f, -0.707f };
	mMainPassCB.Lights[2].Strength = { 0.15f, 0.15f, 0.15f };

	auto currPassCB = mCurrFrameResource
		->GetResource<Ubpa::DX12::ArrayUploadBuffer<PassConstants>>("gbPass constants");
	currPassCB->Set(0, mMainPassCB);
}

void HW08App::LoadTextures()
{
	std::array<std::wstring_view, 3> ironTextures{
		L"../data/textures/iron/albedo.dds",
		L"../data/textures/iron/roughness.dds",
		L"../data/textures/iron/metalness.dds"
	};

	Ubpa::DXRenderer::Instance().RegisterDDSTextureArrayFromFile(
		Ubpa::DXRenderer::Instance().GetUpload(),
		"iron",
		ironTextures.data(), ironTextures.size());
}

void HW08App::BuildRootSignature()
{
	{ // geometry
		CD3DX12_DESCRIPTOR_RANGE texTable;
		texTable.Init(D3D12_DESCRIPTOR_RANGE_TYPE_SRV, 3, 0);

		// Root parameter can be a table, root descriptor or root constants.
		CD3DX12_ROOT_PARAMETER slotRootParameter[4];

		// Perfomance TIP: Order from most frequent to least frequent.
		slotRootParameter[0].InitAsDescriptorTable(1, &texTable, D3D12_SHADER_VISIBILITY_PIXEL);
		slotRootParameter[1].InitAsConstantBufferView(0);
		slotRootParameter[2].InitAsConstantBufferView(1);
		slotRootParameter[3].InitAsConstantBufferView(2);

		auto staticSamplers = GetStaticSamplers();

		// A root signature is an array of root parameters.
		CD3DX12_ROOT_SIGNATURE_DESC rootSigDesc(4, slotRootParameter,
			(UINT)staticSamplers.size(), staticSamplers.data(),
			D3D12_ROOT_SIGNATURE_FLAG_ALLOW_INPUT_ASSEMBLER_INPUT_LAYOUT);

		Ubpa::DXRenderer::Instance().RegisterRootSignature("geometry", &rootSigDesc);
	}

	{ // screen
		CD3DX12_DESCRIPTOR_RANGE texTable;
		texTable.Init(D3D12_DESCRIPTOR_RANGE_TYPE_SRV, 1, 0);

		// Root parameter can be a table, root descriptor or root constants.
		CD3DX12_ROOT_PARAMETER slotRootParameter[1];

		// Perfomance TIP: Order from most frequent to least frequent.
		slotRootParameter[0].InitAsDescriptorTable(1, &texTable, D3D12_SHADER_VISIBILITY_PIXEL);

		auto staticSamplers = GetStaticSamplers();

		// A root signature is an array of root parameters.
		CD3DX12_ROOT_SIGNATURE_DESC rootSigDesc(1, slotRootParameter,
			(UINT)staticSamplers.size(), staticSamplers.data(),
			D3D12_ROOT_SIGNATURE_FLAG_ALLOW_INPUT_ASSEMBLER_INPUT_LAYOUT);

		Ubpa::DXRenderer::Instance().RegisterRootSignature("screen", &rootSigDesc);
	}
	{ // defer lighting
		CD3DX12_DESCRIPTOR_RANGE texTable;
		texTable.Init(D3D12_DESCRIPTOR_RANGE_TYPE_SRV, 3, 0);

		// Root parameter can be a table, root descriptor or root constants.
		CD3DX12_ROOT_PARAMETER slotRootParameter[4];

		// Perfomance TIP: Order from most frequent to least frequent.
		slotRootParameter[0].InitAsDescriptorTable(1, &texTable, D3D12_SHADER_VISIBILITY_PIXEL);
		slotRootParameter[1].InitAsConstantBufferView(0);
		slotRootParameter[2].InitAsConstantBufferView(1);
		slotRootParameter[3].InitAsConstantBufferView(2);

		auto staticSamplers = GetStaticSamplers();

		// A root signature is an array of root parameters.
		CD3DX12_ROOT_SIGNATURE_DESC rootSigDesc(4, slotRootParameter,
			(UINT)staticSamplers.size(), staticSamplers.data(),
			D3D12_ROOT_SIGNATURE_FLAG_ALLOW_INPUT_ASSEMBLER_INPUT_LAYOUT);

		Ubpa::DXRenderer::Instance().RegisterRootSignature("defer lighting", &rootSigDesc);
	}
}

void HW08App::BuildDescriptorHeaps()
{
}

void HW08App::BuildShadersAndInputLayout()
{
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("standardVS",
		L"..\\data\\shaders\\Default.hlsl", nullptr, "VS", "vs_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("opaquePS",
		L"..\\data\\shaders\\Default.hlsl", nullptr, "PS", "ps_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("screenVS",
		L"..\\data\\shaders\\Screen.hlsl", nullptr, "VS", "vs_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("screenPS",
		L"..\\data\\shaders\\Screen.hlsl", nullptr, "PS", "ps_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("geometryVS",
		L"..\\data\\shaders\\Geometry.hlsl", nullptr, "VS", "vs_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("geometryPS",
		L"..\\data\\shaders\\Geometry.hlsl", nullptr, "PS", "ps_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("deferLightingVS",
		L"..\\data\\shaders\\deferLighting.hlsl", nullptr, "VS", "vs_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("deferLightingPS",
		L"..\\data\\shaders\\deferLighting.hlsl", nullptr, "PS", "ps_5_0");

    mInputLayout =
    {
        { "POSITION", 0, DXGI_FORMAT_R32G32B32_FLOAT, 0, 0, D3D12_INPUT_CLASSIFICATION_PER_VERTEX_DATA, 0 },
        { "NORMAL", 0, DXGI_FORMAT_R32G32B32_FLOAT, 0, 12, D3D12_INPUT_CLASSIFICATION_PER_VERTEX_DATA, 0 },
		{ "TEXCOORD", 0, DXGI_FORMAT_R32G32_FLOAT, 0, 24, D3D12_INPUT_CLASSIFICATION_PER_VERTEX_DATA, 0 }
    };
}

void HW08App::BuildShapeGeometry()
{
	std::vector<Vertex> start_vertices;
	std::vector<Vertex> end_vertices;
	std::vector<std::vector<Vertex>> interp_vertices_arr(InterpNum);
	std::vector<std::uint32_t> indices;

	auto obj_source = std::make_unique<Ubpa::TriMesh>("../data/meshes/horse_source.obj");
	auto obj_target = std::make_unique<Ubpa::TriMesh>("../data/meshes/horse_target.obj");
	/*auto obj_source = std::make_unique<Ubpa::TriMesh>("../data/meshes/leaf_source.obj");
	auto obj_target = std::make_unique<Ubpa::TriMesh>("../data/meshes/leaf_target.obj");*/
	/*auto obj_source = std::make_unique<Ubpa::TriMesh>("../data/meshes/rabbit_source.obj");
	auto obj_target = std::make_unique<Ubpa::TriMesh>("../data/meshes/rabbit_target.obj");*/

	/*obj_source->CombineSamePositionVertex();
	obj_source->ScaleToUnit();
	obj_target->CombineSamePositionVertex();
	obj_target->ScaleToUnit();*/

	start_vertices.resize(obj_source->VertexNumber());
	end_vertices.resize(obj_source->VertexNumber());
	for (auto& vertices : interp_vertices_arr)
		vertices.resize(obj_source->VertexNumber());
	indices.resize(3 * obj_source->TriangleNumber());
	for (size_t i = 0; i < obj_source->VertexNumber(); i++) {
		start_vertices[i].Pos = obj_source->positions[i].as<XMFLOAT3>();
		end_vertices[i].Pos   = obj_target->positions[i].as<XMFLOAT3>();

		start_vertices[i].Normal = obj_source->normals[i].as<XMFLOAT3>();
		for (auto& vertices : interp_vertices_arr)
			vertices[i].Normal = obj_target->normals[i].as<XMFLOAT3>();
		end_vertices[i].Normal = obj_target->normals[i].as<XMFLOAT3>();

		start_vertices[i].TexC = obj_source->texcoords[i].as<XMFLOAT2>();
		for (auto& vertices : interp_vertices_arr)
			vertices[i].TexC = obj_target->texcoords[i].as<XMFLOAT2>();
		end_vertices[i].TexC = obj_target->texcoords[i].as<XMFLOAT2>();
	}
	for (size_t i = 0; i < obj_source->TriangleNumber(); i++) {
		indices[3 * i + 0] = obj_source->indices[i][0];
		indices[3 * i + 1] = obj_source->indices[i][1];
		indices[3 * i + 2] = obj_source->indices[i][2];
	}

	Ubpa::HEMesh<Ubpa::HEMeshTriats_EmptyE<V, F, H>> hemesh(std::vector<size_t>{indices.begin(), indices.end()}, 3);
	assert(hemesh.IsValid() && hemesh.IsTriMesh() && hemesh.NumBoundaries() == 1);

	for (size_t i = 0; i < start_vertices.size(); i++) {
		hemesh.Vertices().at(i)->p = obj_source->positions[i].cast_to<Ubpa::pointf2>();
		hemesh.Vertices().at(i)->q = obj_target->positions[i].cast_to<Ubpa::pointf2>();
	}

	// compute area, cot_theta
	for (auto tri : hemesh.Polygons()) {
		auto he01 = tri->HalfEdge();
		auto he12 = he01->Next();
		auto he20 = he12->Next();

		auto v0 = he01->Origin();
		auto v1 = he12->Origin();
		auto v2 = he20->Origin();

		auto p0_p1 = v1->p - v0->p;
		auto p0_p2 = v2->p - v0->p;
		auto p1_p2 = v2->p - v1->p;
		auto p1_p0 = -p0_p1;
		auto p2_p0 = -p0_p2;
		auto p2_p1 = -p1_p2;

		tri->area = 0.5f * std::sqrt(std::max(0.f,1 - Ubpa::pow2(p0_p1.cos_theta(p0_p2)))) * p0_p1.norm() * p0_p2.norm();

		he01->cot_theta = p2_p0.cot_theta(p2_p1);
		he12->cot_theta = p0_p1.cot_theta(p0_p2);
		he20->cot_theta = p1_p2.cot_theta(p1_p0);
	}

	for (auto tri : hemesh.Polygons())
		tri->Init();

	// fix 1 vertex
	auto fix_v0 = hemesh.Vertices().front();
	auto fix_idx0 = static_cast<int>(hemesh.Index(fix_v0));

	// pre-compute A
	size_t N = hemesh.Vertices().size();
	Eigen::SparseMatrix<float> global_A(N, N);
	std::vector<Eigen::Triplet<float>> global_triplets;
	global_triplets.emplace_back(fix_idx0, fix_idx0, 1.f);
	for (auto vi : hemesh.Vertices()) {
		auto i = static_cast<int>(hemesh.Index(vi));
		if (vi == fix_v0)
			continue;
		float diag = 0.f;
		for (auto he_ij : vi->OutHalfEdges()) {
			auto he_ji = he_ij->Pair();
			auto vj = he_ji->Origin();
			auto j = static_cast<int>(hemesh.Index(vj));
			float cot_theta_ij = he_ij->cot_theta; // if he_ij is boundary, cot_theta_ij == 0
			float cot_theta_ji = he_ji->cot_theta; // if he_ji is boundary, cot_theta_ji == 0
			global_triplets.emplace_back(i, j, -(cot_theta_ij + cot_theta_ji));

			diag += cot_theta_ij + cot_theta_ji;
		}
		global_triplets.emplace_back(i, i, diag);
	}
	global_A.setFromTriplets(global_triplets.begin(), global_triplets.end());
	Eigen::SparseLU<Eigen::SparseMatrix<float>, Eigen::COLAMDOrdering<int>> global_solver;
	global_solver.analyzePattern(global_A);
	global_solver.factorize(global_A);

	for (size_t curInterp = 0; curInterp < InterpNum; curInterp++) {
		float t = static_cast<float>(curInterp) / static_cast<float>(InterpNum);

		// local
		for (auto tri : hemesh.Polygons())
			tri->UpdateLt(t);

		// global
		Eigen::MatrixXf global_B(N, 2);
		global_B.setZero();
		auto fix_v0_t = fix_v0->p.lerp(fix_v0->q, t);
		global_B(fix_idx0, 0) = fix_v0_t[0];
		global_B(fix_idx0, 1) = fix_v0_t[1];
		for (auto vi : hemesh.Vertices()) {
			if (vi == fix_v0)
				continue;

			auto i = static_cast<int>(hemesh.Index(vi));
			for (auto he_ij : vi->OutHalfEdges()) {
				auto he_ji = he_ij->Pair();
				auto vj = he_ji->Origin();

				if (he_ij->Polygon() != nullptr) {
					auto w_dp_ij = he_ij->cot_theta * he_ij->Polygon()->Lt * (vi->p - vj->p);
					global_B(i, 0) += w_dp_ij[0];
					global_B(i, 1) += w_dp_ij[1];
				}

				if (he_ji->Polygon() != nullptr) {
					auto w_dp_ji = he_ji->cot_theta * he_ji->Polygon()->Lt * (vj->p - vi->p);
					global_B(i, 0) -= w_dp_ji[0];
					global_B(i, 1) -= w_dp_ji[1];
				}
			}
		}
		Eigen::MatrixXf X = global_solver.solve(global_B);
		for (auto v : hemesh.Vertices()) {
			auto idx = hemesh.Index(v);
			interp_vertices_arr[curInterp][idx].Pos.x = X(idx, 0);
			interp_vertices_arr[curInterp][idx].Pos.y = X(idx, 1);
			interp_vertices_arr[curInterp][idx].Pos.z = 0.f;
		}
	}

	Ubpa::DX12::SubmeshGeometry objSubmesh;
	objSubmesh.IndexCount = obj_source->indices.size() * 3;
	objSubmesh.StartIndexLocation = 0;
	objSubmesh.BaseVertexLocation = 0;
	Ubpa::DXRenderer::Instance()
		.RegisterStaticMeshGeometry(
			Ubpa::DXRenderer::Instance().GetUpload(), "start_objGeo",
			start_vertices.data(), (UINT)start_vertices.size(), sizeof(Vertex),
			indices.data(), (UINT)indices.size(), DXGI_FORMAT_R32_UINT)
		.submeshGeometries["obj"] = objSubmesh;
	Ubpa::DXRenderer::Instance()
		.RegisterStaticMeshGeometry(
			Ubpa::DXRenderer::Instance().GetUpload(), "end_objGeo",
			end_vertices.data(), (UINT)end_vertices.size(), sizeof(Vertex),
			indices.data(), (UINT)indices.size(), DXGI_FORMAT_R32_UINT)
		.submeshGeometries["obj"] = objSubmesh;
	for (size_t i = 0; i < InterpNum; i++) {
		Ubpa::DXRenderer::Instance()
			.RegisterStaticMeshGeometry(
				Ubpa::DXRenderer::Instance().GetUpload(), "interp_" + std::to_string(i) + "_objGeo",
				interp_vertices_arr[i].data(), (UINT)interp_vertices_arr[i].size(), sizeof(Vertex),
				indices.data(), (UINT)indices.size(), DXGI_FORMAT_R32_UINT)
			.submeshGeometries["obj"] = objSubmesh;
	}

	trimeshes.emplace("obj_source", std::move(obj_source));
	trimeshes.emplace("obj_target", std::move(obj_target));
}

void HW08App::BuildPSOs()
{
	auto screenPsoDesc = Ubpa::DX12::Desc::PSO::Basic(
		Ubpa::DXRenderer::Instance().GetRootSignature("screen"),
		nullptr, 0,
		Ubpa::DXRenderer::Instance().GetShaderByteCode("screenVS"),
		Ubpa::DXRenderer::Instance().GetShaderByteCode("screenPS"),
		mBackBufferFormat,
		DXGI_FORMAT_UNKNOWN
	);
	Ubpa::DXRenderer::Instance().RegisterPSO("screen", &screenPsoDesc);

	auto geometryPsoDesc = Ubpa::DX12::Desc::PSO::MRT(
		Ubpa::DXRenderer::Instance().GetRootSignature("geometry"),
		mInputLayout.data(), (UINT)mInputLayout.size(),
		Ubpa::DXRenderer::Instance().GetShaderByteCode("geometryVS"),
		Ubpa::DXRenderer::Instance().GetShaderByteCode("geometryPS"),
		3,
		DXGI_FORMAT_R32G32B32A32_FLOAT,
		mDepthStencilFormat
	);
	geometryPsoDesc.RasterizerState.FillMode = D3D12_FILL_MODE_WIREFRAME;
	Ubpa::DXRenderer::Instance().RegisterPSO("geometry", &geometryPsoDesc);

	auto deferLightingPsoDesc = Ubpa::DX12::Desc::PSO::Basic(
		Ubpa::DXRenderer::Instance().GetRootSignature("defer lighting"),
		nullptr, 0,
		Ubpa::DXRenderer::Instance().GetShaderByteCode("deferLightingVS"),
		Ubpa::DXRenderer::Instance().GetShaderByteCode("deferLightingPS"),
		mBackBufferFormat,
		DXGI_FORMAT_UNKNOWN
	);
	Ubpa::DXRenderer::Instance().RegisterPSO("defer lighting", &deferLightingPsoDesc);
}

void HW08App::BuildFrameResources()
{
    for(int i = 0; i < gNumFrameResources; ++i)
    {
		auto fr = std::make_unique<Ubpa::DX12::FrameResource>(mFence.Get());

		ID3D12CommandAllocator* allocator;
		ThrowIfFailed(uDevice->CreateCommandAllocator(
			D3D12_COMMAND_LIST_TYPE_DIRECT,
			IID_PPV_ARGS(&allocator)));

		fr->RegisterResource("CommandAllocator", allocator, [](void* allocator) {
			reinterpret_cast<ID3D12CommandAllocator*>(allocator)->Release();
		});

		fr->RegisterResource("gbPass constants",
			new Ubpa::DX12::ArrayUploadBuffer<PassConstants>{ uDevice.raw.Get(), 1, true });

		fr->RegisterResource("ArrayUploadBuffer<MaterialConstants>",
			new Ubpa::DX12::ArrayUploadBuffer<MaterialConstants>{ uDevice.raw.Get(), mMaterials.size(), true });

		fr->RegisterResource("ArrayUploadBuffer<ObjectConstants>",
			new Ubpa::DX12::ArrayUploadBuffer<ObjectConstants>{ uDevice.raw.Get(), mAllRitems.size(), true });

		auto fgRsrcMngr = new Ubpa::DX12::FG::RsrcMngr;
		fgRsrcMngr->Init(uGCmdList, uDevice);
		fr->RegisterResource("FrameGraphRsrcMngr", fgRsrcMngr);

		mFrameResources.emplace_back(std::move(fr));
    }
}

void HW08App::BuildMaterials()
{
	auto iron = std::make_unique<Material>();
	iron->Name = "iron";
	iron->MatCBIndex = 0;
	iron->DiffuseSrvGpuHandle = Ubpa::DXRenderer::Instance().GetTextureSrvGpuHandle("iron");
	iron->DiffuseAlbedo = XMFLOAT4(1.0f, 1.0f, 1.0f, 1.0f);
	iron->FresnelR0 = XMFLOAT3(0.05f, 0.05f, 0.05f);
	iron->Roughness = 0.2f;

	mMaterials["iron"] = std::move(iron);
}

void HW08App::BuildRenderItems()
{
	auto start_objRitem = std::make_unique<RenderItem>();
	start_objRitem->World = Ubpa::transformf(Ubpa::pointf3{ 1,1,0 }, Ubpa::quatf{ {0,1,0}, Ubpa::to_radian(180.f) }).as<XMFLOAT4X4>();
	start_objRitem->ObjCBIndex = 0;
	start_objRitem->Mat = mMaterials["iron"].get();
	start_objRitem->Geo = &Ubpa::DXRenderer::Instance().GetMeshGeometry("start_objGeo");
	start_objRitem->PrimitiveType = D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST;
	start_objRitem->IndexCount = start_objRitem->Geo->submeshGeometries["obj"].IndexCount;
	start_objRitem->StartIndexLocation = start_objRitem->Geo->submeshGeometries["obj"].StartIndexLocation;
	start_objRitem->BaseVertexLocation = start_objRitem->Geo->submeshGeometries["obj"].BaseVertexLocation;
	mAllRitems.push_back(std::move(start_objRitem));

	auto end_objRitem = std::make_unique<RenderItem>();
	end_objRitem->World = Ubpa::transformf(Ubpa::pointf3{ 1,-1,0 }, Ubpa::quatf{ {0,1,0}, Ubpa::to_radian(180.f) }).as<XMFLOAT4X4>();
	end_objRitem->ObjCBIndex = 1;
	end_objRitem->Mat = mMaterials["iron"].get();
	end_objRitem->Geo = &Ubpa::DXRenderer::Instance().GetMeshGeometry("end_objGeo");
	end_objRitem->PrimitiveType = D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST;
	end_objRitem->IndexCount = end_objRitem->Geo->submeshGeometries["obj"].IndexCount;
	end_objRitem->StartIndexLocation = end_objRitem->Geo->submeshGeometries["obj"].StartIndexLocation;
	end_objRitem->BaseVertexLocation = end_objRitem->Geo->submeshGeometries["obj"].BaseVertexLocation;
	mAllRitems.push_back(std::move(end_objRitem));

	for (size_t i = 0; i < InterpNum; i++) {
		auto interp_objRitem = std::make_unique<RenderItem>();
		interp_objRitem->World = Ubpa::transformf(Ubpa::pointf3{ -1,0,0 }, Ubpa::quatf{ {0,1,0}, Ubpa::to_radian(180.f) })
			.as<XMFLOAT4X4>();
		interp_objRitem->ObjCBIndex = 2 + i;
		interp_objRitem->Mat = mMaterials["iron"].get();
		interp_objRitem->Geo = &Ubpa::DXRenderer::Instance().GetMeshGeometry("interp_" + std::to_string(i) + "_objGeo");
		interp_objRitem->PrimitiveType = D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST;
		interp_objRitem->IndexCount = interp_objRitem->Geo->submeshGeometries["obj"].IndexCount;
		interp_objRitem->StartIndexLocation = interp_objRitem->Geo->submeshGeometries["obj"].StartIndexLocation;
		interp_objRitem->BaseVertexLocation = interp_objRitem->Geo->submeshGeometries["obj"].BaseVertexLocation;
		mAllRitems.push_back(std::move(interp_objRitem));
	}

	// All the render items are opaque.
	for(auto& e : mAllRitems)
		mOpaqueRitems.push_back(e.get());
}

void HW08App::DrawRenderItems(ID3D12GraphicsCommandList* cmdList, const std::vector<RenderItem*>& ritems)
{
    UINT objCBByteSize = Ubpa::DX12::Util::CalcConstantBufferByteSize(sizeof(ObjectConstants));
    UINT matCBByteSize = Ubpa::DX12::Util::CalcConstantBufferByteSize(sizeof(MaterialConstants));

	auto objectCB = mCurrFrameResource
		->GetResource<Ubpa::DX12::ArrayUploadBuffer<ObjectConstants>>("ArrayUploadBuffer<ObjectConstants>")
		->GetResource();
	auto matCB = mCurrFrameResource
		->GetResource<Ubpa::DX12::ArrayUploadBuffer<MaterialConstants>>("ArrayUploadBuffer<MaterialConstants>")
		->GetResource();

    // For each render item...
    for(size_t i = 0; i < ritems.size(); ++i)
    {
        auto ri = ritems[i];

        cmdList->IASetVertexBuffers(0, 1, &ri->Geo->VertexBufferView());
        cmdList->IASetIndexBuffer(&ri->Geo->IndexBufferView());
        cmdList->IASetPrimitiveTopology(ri->PrimitiveType);

        D3D12_GPU_VIRTUAL_ADDRESS objCBAddress = objectCB->GetGPUVirtualAddress() + ri->ObjCBIndex*objCBByteSize;
		D3D12_GPU_VIRTUAL_ADDRESS matCBAddress = matCB->GetGPUVirtualAddress() + ri->Mat->MatCBIndex*matCBByteSize;

		cmdList->SetGraphicsRootDescriptorTable(0, ri->Mat->DiffuseSrvGpuHandle);
        cmdList->SetGraphicsRootConstantBufferView(1, objCBAddress);
        cmdList->SetGraphicsRootConstantBufferView(3, matCBAddress);

        cmdList->DrawIndexedInstanced(ri->IndexCount, 1, ri->StartIndexLocation, ri->BaseVertexLocation, 0);
    }
}

std::array<const CD3DX12_STATIC_SAMPLER_DESC, 6> HW08App::GetStaticSamplers()
{
	// Applications usually only need a handful of samplers.  So just define them all up front
	// and keep them available as part of the root signature.

	const CD3DX12_STATIC_SAMPLER_DESC pointWrap(
		0, // shaderRegister
		D3D12_FILTER_MIN_MAG_MIP_POINT, // filter
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_WRAP); // addressW

	const CD3DX12_STATIC_SAMPLER_DESC pointClamp(
		1, // shaderRegister
		D3D12_FILTER_MIN_MAG_MIP_POINT, // filter
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP); // addressW

	const CD3DX12_STATIC_SAMPLER_DESC linearWrap(
		2, // shaderRegister
		D3D12_FILTER_MIN_MAG_MIP_LINEAR, // filter
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_WRAP); // addressW

	const CD3DX12_STATIC_SAMPLER_DESC linearClamp(
		3, // shaderRegister
		D3D12_FILTER_MIN_MAG_MIP_LINEAR, // filter
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP); // addressW

	const CD3DX12_STATIC_SAMPLER_DESC anisotropicWrap(
		4, // shaderRegister
		D3D12_FILTER_ANISOTROPIC, // filter
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressW
		0.0f,                             // mipLODBias
		8);                               // maxAnisotropy

	const CD3DX12_STATIC_SAMPLER_DESC anisotropicClamp(
		5, // shaderRegister
		D3D12_FILTER_ANISOTROPIC, // filter
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressW
		0.0f,                              // mipLODBias
		8);                                // maxAnisotropy

	return {
		pointWrap, pointClamp,
		linearWrap, linearClamp,
		anisotropicWrap, anisotropicClamp };
}