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

// With contributions from Cornelius Steinhardt

#ifndef MTL_MATRIX_GIVENS_INCLUDE
#define MTL_MATRIX_GIVENS_INCLUDE

#include <cmath>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/householder.hpp>
#include <boost/numeric/mtl/operation/rank_one_update.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>

namespace mtl { namespace matrix {

/// Given's transformator
/** Requires Hessenberg form, i.e. for transformations near the diagonal.
    For general form use qr_givens.
    \sa qr_givens. **/
template <typename Matrix>
class givens
{
    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Collection<Matrix>::size_type    size_type;

  public:
    /// Re-set the rotation parameters \p a and \p b
    void set_rotation(value_type a, value_type b)
    {
	using std::abs;
	value_type zero= math::zero(a), one= math::one(b), t;

	if ( b == zero ) {
	    c= one; s= zero;
	} else if ( abs(b) > abs(a) ) {
	    t= -a / b;
	    s= one / sqrt(one + t*t);
	    c= s * t;
	} else {
	    t= -b / a;
	    c= one / sqrt(one + t*t);
	    s= c * t;
	}
	G= c, s,
	  -s, c;
    }

    /// Constructor takes %matrix \p H to be transformed and the rotation parameters \p a and \p b
    givens(Matrix& H, value_type a, value_type b) : H(H), G(2, 2)
    {	set_rotation(a, b);    }

    /// Given's transformation of \p H with \p G regarding column \p k
    Matrix& trafo(const Matrix& G, size_type k)
    {
	    irange r(k,k+2);
	    // trans(H[r][ind])*= G; H[ind][r]*= G; // most compact form but does not work yet

	    Matrix col_block(H[r][iall]), col_perm(trans(G) * col_block);
	    H[r][iall]= col_perm;
	    Matrix row_perm(H[iall][r] * G);
	    H[iall][r]= row_perm;

	    return H;
    }

    /// Given's transformation of \p H regarding column \p k
    Matrix& trafo(size_type k)
    {
	return trafo(G, k);
    }

  private:
    Matrix&    H, G;
    value_type c, s;
};

}// namespace matrix

namespace vector {

/// Given's transformator on %vector (swap a*line(k) with b*line(k+1) )
template <typename Vector>
class givens
{
    typedef typename Collection<Vector>::value_type   value_type;
    typedef typename Collection<Vector>::size_type    size_type;

  public:
    /// Constructor takes %vector \p v to be transformed and the rotation parameters \p a and \p b
    givens(Vector& v, value_type a, value_type b) : v(v), a(a), b(b)
    {  }

    /// Given's transformation of \p v with \p a and \p b regarding column \p k
    Vector& trafo(size_type k)
    {
	    value_type w1(0), w2(0);
	    w1= a*v[k] - b*v[k+1]; //given's rotation on solution
            w2= b*v[k] + a*v[k+1]; //rotation on vector
            v[k]= w1;
            v[k+1]= w2;

	    return v;
    }

  private:
    Vector&    v;
    value_type a, b;
};

}// namespace vector

} // namespace mtl

#endif // MTL_MATRIX_GIVENS_INCLUDE


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include <algorithm>
#include <iostream>

#include <Eigen/Dense>
#include <cmath>

#include "Open3D/Camera/PinholeCameraIntrinsic.h"
#include "Open3D/Geometry/Image.h"

namespace open3d {
namespace geometry {

std::shared_ptr<Image> Image::CreateDepthToCameraDistanceMultiplierFloatImage(
        const camera::PinholeCameraIntrinsic &intrinsic) {
    auto fimage = std::make_shared<Image>();
    fimage->Prepare(intrinsic.width_, intrinsic.height_, 1, 4);
    float ffl_inv[2] = {
            1.0f / (float)intrinsic.GetFocalLength().first,
            1.0f / (float)intrinsic.GetFocalLength().second,
    };
    float fpp[2] = {
            (float)intrinsic.GetPrincipalPoint().first,
            (float)intrinsic.GetPrincipalPoint().second,
    };
    std::vector<float> xx(intrinsic.width_);
    std::vector<float> yy(intrinsic.height_);
    for (int j = 0; j < intrinsic.width_; j++) {
        xx[j] = (j - fpp[0]) * ffl_inv[0];
    }
    for (int i = 0; i < intrinsic.height_; i++) {
        yy[i] = (i - fpp[1]) * ffl_inv[1];
    }
    for (int i = 0; i < intrinsic.height_; i++) {
        float *fp =
                (float *)(fimage->data_.data() + i * fimage->BytesPerLine());
        for (int j = 0; j < intrinsic.width_; j++, fp++) {
            *fp = sqrtf(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f);
        }
    }
    return fimage;
}

std::shared_ptr<Image> Image::CreateWeightImage(
        const camera::PinholeCameraIntrinsic &intrinsic) const {

    auto output = std::make_shared<Image>();

    output->Prepare(intrinsic.width_, intrinsic.height_, 1, 4);
    auto focal_length = intrinsic.GetFocalLength();
    auto principal_point = intrinsic.GetPrincipalPoint();

    #ifdef _OPENMP
    #ifdef _WIN32
    #pragma omp parallel for schedule(static)
    #else
    #pragma omp parallel for collapse(2) schedule(static)
    #endif
    #endif
    for (int i = 0; i < output->height_; i++) {
        for (int j = 0; j < output->width_; j++) {
            float *p = output->PointerAt<float>(j, i);
            float *ip = PointerAt<float>(j, i);
            double weight = 0.0;

            if (*ip > 0) {
                if(i > 0 && j > 0 && i < output->height_-1 && j < output->width_-1){

                    // computing normalized vertex
                    double z = (double)(*ip);
                    double x = (j - principal_point.first) * z / focal_length.first;
                    double y =
                            (i - principal_point.second) * z / focal_length.second;
                    Eigen::Vector3d point = Eigen::Vector3d(x, y, z);
                    Eigen::Vector3d v_norm = point.normalized();

                    //computing normalized normal
                    float *dx1 = PointerAt<float>(j+1, i);
                    float *dx2 = PointerAt<float>(j-1, i);

                    float *dy1 = PointerAt<float>(j, i+1);
                    float *dy2 = PointerAt<float>(j, i-1);

                    double dzdx = (((double)*dx1 - (double)*dx2)/2.0)*1000.0;
                    double dzdy = (((double)*dy1 - (double)*dy2)/2.0)*1000.0;

                    Eigen::Vector3d normal = Eigen::Vector3d(-dzdx, -dzdy, 1.0);
                    Eigen::Vector3d n_norm = normal.normalized();

                    // Eigen::Vector3d captureDir = Eigen::Vector3d(0, 0, 1.0);

                    // double w1 = abs(captureDir.dot(n_norm));
                    // double w2 = abs(captureDir.dot(v_norm));

                    // weight = w1;
                    // weight = w2;
                    double w = abs(n_norm.dot(v_norm));
                    weight = w * w;

                    // // Adding gaussian weight centering at principle point
                    // double w = (double)output->width_/2.0;
                    // double h = (double)output->height_/2.0;
                    // double w_x = ((double)j - w)/w ;
                    // double w_y = ((double)i - h)/h;

                    // double d = sqrt(w_x * w_x + w_y * w_y);
                    // weight = exp(-((d*d)/(2.0))); // assuming mu = 0 and sigma = 1.0
                }
                else{
                    weight = 1.0f; // if this weight is set to 0, some of the points are missing in final integrated reconstruction. Therefore assigning it 1.0
                }

            }
            *p = (float)weight;
        }
    }
    return output;
}

std::shared_ptr<Image> Image::CreateFloatImage(
        Image::ColorToIntensityConversionType type /* = WEIGHTED*/) const {
    auto fimage = std::make_shared<Image>();
    if (IsEmpty()) {
        return fimage;
    }
    fimage->Prepare(width_, height_, 1, 4);
    for (int i = 0; i < height_ * width_; i++) {
        float *p = (float *)(fimage->data_.data() + i * 4);
        const uint8_t *pi =
                data_.data() + i * num_of_channels_ * bytes_per_channel_;
        if (num_of_channels_ == 1) {
            // grayscale image
            if (bytes_per_channel_ == 1) {
                *p = (float)(*pi) / 255.0f;
            } else if (bytes_per_channel_ == 2) {
                const uint16_t *pi16 = (const uint16_t *)pi;
                *p = (float)(*pi16);
            } else if (bytes_per_channel_ == 4) {
                const float *pf = (const float *)pi;
                *p = *pf;
            }
        } else if (num_of_channels_ == 3) {
            if (bytes_per_channel_ == 1) {
                if (type == Image::ColorToIntensityConversionType::Equal) {
                    *p = ((float)(pi[0]) + (float)(pi[1]) + (float)(pi[2])) /
                         3.0f / 255.0f;
                } else if (type ==
                           Image::ColorToIntensityConversionType::Weighted) {
                    *p = (0.2990f * (float)(pi[0]) + 0.5870f * (float)(pi[1]) +
                          0.1140f * (float)(pi[2])) /
                         255.0f;
                }
            } else if (bytes_per_channel_ == 2) {
                const uint16_t *pi16 = (const uint16_t *)pi;
                if (type == Image::ColorToIntensityConversionType::Equal) {
                    *p = ((float)(pi16[0]) + (float)(pi16[1]) +
                          (float)(pi16[2])) /
                         3.0f;
                } else if (type ==
                           Image::ColorToIntensityConversionType::Weighted) {
                    *p = (0.2990f * (float)(pi16[0]) +
                          0.5870f * (float)(pi16[1]) +
                          0.1140f * (float)(pi16[2]));
                }
            } else if (bytes_per_channel_ == 4) {
                const float *pf = (const float *)pi;
                if (type == Image::ColorToIntensityConversionType::Equal) {
                    *p = (pf[0] + pf[1] + pf[2]) / 3.0f;
                } else if (type ==
                           Image::ColorToIntensityConversionType::Weighted) {
                    *p = (0.2990f * pf[0] + 0.5870f * pf[1] + 0.1140f * pf[2]);
                }
            }
        }
    }
    return fimage;
}

template <typename T>
std::shared_ptr<Image> Image::CreateImageFromFloatImage() const {
    auto output = std::make_shared<Image>();
    if (num_of_channels_ != 1 || bytes_per_channel_ != 4) {
        utility::LogError(
                "[CreateImageFromFloatImage] Unsupported image format.");
    }

    output->Prepare(width_, height_, num_of_channels_, sizeof(T));
    const float *pi = (const float *)data_.data();
    T *p = (T *)output->data_.data();
    for (int i = 0; i < height_ * width_; i++, p++, pi++) {
        if (sizeof(T) == 1) *p = static_cast<T>(*pi * 255.0f);
        if (sizeof(T) == 2) *p = static_cast<T>(*pi);
    }
    return output;
}

template std::shared_ptr<Image> Image::CreateImageFromFloatImage<uint8_t>()
        const;
template std::shared_ptr<Image> Image::CreateImageFromFloatImage<uint16_t>()
        const;

ImagePyramid Image::CreatePyramid(size_t num_of_levels,
                                  bool with_gaussian_filter /*= true*/) const {
    std::vector<std::shared_ptr<Image>> pyramid_image;
    pyramid_image.clear();
    if ((num_of_channels_ != 1) || (bytes_per_channel_ != 4)) {
        utility::LogError("[CreateImagePyramid] Unsupported image format.");
    }

    for (size_t i = 0; i < num_of_levels; i++) {
        if (i == 0) {
            std::shared_ptr<Image> input_copy_ptr = std::make_shared<Image>();
            *input_copy_ptr = *this;
            pyramid_image.push_back(input_copy_ptr);
        } else {
            if (with_gaussian_filter) {
                // https://en.wikipedia.org/wiki/Pyramid_(image_processing)
                auto level_b = pyramid_image[i - 1]->Filter(
                        Image::FilterType::Gaussian3);
                auto level_bd = level_b->Downsample();
                pyramid_image.push_back(level_bd);
            } else {
                auto level_d = pyramid_image[i - 1]->Downsample();
                pyramid_image.push_back(level_d);
            }
        }
    }
    return pyramid_image;
}

}  // namespace geometry
}  // namespace open3d


#include <iostream>
#include <array>
#include <mtao/algebra/horner_evaluation.hpp>
#include "mtao/geometry/grid/grid_utils.h"
#include <Eigen/Dense>
#include <iterator>

int main() {
    std::array<int,3> coeffs{{1,1,0}};
    for(int i = 0; i < 10; ++i) {
        std::cout << mtao::algebra::horner_evaluate(i,coeffs) << std::endl;
    }
    {
        std::array<int,2> coeffs{{3,4}};
        for(int i = 0; i < coeffs[0]; ++i) {
            for(int j = 0; j < coeffs[1]; ++j) {
                std::cout << mtao::algebra::horner_rowminor_index(std::array<int,2>{{i,j}},coeffs) << " ";
            }
            std::cout << std::endl;
        }
    }
    std::cout << std::endl << std::endl;
    {
        std::array<int,2> coeffs{{3,4}};
        for(int i = 0; i < coeffs[0]; ++i) {
            for(int j = 0; j < coeffs[1]; ++j) {
                std::cout << mtao::algebra::horner_rowmajor_index(std::array<int,2>{{i,j}},coeffs) << " ";
            }
            std::cout << std::endl;
        }
    }
    {
        std::array<int,3> coeffs{{3,4,5}};
        for(int i = 0; i < coeffs[0]; ++i) {
            for(int j = 0; j < coeffs[1]; ++j) {
                for(int k = 0; k < coeffs[2]; ++k) {
                    std::cout << mtao::algebra::horner_rowminor_index(std::array<int,3>{{i,j,k}},coeffs) << " ";
                }
                std::cout << std::endl;
            }
            std::cout << std::endl;
        }
    }
    std::cout << std::endl << std::endl;
    {
        std::array<int,3> coeffs{{3,4,5}};
        for(int i = 0; i < coeffs[0]; ++i) {
            for(int j = 0; j < coeffs[1]; ++j) {
                for(int k = 0; k < coeffs[2]; ++k) {
                    std::cout << mtao::algebra::horner_rowmajor_index(std::array<int,3>{{i,j,k}},coeffs) << " ";
                }
                std::cout << std::endl;
            }
            std::cout << std::endl;
        }
    }

    {
        Eigen::Matrix<int,3,4,Eigen::ColMajor> A;
        for(int i = 0; i < A.rows(); ++i) {
            for(int j = 0; j < A.cols(); ++j) {
                A(i,j) = &A(i,j) - A.data();

            }
        }
        std::array<int,2> coeffs{{3,4}};
        for(int i = 0; i < coeffs[0]; ++i) {
            for(int j = 0; j < coeffs[1]; ++j) {
                std::cout << mtao::algebra::horner_rowminor_index(std::array<int,2>{{i,j}},coeffs) << ":" << A(i,j) << " ";
            }
            std::cout << std::endl;
        }
    }
    std::cout << std::endl << std::endl;
    {
        Eigen::Matrix<int,3,4,Eigen::RowMajor> A;
        for(int i = 0; i < A.rows(); ++i) {
            for(int j = 0; j < A.cols(); ++j) {
                A(i,j) = &A(i,j) - A.data();

            }
        }
        std::array<int,2> coeffs{{3,4}};
        for(int i = 0; i < coeffs[0]; ++i) {
            for(int j = 0; j < coeffs[1]; ++j) {
                std::cout << mtao::algebra::horner_rowmajor_index(std::array<int,2>{{i,j}},coeffs)  << ":" << A(i,j) << " ";
            }
            std::cout << std::endl;
        }
    }

    auto arrstr = [](auto&& idx) {
        using ArrType = mtao::types::remove_cvref_t<decltype(idx)>;
        using VT = typename ArrType::value_type;
        std::stringstream ss;

        std::copy(idx.begin(),idx.end(),std::ostream_iterator<VT>(ss,","));
        return ss.str();

    };

    using namespace mtao::algebra;
    auto test_inversing = [&](auto&& coeffs) {
        std::cout << "Minor" << coeffs.size() << std::endl;
        mtao::geometry::grid::utils::multi_loop(coeffs,[&](auto&& ij) {
                int idx = horner_rowminor_index(ij,coeffs);
                auto arr = horner_rowminor_inverse_index(idx,coeffs);
                std::cout << arrstr(ij) << " => " << idx << " => " << arrstr(arr) << std::endl;
                assert(ij == arr);
                });
        std::cout << std::endl;
        std::cout << "Major" << coeffs.size() << std::endl;
        mtao::geometry::grid::utils::multi_loop(coeffs,[&](auto&& ij) {
                int idx = horner_rowmajor_index(ij,coeffs);
                auto arr = horner_rowmajor_inverse_index(idx,coeffs);
                std::cout << arrstr(ij) << " => " << idx << " => " << arrstr(arr) << std::endl;
                assert(ij == arr);
                });
    };
    test_inversing(std::array<int,2>{{3,4}});
    std::cout << std::endl;
    std::cout << std::endl;
    test_inversing(std::array<int,3>{{3,4,5}});
    std::cout << std::endl;
    std::cout << std::endl;
    test_inversing(std::array<int,4>{{3,4,5,7}});

}


#include <iostream>
//#include <unordered_map>
#include <vector>
#include <algorithm>

// Check for alternative Multiprecision Implementation from boost
#if defined(CPP_INT)
    // alternative cpp_int (~4x slower than GMP)
    // no external linking required
    // use compile command with "-DCPP_INT"
    #include <boost/multiprecision/cpp_int.hpp>
    using boost_mp_int = boost::multiprecision::cpp_int;
#elif defined(TOM_INT)
    // alternative libtommath (~50x slower than GMP)
    // use compile command with "-DTOM_INT -ltommath"
    #include <boost/multiprecision/tommath.hpp>
    using boost_mp_int = boost::multiprecision::tom_int;
#else
    // Default GMP (fastest)
    // use compile command with "-lgmp"
    #include <boost/multiprecision/gmp.hpp>
    using boost_mp_int = boost::multiprecision::mpz_int;
#endif

#include <limits>
#include <cstdlib>
#include <cassert>
using namespace std;
using namespace boost::multiprecision;

// Efficient Multiplicative Persistence checker by Hans Loeblich
// This represents large numbers as a collection of digit counts
// for [0:9]
// Multiplying digits is done by taking each digit to the power of its
// given count, and multiplying the resulting powers together.
//
// Powers are calculated and cached up to the max specified lengths
// before doing any checks.  This makes each persistence check
// boil down to some fast lookups of powers, and then a few
// multiplications depending on which digits are present.
//
// Inspired by the NumberPhile video about 277777788888899
// https://www.youtube.com/watch?v=Wim9WJeDTHQ

class digit_bag {
public:
    typedef array<vector<boost_mp_int>, 10> pow_cache_t;
    typedef vector<size_t> rule_prefix_t; // the prefix for a rule
    typedef vector<size_t> rule_tail_digits_t; // list of possible digits for tail
    typedef pair<rule_prefix_t,rule_tail_digits_t> rule_t;
    typedef vector<rule_t> rules_t;
    typedef array<size_t, 10> digits_t;

    digit_bag(size_t length) : size(length), rule_it(rules.begin()), digits(), result_str(length+1,'\0') {
        init_rule(*rule_it);
    }
    digit_bag(digit_bag&&) = delete;
    digit_bag& operator=(digit_bag&&) = delete;
    digit_bag(const digit_bag&) = delete;
    digit_bag& operator=(const digit_bag&) = delete;

    // use a fast lookup table for powers of each digit up to max length
    // ignore 0 and 1
    static void init_cache(size_t max) {
        boost_mp_int power;
        for(size_t i = 2; i < 10; ++i) {
            boost_mp_int base = i;
            for(size_t p = 0; p <= max; ++p) {
                power = pow(base, p);
                cache[i].push_back(power);
            }
        }
    }

    // when iterating to a new rule,
    // initialize digits based on lowest number in the rule
    inline void init_rule(const rule_t& rule) {
        // reset digit counts
        for(auto &d : digits) d = 0;
        // initialize head
        for(const size_t di : rule.first) digits[di]++;
        // set remaining digits to first option in tail_digits
        digits[rule.second[0]] += size - rule.first.size();
    }

    // check every combination of given length
    // starting from digits already in bag
    pair<digits_t, size_t> check_all() {
        digits_t current_max;
        size_t current_max_count = 0;
        do {
            size_t count = persistence();
            if (count > current_max_count) {
                current_max_count = count;
                current_max = digits;
            }
        } while(next());
        return make_pair(current_max,current_max_count);
    }

    // check persitence of current digits
    // assumes input is already length > 1
    size_t persistence() const {
        result = 1;
        for (size_t i=2; i<10; ++i) {
            if (digits[i]) {
                result *= cache[i][digits[i]];
            }
        }
        size_t mult_count = 1;

        while (true) {
            digits_t new_digits{0,0,0,0,0,0,0,0,0,0};
            constexpr const size_t CH_ZERO = '0';
            // convert result to string and count digits of each type
        #if !defined(CPP_INT) && !defined(TOM_INT)
            // Minor optimization (~2%) for GMP string conversion
            mpz_get_str(&result_str[0], 10, result.backend().data());
            if (result_str[1] == '\0') { // single digit result
                return mult_count;
            }
            for(size_t ch : result_str) {
                if (ch == '\0') break;
                size_t di = ch - CH_ZERO; // turn character into numeric digit
                if (di == 0) {
                    return mult_count+1;
                }
                ++new_digits[di];
            }
        #else
            // Generic std::string check
            result_str = result.str();
            if (result_str.size() == 1) { // single digit result
                return mult_count;
            }
            for(size_t ch : result_str) {
                size_t di = ch-'0'; // turn character into numeric digit
                if (di == 0) {
                    return mult_count+1;
                }
                ++new_digits[di];
            }
        #endif
            // we can't check this inside the loop in case there is a zero also,
            // we would return 1 higher than expected
            if (new_digits[5] && (new_digits[2] || new_digits[4] || new_digits[6] || new_digits[8])) {
                return mult_count+2;
            }

            result = 1;
            for (size_t i=2; i<10; ++i) {
                if (new_digits[i]) {
                    result *= cache[i][new_digits[i]];
                }
            }
            ++mult_count;
        }
    }

    // get the next set of digits, in ascending order based on rules
    inline bool next() noexcept {
        const rule_prefix_t &head = rule_it->first;
        const rule_tail_digits_t &tail_digits = rule_it->second;
        const size_t last_digit = tail_digits[tail_digits.size()-1];
        if (digits[last_digit] == size - head.size()) {
            rule_it++;
            if (rule_it == rules.end()) return false;
            init_rule(*rule_it);
            return true;
        } else {
            const size_t tds = tail_digits.size();
            if (tds >= 2) {
                for(size_t ti=tds-2; ti>=0; --ti) {
                    const size_t di = tail_digits[ti];
                    if (digits[di]) {
                        const size_t di2 = tail_digits[ti+1];
                        digits[di]--;
                        digits[di2]++;
                        for(size_t ji2=ti+2; ji2<tds; ++ji2) {
                            const size_t di3 = tail_digits[ji2];
                            digits[di2] += digits[di3];
                            digits[di3] = 0;
                        }
                        return true;
                    }
                }
                //assert(false && "where the digits go?");
            } else {
                rule_it++;
                if (rule_it == rules.end()) return false;
                init_rule(*rule_it);
                return true;
            }
        }
    }

    inline const digits_t& getDigits() const {
        return digits;
    }

private:
    static const rules_t rules;
    static pow_cache_t cache;

    size_t size;
    rules_t::const_iterator rule_it;
    digits_t digits;
    mutable string result_str;
    mutable boost_mp_int result;
};

ostream& operator<<(ostream &out, const digit_bag::digits_t &digits)
{
    out << "{ " << digits[0];
    for (size_t i = 1; i<10; ++i) cout << ", " << digits[i];
    cout << " }";
    return out;
}

// https://oeis.org/A003001
// Summarizing, a term a(n) for n > 2 consists of 7's, 8's and 9's
// with a prefix of one of the following sets of digits:
// {{}, {2}, {3}, {4}, {6}, {2,6}, {3,5}, {5, 5,...}}
// [Amended by Kohei Sakai, May 27 2017]

// Rules are listed to generate numbers in ascending order
const digit_bag::rules_t digit_bag::rules{
    {{2,6},{7,8,9}},
    {{2},  {7,8,9}},
    {{3,5},{7,8,9}},
    {{3},  {7,8,9}},
    {{4},  {7,8,9}},
    {{5},  {5,7,9}},
    {{6},  {7,8,9}},
    {{ },  {7,8,9}}
};
digit_bag::pow_cache_t digit_bag::cache;

int main(int argc, char** argv) {
    if (argc < 3 || argc > 4) {
        cout << "Search for the smallest numbers with the highest multiplicative persistence for base10 numbers with lengths in the range of [START:END)\n";
        cout << "   Or in other words: START < log10(number) < END\n";
        cout << "usage: multper START END [MAX]\n";
        cout << "   START   range lower bound(inclusive).  START must be >= 2\n";
        cout << "   END     range upper bound(exclusive)\n";
        cout << "   MAX     (optional) sets the maximum persistence to treat specially(default=0 regardless of START length)\n";
        cout << "     Any number found with persistence greater than MAX will print a line starting with \"NEW MAX\", with empty lines before/after for visibility.\n";
        cout << "     Otherwise print a single line for each length indicating the best match (smallest number with highest persistence for that length)\n";
        return 1;
    }

    const size_t START = stoul(argv[1]);
    assert(START>=2 && "");
    const size_t END = stoul(argv[2]);
    size_t max_count = 0;
    if (argc == 4) {
        max_count = stoul(argv[3]);
    }

    //cout << "Initializing lookup..."; cout.flush();
    digit_bag::init_cache(END);
    //cout << "Done." << endl;

    for(size_t length=START; length<END; ++length) {
        digit_bag bag(length);
        pair<digit_bag::digits_t, size_t> current_max = bag.check_all();
        if (current_max.second > max_count) {
            max_count = current_max.second;
            cout << endl << "NEW MAX " << max_count << " persistence for " << length << " digits: " << current_max.first << endl << endl;
        } else {
            cout << current_max.second << " best persistence for " << length << " digits: " << current_max.first << endl;
        }
    }
    return 0;
}


#include "laplacian_inpainting.h"

#include <mutex>

#define EIGEN_STACK_ALLOCATION_LIMIT 0
#include <Eigen/Sparse>

#include <tbb/parallel_for.h>
#include <tbb/task_group.h>

namespace possumwood {
namespace opencv {

namespace {

class Triplets;

class Row {
  public:
	Row() = default;

	void addValue(int64_t row, int64_t col, double value) {
		if(value != 0.0)
			m_values[(row << 32) + col] += value;
	}

  private:
	Row(const Row&) = delete;
	Row& operator=(const Row&) = delete;

	std::map<int64_t, double> m_values;

	friend class Triplets;
};

class Triplets {
  public:
	Triplets(int rows, int cols) : m_rowCount(0), m_rows(rows), m_cols(cols) {
	}

	void addRow(const Row& r) {
		for(auto& v : r.m_values) {
			int32_t row = v.first >> 32;
			int32_t col = v.first & 0xffffffff;

			assert(row < m_rows);
			assert(col < m_cols);

			m_triplets.push_back(Eigen::Triplet<double>(m_rowCount, row * m_cols + col, v.second));
		}

		++m_rowCount;
	}

	std::size_t rows() const {
		return m_rowCount;
	}

	const std::vector<Eigen::Triplet<double>>& triplets() const {
		return m_triplets;
	}

  private:
	std::vector<Eigen::Triplet<double>> m_triplets;

	int m_rowCount, m_rows, m_cols;

	friend class Row;
};

static const cv::Mat kernel = (cv::Mat_<double>(3, 3) << 0.0, -1.0, 0.0, -1.0, 4.0, -1.0, 0.0, -1.0, 0.0);

// static const cv::Mat kernel = (cv::Mat_<double>(3,3) <<
// 	-1.0, -1.0, -1.0,
// 	-1.0,  8.0, -1.0,
// 	-1.0, -1.0, -1.0
// );

// static const cv::Mat kernel = (cv::Mat_<double>(3,3) <<
// 	-1.0, -2.0, -1.0,
// 	-2.0, 12.0, -2.0,
// 	-1.0, -2.0, -1.0
// );

// static const cv::Mat kernel = (cv::Mat_<double>(5,5) <<
// 	 0.0,  0.0,  1.0,  0.0,  0.0,
// 	 0.0,  2.0, -8.0,  2.0,  0.0,
// 	 1.0, -8.0, 20.0, -8.0,  1.0,
// 	 0.0,  2.0, -8.0,  2.0,  0.0,
// 	 0.0,  0.0,  1.0,  0.0,  0.0
// );

float buildMatrices(const cv::Mat& image, const cv::Mat& mask, Eigen::SparseMatrix<double>& A, Eigen::VectorXd& b) {
	Triplets triplets(image.rows, image.cols);
	std::vector<double> values;

	std::size_t validCtr = 0, interpolatedCtr = 0;

	for(int y = 0; y < image.rows; ++y)
		for(int x = 0; x < image.cols; ++x) {
			Row row;

			// masked and/or edge
			if(mask.at<unsigned char>(y, x) > 128) {
				values.push_back(0.0f);

				// convolution
				for(int yi = 0; yi < kernel.rows; ++yi)
					for(int xi = 0; xi < kernel.cols; ++xi) {
						int ypos = y + yi - kernel.rows / 2;
						int xpos = x + xi - kernel.cols / 2;

						// handling of edges - "clip" (or "mirror", commented out for now)
						if(ypos < 0)
							// ypos = -ypos;
							ypos = 0;
						if(ypos >= image.rows)
							// ypos = (image.rows-1) - (ypos-image.rows);
							ypos = image.rows - 1;

						if(xpos < 0)
							// xpos = -xpos;
							xpos = 0;
						if(xpos >= image.cols)
							// xpos = (image.cols-1) - (xpos-image.cols);
							xpos = image.cols - 1;

						row.addValue(ypos, xpos, kernel.at<double>(yi, xi));
					}

				++interpolatedCtr;
			}

			// non-masked
			if(mask.at<unsigned char>(y, x) <= 128) {
				values.push_back(image.at<float>(y, x));
				row.addValue(y, x, 1);

				++validCtr;
			}

			triplets.addRow(row);
		}

	// initialise the sparse matrix
	A = Eigen::SparseMatrix<double>(triplets.rows(), image.rows * image.cols);
	A.setFromTriplets(triplets.triplets().begin(), triplets.triplets().end());

	// and the "b" vector
	assert(values.size() == triplets.rows());
	b = Eigen::VectorXd(values.size());
	for(std::size_t i = 0; i < values.size(); ++i)
		b[i] = values[i];

	return (float)validCtr / ((float)validCtr + (float)interpolatedCtr);
}

dependency_graph::State solve(const cv::Mat& input, const cv::Mat& mask, std::vector<float>& output) {
	assert(input.rows == mask.rows);
	assert(input.cols == mask.cols);
	assert(input.type() == CV_32FC1);
	assert(mask.type() == CV_8UC1);

	output = std::vector<float>(input.rows * input.cols, 0.0f);

	Eigen::SparseMatrix<double> A;
	Eigen::VectorXd b, tmp;

	dependency_graph::State state;

	const float ratio = buildMatrices(input, mask, A, b);

	if(ratio > 0.003) {
		const char* stage = "solver construction";

		Eigen::SparseLU<Eigen::SparseMatrix<double> /*, Eigen::NaturalOrdering<int>*/> chol(A);

		if(chol.info() == Eigen::Success) {
			stage = "analyze pattern";

			chol.analyzePattern(A);

			if(chol.info() == Eigen::Success) {
				stage = "factorize";

				chol.factorize(A);

				if(chol.info() == Eigen::Success) {
					stage = "solve";

					tmp = chol.solve(b);

					assert(tmp.size() == input.rows * input.cols);
					for(int i = 0; i < tmp.size(); ++i) {
						const int row = i / input.cols;
						const int col = i % input.cols;
						const int index = row * mask.cols + col;

						assert((std::size_t)index < output.size());
						output[index] = tmp[i];
					}
				}
			}
		}

		if(chol.info() == Eigen::NumericalIssue)
			state.addWarning("Decomposition failed - Eigen::NumericalIssue at stage " + std::string(stage));
		else if(chol.info() == Eigen::NoConvergence)
			state.addWarning("Decomposition failed - Eigen::NoConvergence at stage " + std::string(stage));
		else if(chol.info() == Eigen::InvalidInput)
			state.addWarning("Decomposition failed - Eigen::InvalidInput at stage " + std::string(stage));
		else if(chol.info() != Eigen::Success)
			state.addWarning("Decomposition failed - unknown error at stage " + std::string(stage));
	}

	return state;
}

}  // namespace

dependency_graph::State inpaint(const std::vector<cv::Mat>& inputs,
                                const std::vector<cv::Mat>& masks,
                                std::vector<cv::Mat>& result) {
	if(inputs.size() != masks.size() || inputs.empty() || masks.empty())
		throw std::runtime_error("Laplacian inpainting - number of inputs and masks has to match.");

	for(std::size_t i = 0; i < inputs.size(); ++i) {
		if(inputs[i].depth() != CV_32F)
			throw std::runtime_error("Laplacian inpainting - input image type has to be CV_32F.");
		if(masks[i].depth() != CV_8U || masks[i].type() != masks[0].type())
			throw std::runtime_error("Laplacian inpainting - mask image type has to be CV_8UC1 or CV_8UC3.");
		if(inputs[i].empty() || masks[i].empty())
			throw std::runtime_error("Laplacian inpainting - empty input image and/or mask.");
		if(inputs[i].size != masks[i].size || inputs[i].size != inputs[0].size)
			throw std::runtime_error("Laplacian inpainting - input and mask image size have to match.");
	}

	// split the inputs and masks per channel
	std::vector<cv::Mat> inputs_1ch, masks_1ch;

	for(std::size_t i = 0; i < inputs.size(); ++i) {
		std::vector<cv::Mat> i_tmp, m_tmp;

		cv::split(inputs[i], i_tmp);
		cv::split(masks[i], m_tmp);

		while(m_tmp.size() < i_tmp.size())
			m_tmp.push_back(m_tmp.back());

		inputs_1ch.insert(inputs_1ch.end(), i_tmp.begin(), i_tmp.end());
		masks_1ch.insert(masks_1ch.end(), m_tmp.begin(), m_tmp.end());
	}

	std::vector<std::vector<float>> x(inputs_1ch.size(), std::vector<float>(inputs[0].rows * inputs[0].cols, 0.0f));

	dependency_graph::State state;

	tbb::task_group tasks;
	std::mutex state_mutex;

	for(std::size_t channel = 0; channel < inputs_1ch.size(); ++channel) {
		tasks.run([channel, &inputs_1ch, &masks_1ch, &x, &state, &state_mutex]() {
			const dependency_graph::State currentState = solve(inputs_1ch[channel], masks_1ch[channel], x[channel]);

			std::lock_guard<std::mutex> guard(state_mutex);
			state.append(currentState);
		});
	}

	tasks.wait();

	result = std::vector<cv::Mat>();

	int channel = 0;
	for(auto& in : inputs) {
		cv::Mat tmp = cv::Mat::zeros(in.rows, in.cols, in.type());

		tbb::parallel_for(0, tmp.rows, [&](int yi) {
			for(int xi = 0; xi < tmp.cols; ++xi)
				for(int c = 0; c < in.channels(); ++c)
					tmp.ptr<float>(yi, xi)[c] = x[c + channel][yi * tmp.cols + xi];
		});

		channel += in.channels();
		result.push_back(tmp);
	}

	return state;
}

}  // namespace opencv
}  // namespace possumwood


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

int
main()
{
  using namespace boost;
  GraphvizGraph g_dot;
  read_graphviz("figs/telephone-network.dot", g_dot);

  typedef adjacency_list < vecS, vecS, undirectedS, no_property,
    property < edge_weight_t, int > > Graph;
  Graph g(num_vertices(g_dot));
  property_map < GraphvizGraph, edge_attribute_t >::type
    edge_attr_map = get(edge_attribute, g_dot);
  graph_traits < GraphvizGraph >::edge_iterator ei, ei_end;
  for (tie(ei, ei_end) = edges(g_dot); ei != ei_end; ++ei) {
    int weight = lexical_cast < int >(edge_attr_map[*ei]["label"]);
    property < edge_weight_t, int >edge_property(weight);
    add_edge(source(*ei, g_dot), target(*ei, g_dot), edge_property, g);
  }

  std::vector < graph_traits < Graph >::edge_descriptor > mst;
  kruskal_minimum_spanning_tree(g, std::back_inserter(mst));

  property_map < Graph, edge_weight_t >::type weight = get(edge_weight, g);
  int total_weight = 0;
  for (int e = 0; e < mst.size(); ++e)
    total_weight += get(weight, mst[e]);
  std::cout << "total weight: " << total_weight << std::endl;

  typedef graph_traits < Graph >::vertex_descriptor Vertex;
  for (int i = 0; i < mst.size(); ++i) {
    Vertex u = source(mst[i], g), v = target(mst[i], g);
    edge_attr_map[edge(u, v, g_dot).first]["color"] = "black";
  }
  std::ofstream out("figs/telephone-mst-kruskal.dot");
  graph_property < GraphvizGraph, graph_edge_attribute_t >::type &
    graph_edge_attr_map = get_property(g_dot, graph_edge_attribute);
  graph_edge_attr_map["color"] = "gray";
  graph_edge_attr_map["style"] = "bold";
  write_graphviz(out, g_dot);

  return EXIT_SUCCESS;
}


#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/boost/graph/graph_traits_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>

#include <boost/chrono.hpp>
#include <boost/graph/iteration_macros.hpp>

#include "GraphDefs.h"
#include "timer.h"
#include "GraphUtil.h"
#include "GraphParse.h"
#include "GraphGen.h"

#include <fstream>
#include <string>

#define SHOW_DEBUG false

const int vertexIdx = 0; // Arbitrary index id

void printGraph(const char* title, VertexVector* vertices, EdgeVector* edges, bool printVertices = false) {
	std::cout << std::endl << "=== " << title << std::endl;

	if (printVertices) {
		// Iterate through the vertices and print them out
		for (int i = 0; i < vertices->size(); i++) {
			CGALPoint* v = (*vertices)[i];
			std::cout << "index: " << i << " (" << v->x() << ", " << v->y() << ")" << std::endl;
		}
	}

	// Iterate through the edges and print them out
	std::cout << std::endl << "Edges: " << std::endl;
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* e = (*edges)[i];
		CGALPoint* src = (*vertices)[e->u];
		CGALPoint* tar = (*vertices)[e->v];
		//std::cout << edgeWeightMap->at(e).weight << " (" << (*g)[src].pt << ") (" << (*g)[tar].pt << ")" << std::endl;
		std::cout << e->u << " " << e->v << " (" << (*src) << ") (" << (*tar) << ")" << std::endl;
	}

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

void printGDFGraph(const char* fileName, VertexVector* vertices, EdgeVector* edges) {
	std::ofstream myfile;
	myfile.open(fileName, std::ios::out | std::ios::in);

	myfile << "nodedef> name VARCHAR,label VARCHAR,width DOUBLE,height DOUBLE,x DOUBLE,y DOUBLE,color VARCHAR" << std::endl;

	// Iterate through the vertices and print them out
	boost::unordered_map<CGALPoint*, VertexIndex> vertexHandles;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* v = (*vertices)[i];
		myfile << i << "," << i << ",10.0,10.0," << (*v).x() << "," << (*v).y() << ",'153,153,153'" << std::endl;
		vertexHandles.emplace(v, i);
	}

	myfile << "edgedef> node1,node2,weight DOUBLE,directed BOOLEAN,color VARCHAR" << std::endl;

	// Iterate through the edges and print them out
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* e = (*edges)[i];
		CGALPoint* src = (*vertices)[e->u];
		CGALPoint* tar = (*vertices)[e->v];
		VertexIndex srcInd = vertexHandles[src];
		VertexIndex tarInd = vertexHandles[tar];
		//EdgeWeight weight = CGAL::squared_distance(*src, *tar);
		EdgeWeight weight = 1.0;
		myfile << srcInd << "," << tarInd << "," << weight << ",false,'128,128,128'" << std::endl;
	}

	myfile.close();
}

TriVertexHandle OppositeOfEdge(TriVertexHandle ev0, TriVertexHandle ev1, TriFaceHandle f) {
	TriVertexHandle v0 = f->vertex(0);
	if (v0 != ev0 && v0 != ev1) {
		return v0;
	}

	TriVertexHandle v1 = f->vertex(1);
	if (v1 != ev0 && v1 != ev1) {
		return v1;
	}

	TriVertexHandle v2 = f->vertex(2);
	if (v2 != ev0 && v2 != ev1) {
		return v2;
	}

	assert(false); // Unreachable
	return NULL;
}

bool IsLocallyDelaunay(CGALPoint* p, CGALPoint* q, CGALPoint* r, CGALPoint* t) {
	if (SHOW_DEBUG) {
		//std::cout << "(" << (*p) << ") (" << (*q) << ") (" << (*r) << ") (" << (*t) << ")" << std::endl;
	}
	CGALCircle c(*p, *q, *r);
	CGAL::Bounded_side side = c.bounded_side(*t);

	// We consider ON_BOUNDARY because if a constraint edge is on the boundary and the DT flip of quadrilateral has the same total angle, i.e. both
	// flips are DT, then we have to record the constraint so that the CDT picks the right flip in the subgraph check.
	return side == CGAL::Bounded_side::ON_UNBOUNDED_SIDE;

	// v is outside or on the circumcircle of f
	// return side == CGAL::Bounded_side::ON_UNBOUNDED_SIDE
		//|| side == CGAL::Bounded_side::ON_BOUNDARY;
}

CDT* computeCdt(VertexVector* vertices, EdgeVector* edges, boost::unordered_map<TriVertexHandle, VertexIndex>** handlesToIndex) {
	CDT* cdt = new CDT();

	(*handlesToIndex) = new boost::unordered_map<TriVertexHandle, VertexIndex>();
	boost::unordered_map<VertexIndex, TriVertexHandle> vertexHandles;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* pt = (*vertices)[i];
		TriVertexHandle vHandle = cdt->insert(*pt);
		vertexHandles.emplace(i, vHandle);
		(*handlesToIndex)->emplace(vHandle, i);
	}

	// Insert constraint edges
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* edge = (*edges)[i];
		VertexIndex u = edge->u;
		VertexIndex v = edge->v;
		TriVertexHandle uH = vertexHandles[u];
		TriVertexHandle vH = vertexHandles[v];
		cdt->insert_constraint(uH, vH);
	}

	assert(cdt->is_valid());

	return cdt;
}

// Given 3 colinear points, if the 2 furthest points are constrained, how is this delaunay triangulated?
// It seems the outcome is implementation dependent. CGAL will split the constraint into 2 edges.
// Other implementations might allow for overlapping, collinear edges. Either way, the current plan
// is to assume that an input forest, F, with collinear edges will be replaced by smaller constraint edges.
EdgeVector* newConstraintSetFromCt(CDT* cdt, VertexVector* originalVertices) {
	EdgeVector* newEdgeVector = new EdgeVector();

	boost::unordered_map<CGALPoint, VertexIndex> vertexIndex;
	for (int i = 0; i < originalVertices->size(); i++) {
		vertexIndex[*(*originalVertices)[i]] = i;
	}

	// Add edges to graph
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge cgal_e = *eit;
		CGALSegment segement = cdt->segment(cgal_e);

		/*CGALPoint cgal_u = segement.point(0);
		double uxpos = CGAL::to_double(cgal_u.x());
		double uypos = CGAL::to_double(cgal_u.y());

		CGALPoint cgal_v = segement.point(1);
		double vxpos = CGAL::to_double(cgal_v.x());
		double vypos = CGAL::to_double(cgal_v.y());*/

		if (cdt->is_constrained(cgal_e)) {
			// Assumes point coord are unique
			CGALSegment segement = cdt->segment(cgal_e);
			CGALPoint cgal_u = segement.point(0);
			CGALPoint cgal_v = segement.point(1);
			VertexIndex u = vertexIndex[cgal_u];
			VertexIndex v = vertexIndex[cgal_v];

			SimpleEdge* edge = new SimpleEdge(u, v, 0);
			newEdgeVector->push_back(edge);
		}
	}
	return newEdgeVector;
}

EdgeVector* convertCdtToGraph(VertexVector* vertices, CDT* cdt) {
	EdgeVector* edgeVec = new EdgeVector();

	// Map CGALPoint -> VertexIndex
	boost::unordered_map<CGALPoint, VertexIndex> vertexIndex;
	for (int i = 0; i < vertices->size(); i++) {
		vertexIndex[*(*vertices)[i]] = i;
	}

	// Add edges to graph
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge cgal_e = *eit;
		CGALSegment segement = cdt->segment(cgal_e);
		CGALPoint cgal_u = segement.point(0);
		CGALPoint cgal_v = segement.point(1);
		VertexIndex u = vertexIndex[cgal_u];
		VertexIndex v = vertexIndex[cgal_v];

		SimpleEdge* edge = new SimpleEdge(u, v, 0);
		edgeVec->push_back(edge);
	}

	return edgeVec;
}

void computeNonLocallyDelaunay(
	VertexVector* vertices,
	EdgeVector* edges,
	EdgeVector** NewEdges,
	EdgeVector** S_Edges) {

	boost::chrono::high_resolution_clock::time_point start;
	boost::chrono::high_resolution_clock::time_point end;
	boost::chrono::milliseconds duration(0);
	boost::chrono::milliseconds total(0);

	// Compute CDT(F)
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_map<TriVertexHandle, VertexIndex>* handlesToIndex;
	CDT* cdt = computeCdt(vertices, edges, &handlesToIndex);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("CDT(F)", duration);

	// Replace F with NewF
	start = boost::chrono::high_resolution_clock::now();
	(*NewEdges) = newConstraintSetFromCt(cdt, vertices);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("F -> NewF", duration);

	// Compute Non-Locally Delaunay edges
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_set<TriEdge>* S = new boost::unordered_set<TriEdge>();

	TriVertexHandle infiniteVertex = cdt->infinite_vertex();
	TriFaceHandle infiniteFace = cdt->infinite_face();
	int edgeCount = 0;

	for (FiniteEdgeIter iter = cdt->finite_edges_begin(); iter != cdt->finite_edges_end(); ++iter) {
		edgeCount++;

		// typedef std::pair<Face_handle, int> Edge;
		TriEdge e = *iter;
		int eIndex = e.second;

		// Edge shared by faces f0 and f1
		TriFaceHandle f0 = e.first;
		TriFaceHandle f1 = e.first->neighbor(eIndex);

		// Vertex opposite of edge e in f0
		TriVertexHandle opp0 = f0->vertex(eIndex);

		// Vertex of edge e
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		TriVertexHandle opp1 = OppositeOfEdge(e0, e1, f1);

		if (SHOW_DEBUG) {
			// Vertex endpoint v0, v1 of edge e for f0 (doesn't seem to be possible to identify the same edge for f1 in a similar way)
			TriVertexHandle v0f0 = f0->vertex(f0->cw(eIndex));
			TriVertexHandle v1f0 = f0->vertex(f0->ccw(eIndex));

			//std::cout << eIndex << std::endl;
			std::cout << (*handlesToIndex)[v0f0] << " " << (*handlesToIndex)[v1f0] << " (" << *v0f0 << ") (" << *v1f0 << ") " << (*handlesToIndex)[opp0] << " (" << *opp0 << ")" << std::endl;
		}

		bool addToS = false;

		if (opp0 != infiniteVertex && f1 != infiniteFace) {
			TriVertexHandle pH = f1->vertex(0);
			TriVertexHandle qH = f1->vertex(1);
			TriVertexHandle rH = f1->vertex(2);

			if (pH != infiniteVertex && qH != infiniteVertex && rH != infiniteVertex) {
				CGALPoint p = pH->point();
				CGALPoint q = qH->point();
				CGALPoint r = rH->point();
				CGALPoint t = opp0->point();
				if (!IsLocallyDelaunay(&p, &q, &r, &t)) {
					addToS = true;
				}
			}
		}

		if (opp1 != infiniteVertex && f0 != infiniteFace) {
			TriVertexHandle pH = f0->vertex(0);
			TriVertexHandle qH = f0->vertex(1);
			TriVertexHandle rH = f0->vertex(2);

			if (pH != infiniteVertex && qH != infiniteVertex && rH != infiniteVertex) {
				CGALPoint p = pH->point();
				CGALPoint q = qH->point();
				CGALPoint r = rH->point();
				CGALPoint t = opp1->point();
				if (!IsLocallyDelaunay(&p, &q, &r, &t)) {
					addToS = true;
				}
			}
		}

		if (addToS) {
			S->emplace(e);
		}
	}

	(*S_Edges) = new EdgeVector();
	for (boost::unordered_set<TriEdge>::iterator iter = S->begin(); iter != S->end(); ++iter) {
		TriEdge e = *iter;
		int eIndex = e.second;
		TriFaceHandle f0 = e.first;
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		VertexIndex u = (*handlesToIndex)[e0];
		VertexIndex v = (*handlesToIndex)[e1];

		(*S_Edges)->push_back(new SimpleEdge(u, v, 0));
	}

	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("computeNonLocallyDelaunay duration", duration);

	printDuration("Total", total);

	delete S;
	delete cdt;
	delete handlesToIndex;
}

EdgeVector* intersectInputSetWithConstraintSet(EdgeVector* inputSet, EdgeVector* constraintSet) {
	boost::unordered_set<SimpleEdge>* NewEdges_Hashset = new boost::unordered_set<SimpleEdge>();
	for (int i = 0; i < inputSet->size(); i++) {
		SimpleEdge* e = (*inputSet)[i];
		NewEdges_Hashset->emplace((*e));
	}

	EdgeVector* intersect = new EdgeVector();
	for (int i = 0; i < constraintSet->size(); i++) {
		SimpleEdge* se = (*constraintSet)[i];
		if (NewEdges_Hashset->count(*se) > 0) {
			intersect->push_back(new SimpleEdge(se->u, se->v, se->weight));
		}
	}

	return intersect;
}

bool containsEdge(boost::unordered_set<SimpleEdge>* edgeSet, SimpleEdge* edge) {
	SimpleEdge se(edge->u, edge->v, 0);
	return edgeSet->count(se) > 0;
}

// True if A a subgraph of B
bool isSubgraph(VertexVector* vertices, EdgeVector* a, EdgeVector* b) {
	boost::unordered_set<SimpleEdge>* bEdgeSet = createSimpleEdgeSet(b);

	bool result = true;

	// Iterate through the edges
	for (int i = 0; i < a->size(); i++) {
		SimpleEdge* edge = (*a)[i];
		if (!containsEdge(bEdgeSet, edge)) {
			CGALPoint* u = (*vertices)[edge->u];
			CGALPoint* v = (*vertices)[edge->v];
			//EdgeWeight weight = CGAL::squared_distance(*u, *v);
			CGAL::Lazy_exact_nt<CGAL::Gmpq> exactWeight = CGAL::squared_distance(*u, *v);
			std::cout << "b contains edge not in a: " << CGAL::to_double(exactWeight) << "(" << edge->u << "," << edge->v << ")" << " (" << *u << ") (" << *v << ")" << std::endl;
			result = false;
		}
	}

	return result;
}

bool isCdtSubgraph(VertexVector* vertices, EdgeVector* edgesF, EdgeVector* edgesS) {
	boost::unordered_map<TriVertexHandle, VertexIndex>* handlesToIndex;
	CDT* cdtS = computeCdt(vertices, edgesS, &handlesToIndex); // CDT of mimimum edge constraint
	EdgeVector* ev_cdtS = convertCdtToGraph(vertices, cdtS);

	//sortByXY(ev_cdtS);
	//printGDFGraph("D:\\g\\results\\graph examples\\CDT_exact_DC\\CdtS.gdf", vertices, ev_cdtS);

	bool res = isSubgraph(vertices, edgesF, ev_cdtS);

	delete ev_cdtS;
	delete cdtS;
	delete handlesToIndex;

	return res;
}

int main(int argc, char* argv[]) {
	const char* vertFile = (argc > 2) ? argv[1] : NULL;
	const char* edgeFile = (argc > 2) ? argv[2] : NULL;
	VertexVector* vertices = NULL;
	EdgeVector* edges = NULL;

	if (vertFile == NULL || edgeFile == NULL) {
		// Random graph
		//createRandomCirclePlaneForest(1000, 1000, 100, &vertices, &edges);
		createRandomMediumLengthPlaneForest(1000, 1000, 100, &vertices, &edges);
		//createRandomNearTriangulation(1000, 1000, 100, &vertices, &edges);
	}
	else {
		// Load graph from file
		parseGraph(vertFile, edgeFile, &vertices, &edges);
	}

	if (SHOW_DEBUG) {
		printGraph("Input", vertices, edges, true);
	}

	/*vertices = new VertexVector();
	vertices->push_back(new CGALPoint(0, 0));
	vertices->push_back(new CGALPoint(1, 0));
	vertices->push_back(new CGALPoint(2, 0));

	edges = new EdgeVector();
	edges->push_back(new SimpleEdge(0, 1, 0));
	edges->push_back(new SimpleEdge(1, 2, 0));
	edges->push_back(new SimpleEdge(2, 0, 0));*/

	// Compute CT(V, E)
	EdgeVector* NewEdges;
	EdgeVector* cdtS;
	computeNonLocallyDelaunay(vertices, edges, &NewEdges, &cdtS);
	EdgeVector* S = intersectInputSetWithConstraintSet(NewEdges, cdtS);
	//std::cout << "Edges in E: " << NewEdges->size() << " Edges in S: " << S->size() << " Ratio: " << (double)((double)S->size() / (double)NewEdges->size()) << std::endl;
	std::cout << S->size() << std::endl;
	std::cout << (double)((double)S->size() / (double)NewEdges->size()) << std::endl;
	std::cout << std::endl;

	//printGDFGraph("D:\\g\\results\\graph examples\\CDT_exact_Med\\Edges.gdf", vertices, edges);
	//printGDFGraph("D:\\g\\results\\graph examples\\CDT_exact_Med\\NewEdges.gdf", vertices, NewEdges);
	//printGDFGraph("D:\\g\\results\\graph examples\\CDT_exact_Med\\S.gdf", vertices, S);

	if (SHOW_DEBUG) {
		printGraph("computeNonLocallyDelaunay", vertices, cdtS, false);
		printGraph("S", vertices, S, false);
	}

	// Validatation:
	// S ⊆ E
	if (!isSubgraph(vertices, S, NewEdges)) {
		std::cout << "Error: isSubgraph is false" << std::endl;
	}

	// F ⊆ CDT(V, S)
	if (!isCdtSubgraph(vertices, NewEdges, S)) {
		std::cout << "Error: isCdtSubgraph is false" << std::endl;
	}

	deleteEdgeVector(S);
	deleteEdgeVector(cdtS);
	deleteEdgeVector(NewEdges);
	deleteEdgeVector(edges);
	deleteVerticesVector(vertices);

	return 0;
}


//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
// Copyright 2021 Pranam Lashkari <plashkari628@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_GIL_IMAGE_PROCESSING_DIFFUSION_HPP
#define BOOST_GIL_IMAGE_PROCESSING_DIFFUSION_HPP

#include "boost/gil/detail/math.hpp"
#include <boost/gil/algorithm.hpp>
#include <boost/gil/color_base_algorithm.hpp>
#include <boost/gil/image.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/image_view_factory.hpp>
#include <boost/gil/pixel.hpp>
#include <boost/gil/point.hpp>
#include <boost/gil/typedefs.hpp>
#include <functional>
#include <numeric>
#include <vector>

namespace boost { namespace gil {
namespace conductivity {
struct perona_malik_conductivity
{
    double kappa;
    template <typename Pixel>
    Pixel operator()(Pixel input)
    {
        using channel_type = typename channel_type<Pixel>::type;
        // C++11 doesn't seem to capture members
        static_transform(input, input, [this](channel_type value) {
            value /= kappa;
            return std::exp(-std::abs(value));
        });

        return input;
    }
};

struct gaussian_conductivity
{
    double kappa;
    template <typename Pixel>
    Pixel operator()(Pixel input)
    {
        using channel_type = typename channel_type<Pixel>::type;
        // C++11 doesn't seem to capture members
        static_transform(input, input, [this](channel_type value) {
            value /= kappa;
            return std::exp(-value * value);
        });

        return input;
    }
};

struct wide_regions_conductivity
{
    double kappa;
    template <typename Pixel>
    Pixel operator()(Pixel input)
    {
        using channel_type = typename channel_type<Pixel>::type;
        // C++11 doesn't seem to capture members
        static_transform(input, input, [this](channel_type value) {
            value /= kappa;
            return 1.0 / (1.0 + value * value);
        });

        return input;
    }
};

struct more_wide_regions_conductivity
{
    double kappa;
    template <typename Pixel>
    Pixel operator()(Pixel input)
    {
        using channel_type = typename channel_type<Pixel>::type;
        // C++11 doesn't seem to capture members
        static_transform(input, input, [this](channel_type value) {
            value /= kappa;
            return 1.0 / std::sqrt((1.0 + value * value));
        });

        return input;
    }
};
} // namespace diffusion

/**
    \brief contains discrete approximations of 2D Laplacian operator
*/
namespace laplace_function {
// The functions assume clockwise enumeration of stencil points, as such
// NW   North NE          0 1 2      (-1, -1) (0, -1) (+1, -1)
// West       East   ===> 7   3 ===> (-1, 0)          (+1, 0)
// SW   South SE          6 5 4      (-1, +1) (0, +1) (+1, +1)

/**
    \brief This function makes sure all Laplace functions enumerate
    values in the same order and direction.

    The first element is difference North West direction, second in North,
    and so on in clockwise manner. Leave element as zero if it is not
    to be computed.
*/
inline std::array<gil::point_t, 8> get_directed_offsets()
{
    return {point_t{-1, -1}, point_t{0, -1}, point_t{+1, -1}, point_t{+1, 0},
            point_t{+1, +1}, point_t{0, +1}, point_t{-1, +1}, point_t{-1, 0}};
}

template <typename PixelType>
using stencil_type = std::array<PixelType, 8>;

/**
    \brief 5 point stencil approximation of Laplacian

    Only main 4 directions are non-zero, the rest are zero
*/
struct stencil_5points
{
    double delta_t = 0.25;

    template <typename SubImageView>
    stencil_type<typename SubImageView::value_type> compute_laplace(SubImageView view,
                                                                    point_t origin)
    {
        auto current = view(origin);
        stencil_type<typename SubImageView::value_type> stencil;
        using channel_type = typename channel_type<typename SubImageView::value_type>::type;
        std::array<gil::point_t, 8> offsets(get_directed_offsets());
        typename SubImageView::value_type zero_pixel;
        static_fill(zero_pixel, 0);
        for (std::size_t index = 0; index < offsets.size(); ++index)
        {
            if (index % 2 != 0)
            {
                static_transform(view(origin.x + offsets[index].x, origin.y + offsets[index].y),
                                 current, stencil[index], std::minus<channel_type>{});
            }
            else
            {
                stencil[index] = zero_pixel;
            }
        }
        return stencil;
    }

    template <typename Pixel>
    Pixel reduce(const stencil_type<Pixel>& stencil)
    {
        auto first = stencil.begin();
        auto last = stencil.end();
        using channel_type = typename channel_type<Pixel>::type;
        auto result = []() {
            Pixel zero_pixel;
            static_fill(zero_pixel, channel_type(0));
            return zero_pixel;
        }();

        for (std::size_t index : {1u, 3u, 5u, 7u})
        {
            static_transform(result, stencil[index], result, std::plus<channel_type>{});
        }
        Pixel delta_t_pixel;
        static_fill(delta_t_pixel, delta_t);
        static_transform(result, delta_t_pixel, result, std::multiplies<channel_type>{});

        return result;
    }
};

/**
    \brief 9 point stencil approximation of Laplacian

    This is full 8 way approximation, though diagonal
    elements are halved during reduction.
*/
struct stencil_9points_standard
{
    double delta_t = 0.125;

    template <typename SubImageView>
    stencil_type<typename SubImageView::value_type> compute_laplace(SubImageView view,
                                                                    point_t origin)
    {
        stencil_type<typename SubImageView::value_type> stencil;
        auto out = stencil.begin();
        auto current = view(origin);
        using channel_type = typename channel_type<typename SubImageView::value_type>::type;
        std::array<gil::point_t, 8> offsets(get_directed_offsets());
        for (auto offset : offsets)
        {
            static_transform(view(origin.x + offset.x, origin.y + offset.y), current, *out++,
                             std::minus<channel_type>{});
        }

        return stencil;
    }

    template <typename Pixel>
    Pixel reduce(const stencil_type<Pixel>& stencil)
    {
        using channel_type = typename channel_type<Pixel>::type;
        auto result = []() {
            Pixel zero_pixel;
            static_fill(zero_pixel, channel_type(0));
            return zero_pixel;
        }();
        for (std::size_t index : {1u, 3u, 5u, 7u})
        {
            static_transform(result, stencil[index], result, std::plus<channel_type>{});
        }

        for (std::size_t index : {0u, 2u, 4u, 6u})
        {
            Pixel half_pixel;
            static_fill(half_pixel, channel_type(1 / 2.0));
            static_transform(stencil[index], half_pixel, half_pixel,
                             std::multiplies<channel_type>{});
            static_transform(result, half_pixel, result, std::plus<channel_type>{});
        }

        Pixel delta_t_pixel;
        static_fill(delta_t_pixel, delta_t);
        static_transform(result, delta_t_pixel, result, std::multiplies<channel_type>{});

        return result;
    }
};
} // namespace laplace_function

namespace brightness_function {
using laplace_function::stencil_type;
struct identity
{
    template <typename Pixel>
    stencil_type<Pixel> operator()(const stencil_type<Pixel>& stencil)
    {
        return stencil;
    }
};

// TODO: Figure out how to implement color gradient brightness, as it
// seems to need dx and dy using sobel or scharr kernels

struct rgb_luminance
{
    using pixel_type = rgb32f_pixel_t;
    stencil_type<pixel_type> operator()(const stencil_type<pixel_type>& stencil)
    {
        stencil_type<pixel_type> output;
        std::transform(stencil.begin(), stencil.end(), output.begin(), [](const pixel_type& pixel) {
            float32_t luminance = 0.2126f * pixel[0] + 0.7152f * pixel[1] + 0.0722f * pixel[2];
            pixel_type result_pixel;
            static_fill(result_pixel, luminance);
            return result_pixel;
        });
        return output;
    }
};

} // namespace brightness_function

enum class matlab_connectivity
{
    minimal,
    maximal
};

enum class matlab_conduction_method
{
    exponential,
    quadratic
};

template <typename InputView, typename OutputView>
void classic_anisotropic_diffusion(const InputView& input, const OutputView& output,
                                   unsigned int num_iter, double kappa)
{
    anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_5points{},
                          brightness_function::identity{},
                          conductivity::perona_malik_conductivity{kappa});
}

template <typename InputView, typename OutputView>
void matlab_anisotropic_diffusion(const InputView& input, const OutputView& output,
                                  unsigned int num_iter, double kappa,
                                  matlab_connectivity connectivity,
                                  matlab_conduction_method conduction_method)
{
    if (connectivity == matlab_connectivity::minimal)
    {
        if (conduction_method == matlab_conduction_method::exponential)
        {
            anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_5points{},
                                  brightness_function::identity{},
                                  conductivity::gaussian_conductivity{kappa});
        }
        else if (conduction_method == matlab_conduction_method::quadratic)
        {
            anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_5points{},
                                  brightness_function::identity{},
                                  conductivity::gaussian_conductivity{kappa});
        }
        else
        {
            throw std::logic_error("unhandled conduction method found");
        }
    }
    else if (connectivity == matlab_connectivity::maximal)
    {
        if (conduction_method == matlab_conduction_method::exponential)
        {
            anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_5points{},
                                  brightness_function::identity{},
                                  conductivity::gaussian_conductivity{kappa});
        }
        else if (conduction_method == matlab_conduction_method::quadratic)
        {
            anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_5points{},
                                  brightness_function::identity{},
                                  conductivity::gaussian_conductivity{kappa});
        }
        else
        {
            throw std::logic_error("unhandled conduction method found");
        }
    }
    else
    {
        throw std::logic_error("unhandled connectivity found");
    }
}

template <typename InputView, typename OutputView>
void default_anisotropic_diffusion(const InputView& input, const OutputView& output,
                                   unsigned int num_iter, double kappa)
{
    anisotropic_diffusion(input, output, num_iter, laplace_function::stencil_9points_standard{},
                          brightness_function::identity{}, conductivity::gaussian_conductivity{kappa});
}

/// \brief Performs diffusion according to Perona-Malik equation
///
/// WARNING: Output channel type must be floating point,
/// otherwise there will be loss in accuracy which most
/// probably will lead to incorrect results (input will be unchanged).
/// Anisotropic diffusion is a smoothing algorithm that respects
/// edge boundaries and can work as an edge detector if suitable
/// iteration count is set and grayscale image view is used
/// as an input
template <typename InputView, typename OutputView,
          typename LaplaceStrategy = laplace_function::stencil_9points_standard,
          typename BrightnessFunction = brightness_function::identity,
          typename DiffusivityFunction = conductivity::gaussian_conductivity>
void anisotropic_diffusion(const InputView& input, const OutputView& output, unsigned int num_iter,
                           LaplaceStrategy laplace, BrightnessFunction brightness,
                           DiffusivityFunction diffusivity)
{
    using input_pixel_type = typename InputView::value_type;
    using pixel_type = typename OutputView::value_type;
    using channel_type = typename channel_type<pixel_type>::type;
    using computation_image = image<pixel_type>;
    const auto width = input.width();
    const auto height = input.height();
    const point_t dims(width, height);
    const auto zero_pixel = []() {
        pixel_type pixel;
        static_fill(pixel, static_cast<channel_type>(0));

        return pixel;
    }();
    computation_image result_image(width + 2, height + 2, zero_pixel);
    auto result = view(result_image);
    computation_image scratch_result_image(width + 2, height + 2, zero_pixel);
    auto scratch_result = view(scratch_result_image);
    transform_pixels(input, subimage_view(result, 1, 1, width, height),
                     [](const input_pixel_type& pixel) {
                         pixel_type converted;
                         for (std::size_t i = 0; i < num_channels<pixel_type>{}; ++i)
                         {
                             converted[i] = pixel[i];
                         }
                         return converted;
                     });

    for (unsigned int iteration = 0; iteration < num_iter; ++iteration)
    {
        for (std::ptrdiff_t relative_y = 0; relative_y < height; ++relative_y)
        {
            for (std::ptrdiff_t relative_x = 0; relative_x < width; ++relative_x)
            {
                auto x = relative_x + 1;
                auto y = relative_y + 1;
                auto stencil = laplace.compute_laplace(result, point_t(x, y));
                auto brightness_stencil = brightness(stencil);
                laplace_function::stencil_type<pixel_type> diffusivity_stencil;
                std::transform(brightness_stencil.begin(), brightness_stencil.end(),
                               diffusivity_stencil.begin(), diffusivity);
                laplace_function::stencil_type<pixel_type> product_stencil;
                std::transform(stencil.begin(), stencil.end(), diffusivity_stencil.begin(),
                               product_stencil.begin(), [](pixel_type lhs, pixel_type rhs) {
                                   static_transform(lhs, rhs, lhs, std::multiplies<channel_type>{});
                                   return lhs;
                               });
                static_transform(result(x, y), laplace.reduce(product_stencil),
                                 scratch_result(x, y), std::plus<channel_type>{});
            }
        }
        using std::swap;
        swap(result, scratch_result);
    }

    copy_pixels(subimage_view(result, 1, 1, width, height), output);
}

}} // namespace boost::gil

#endif


/**
 * @file libfrcima.hpp
 * @author Jorge Agüero Zamora
 * @brief Contains the RCIMA and fast-RCIMA methods
 * @version 0.1
 * @date 2021-06-14
 *
 * @copyright Copyright (c) 2021
 *
 */
#ifndef LIBFRCIMA_H
#define LIBFRCIMA_H

#define LOW_RANK_APPROX_ITERATIONS 3 //!< amount of iteration to approximate low rank matrix

#include <armadillo>
#include <vector>
#include <algorithm>

namespace frcima
{
  using arma::mat;
  using std::vector;

  extern double _calculated_error; //!< global variable to hold calculated error

  /**
 * @brief calculates rank constrained inverse matrix approximation using RCIMA method
 *
 * @param t_data_x the original source images. The training data is a vector of vectors
 *                  containing a vectorized form of an image
 * @param t_data_c the images with a noise. The training data is a vector of vectors
 *                  containing a vectorized form of an image
 * @param rank desired rank constrain, should be less than the size of training data
 * @return mat rank constrained inverse matrix approximation
 */
  mat rcima(const vector<vector<double>> &t_data_x, const vector<vector<double>> &t_data_c, const size_t rank);

  /**
 * @brief calculates rank constrained inverse matrix approximation using RCIMA method
 *
 * @param t_data_x the original source images. The training data is an arma::mat
 *                  where each column is a vectorized form of a training image
 * @param t_data_c the images with a noise applied. The training data is an arma::mat
 *                  where each column is a vectorized form of a training image
 * @param rank desired rank constrain, should be less than the size of training data
 * @return mat rank constrained inverse matrix approximation
 */
  mat rcima(const mat &X, const mat &C, const size_t rank);

  /**
 * @brief calculates rank constrained inverse matrix approximation using fast-RCIMA algorithm
 *
 * @param t_data_x the original source images. The training data is a vector of vectors
 *                  containing a vectorized form of an image
 * @param t_data_c the images with noise. The training data is a vector of vectors
 *                  containing a vectorized form of an image
 * @param rank desired rank approximation, should be less than the size of training data
 * @return mat rank constrained inverse matrix approximation
 */
  mat fast_rcima(const vector<vector<double>> &t_data_x, const vector<vector<double>> &t_data_c, const size_t rank);

  /**
 * @brief calculates rank constrained inverse matrix approximation using fast-RCIMA algorithm
 *
 * @param t_data_x the original source images. The training data is an arma::mat
 *                  where each column is a vectorized form of a training image
 * @param t_data_c the images with noise. The training data is an arma::mat
 *                  where each column is a vectorized form of a training image
 * @param rank desired rank approximation, should be less than the size of training data
 * @return mat rank constrained inverse matrix approximation
 */
  mat fast_rcima(const mat &X, const mat &C, const size_t rank);

}

#endif


#include <NTL/mat_poly_lzz_p.h>

NTL_START_IMPL

void CharPoly(zz_pX& f, const mat_zz_p& M)
{
   long n = M.NumRows();
   if (M.NumCols() != n)
      LogicError("CharPoly: nonsquare matrix");

   if (n == 0) {
      set(f);
      return;
   }

   zz_p t;

   if (n == 1) {
      SetX(f);
      negate(t, M(1, 1));
      SetCoeff(f, 0, t);
      return;
   }

   mat_zz_p H;

   H = M;

   long i, j, m;
   zz_p u, t1;

   for (m = 2; m <= n-1; m++) {
      i = m;
      while (i <= n && IsZero(H(i, m-1)))
         i++;

      if (i <= n) {
         t = H(i, m-1);
         if (i > m) {
            swap(H(i), H(m));
            // swap columns i and m
            for (j = 1; j <= n; j++)
               swap(H(j, i), H(j, m));
         }

         for (i = m+1; i <= n; i++) {
            div(u, H(i, m-1), t);
            for (j = m; j <= n; j++) {
               mul(t1, u, H(m, j));
               sub(H(i, j), H(i, j), t1);
            }

            for (j = 1; j <= n; j++) {
               mul(t1, u, H(j, i));
               add(H(j, m), H(j, m), t1);
            }
         }
      }
   }

   vec_zz_pX F;
   F.SetLength(n+1);
   zz_pX T;
   T.SetMaxLength(n);

   set(F[0]);
   for (m = 1; m <= n; m++) {
      LeftShift(F[m], F[m-1], 1);
      mul(T, F[m-1], H(m, m));
      sub(F[m], F[m], T);

      set(t);
      for (i = 1; i <= m-1; i++) {
         mul(t, t, H(m-i+1, m-i));
         mul(t1, t, H(m-i, m));
         mul(T, F[m-i-1], t1);
         sub(F[m], F[m], T);
      }
   }

   f = F[n];
}

NTL_END_IMPL


/**
 * \copyright
 * Copyright (c) 2015, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "fem_ele_std.h"

#include <Eigen/Eigen>

#include "rf_mmp_new.h"
#include "rf_pcs.h"

namespace FiniteElement
{

void CFiniteElementStd::AssembleTHResidual()
{
	const unsigned c_dim = dim;
	const int c_nnodes = nnodes;
	const double dt = pcs->Tim->time_step_length;
	const double theta = pcs->m_num->ls_theta;
	const bool hasGravity =
		(coordinate_system) % 10 == 2 && FluidProp->CheckGravityCalculation();
	const double g_const = hasGravity ? gravity_constant : .0;
	const bool useSUPG = (pcs->m_num->ele_supg_method > 0);
	const bool useLumpedMass = (pcs->m_num->ele_mass_lumping > 0);
	const bool isTransient =
		(this->pcs->tim_type == FiniteElement::TIM_TRANSIENT);
	const bool isMatrixFlowInactive = pcs->deactivateMatrixFlow;
	const bool isMatrixElement = (MeshElement->GetDimension() == pcs->m_msh->GetMaxElementDim());

	Matrix t_transform_tensor(3, 3);
	if (dim > MediaProp->geo_dimension)
	{
		if (MeshElement->getTransformTensor() == NULL)
		{
			std::cout << "***Error: Geometric dimension in MMP is not "
						 "consistent with element."
					  << "\n";
			exit(0);
		}
		t_transform_tensor.LimitSize(dim, dim);
		MeshElement->getTransformTensor()->GetTranspose(t_transform_tensor);
	}

	Eigen::VectorXd nodal_p0(nnodes), nodal_p1(nnodes);
	Eigen::VectorXd nodal_T0(nnodes), nodal_T1(nnodes);
	for (int i = 0; i < nnodes; i++)
	{
		nodal_p0[i] = pcs->GetNodeValue(nodes[i], idxp0);
		nodal_p1[i] = pcs->GetNodeValue(nodes[i], idxp1);
		nodal_T0[i] = pcs->GetNodeValue(nodes[i], idxT0);
		nodal_T1[i] = pcs->GetNodeValue(nodes[i], idxT1);
	}

	// Calculate matrices
	const int offset_p = 0;
	const int offset_T = c_nnodes;
	(*RHS) = 0.0;          // Residual

//#define TH_DEBUG
#ifdef TH_DEBUG
	// for debugging
	Mass->LimitSize(nnodes, nnodes);
	Laplace->LimitSize(nnodes, nnodes);
	Advection->LimitSize(nnodes, nnodes);
	(*Mass) = .0;
	(*Laplace) = .0;
	(*Advection) = .0;
#endif
	double* tmp_k_tensor = MediaProp->PermeabilityTensor(Index);
	if (c_dim > MediaProp->geo_dimension)
	{
		Matrix local_tensor(c_dim, c_dim), temp_tensor(c_dim, c_dim),
			global_tensor(c_dim, c_dim);
		const unsigned c_ele_dim = ele_dim;
		for (size_t i = 0; i < c_ele_dim; i++)
			for (size_t j = 0; j < c_ele_dim; j++)
				local_tensor(i, j) = tmp_k_tensor[j + i * c_ele_dim];
		// cout << "K':" << endl; local_tensor.Write();
		local_tensor.multi(t_transform_tensor, temp_tensor);
		for (size_t i = 0; i < c_dim; i++)
			for (size_t j = 0; j < c_dim; j++)
				for (size_t k = 0; k < c_dim; k++)
					global_tensor(i, j) +=
						(*MeshElement->getTransformTensor())(i, k) *
						temp_tensor(k, j);
		// cout << "K:" << endl; global_tensor.Write();
		for (size_t i = 0; i < c_dim; i++)
			for (size_t j = 0; j < c_dim; j++)
				tmp_k_tensor[c_dim * i + j] = global_tensor(i, j);
	}
	double const* const k_tensor = tmp_k_tensor;
	const double Ss =
		MediaProp->StorageFunction(Index, unit, theta);
	double dummy[3] = {};
	double const* const lambda_tensor =
		MediaProp->HeatDispersionTensorNew(0, dummy);
	Eigen::MatrixXd k(c_dim, c_dim);
	for (unsigned i=0; i<c_dim; i++)
		for (unsigned j=0; j<c_dim; j++)
			k(i,j) = k_tensor[i*c_dim + j];
	Eigen::MatrixXd lambda(c_dim, c_dim);
	for (unsigned i=0; i<c_dim; i++)
		for (unsigned j=0; j<c_dim; j++)
			lambda(i,j) = lambda_tensor[i*c_dim + j];
	Eigen::Vector3d vec_g;
	vec_g << 0, 0, -g_const;

	//======================================================================
	Eigen::VectorXd r_p(c_nnodes), r_T(c_nnodes);
	r_p.setZero();
	r_T.setZero();

	//======================================================================
	// Mass lumping
	if (isTransient && useLumpedMass)
	{
		const double vol =
			MeshElement->GetVolume() * MeshElement->GetFluxArea();
		// Center of the reference element
		SetCenterGP();
		ComputeShapefct(1);
		double const* const c_shapefct = shapefct;
		Eigen::RowVectorXd N(c_nnodes);
		N.setZero();
		for (int i=0; i<c_nnodes; i++)
			N(i) = c_shapefct[i];
		const double gp_p1 = N * nodal_p1;
		const double gp_T1 = N * nodal_T1;
		double var[3] = {};
		var[0] = gp_p1;
		var[1] = gp_T1;
		const double rhocp = MediaProp->HeatCapacity(Index, theta, this, var);
		const double fkt = vol / (double)nnodes;
		r_T += rhocp * (nodal_T1 - nodal_T0)/dt * fkt;
	}

	//======================================================================
	// Loop over Gauss points
	int gp_r, gp_s, gp_t;
	double var[3] = {};
	Eigen::RowVectorXd N(c_nnodes);
	Eigen::RowVectorXd W_T(c_nnodes), W_SUPG(c_nnodes);
	Eigen::MatrixXd dN(c_dim, c_nnodes);
	for (gp = 0; gp < nGaussPoints; gp++)
	{
		//---------------------------------------------------------
		//  Get local coordinates and weights
		//  Compute Jacobian matrix and its determinate
		//---------------------------------------------------------
		const double fkt = GetGaussData(gp, gp_r, gp_s, gp_t);
		// Compute geometry
		ComputeShapefct(1);
		ComputeGradShapefct(1);
		double const* const c_shapefct = shapefct;
		double const* const c_dshapefct = dshapefct;
		N.setZero();
		dN.setZero();
		for (int i=0; i<c_nnodes; i++)
			N(i) = c_shapefct[i];
		for (unsigned i = 0; i < c_dim; i++)
			for (int j = 0; j < c_nnodes; j++)
				dN(i,j) = c_dshapefct[i * c_nnodes + j];
		W_T = N;

		//---------------------------------------------------------
		//  Get state variables
		//---------------------------------------------------------
		const double gp_p0 = N * nodal_p0;
		const double gp_T0 = N * nodal_T0;
		const double gp_p1 = N * nodal_p1;
		const double gp_T1 = N * nodal_T1;
		Eigen::VectorXd grad_p1 = dN * nodal_p1;
		Eigen::VectorXd grad_T1 = dN * nodal_T1;

		//---------------------------------------------------------
		//  Get material properties
		//---------------------------------------------------------
		var[0] = gp_p1;
		var[1] = gp_T1;
		// Fluid properties
		const double rho_w = FluidProp->Density(var);
		const double vis = FluidProp->Viscosity(var);
		const double cp_w = FluidProp->SpecificHeatCapacity(var);
		// Medium properties
		const double rhocp = MediaProp->HeatCapacity(Index, theta, this, var);

		//---------------------------------------------------------
		//  Set velocity
		//---------------------------------------------------------
		Eigen::VectorXd grad_h1 = grad_p1;
		if (hasGravity)
			grad_h1 -= rho_w * vec_g;
		Eigen::VectorXd q = - k / vis * grad_h1;

		//---------------------------------------------------------
		//  SUPG coefficients
		//---------------------------------------------------------
		if (useSUPG)
		{
			W_SUPG.setZero();
			double supg_tau = 0;
			CalcSUPGWeightingFunction(&q[0], gp, supg_tau, &W_SUPG[0]);
			W_T += supg_tau * W_SUPG;
		}

		//---------------------------------------------------------
		//  Assemble Liquid flow equation
		//  original: N^T*S*N*dp/dt + dN^T*k/mu*dN*p+dN^T*rho*g*z = 0
		//---------------------------------------------------------
		// Rp += [1/dt*N^T*Ss*N + theta*dN^T*k/mu*dN]*p1 -
		// [1/dt*N^T*Ss*N - (1-theta)*dN^T*k/mu*dN]*p0 +
		// dN^T*k/mu*rho*g*z
		if (isTransient)
		{
			// 1/dt*N^T*Ss*(p1-p0)
			r_p.noalias() += N.transpose() * Ss * (gp_p1 - gp_p0)/dt * fkt;
		}
		// - dN^T*vel
		r_p.noalias() += - fkt * dN.transpose() * q;

		//---------------------------------------------------------
		//  Assemble Heat transport equation
		//---------------------------------------------------------

		// Rt += [1/dt*N^T*Cp*N + theta*(dN^T*lambda*dN+N^T*Cp_w*dN)]*T1 -
		// [1/dt*N^T*Cp*N - (1-theta)*(dN^T*lambda*dN+N^T*Cp_w*dN)]*T0
		if (isTransient && !useLumpedMass)
		{
			r_T.noalias() += W_T.transpose() * fkt * rhocp * (gp_T1 - gp_T0)/dt;
		}
		r_T.noalias() += fkt * dN.transpose() * lambda * grad_T1;
		r_T.noalias() += fkt * W_T.transpose() * rho_w * cp_w * q.transpose() * grad_T1;
	}

	if (isMatrixElement && isMatrixFlowInactive)
		r_p.setZero();

	for (int i = 0; i < c_nnodes; i++)
	{
		(*RHS)(offset_p + i) = r_p[i];
		(*RHS)(offset_T + i) = r_T[i];
	}

	if (pcs->scaleEQS)
	{
		for (int ii = 0; ii < 2; ii++)
		{
			const double scale_eqs = pcs->vec_scale_eqs[ii];
			for (int i = 0; i < c_nnodes; i++)
				(*RHS)(ii* c_nnodes + i) *= scale_eqs;
		}
	}

	// RHS should be - residual
	(*RHS) *= -1.;

#ifdef NEW_EQS
	for (size_t ii = 0; ii < pcs->GetPrimaryVNumber(); ii++)
		for (long i = 0; i < c_nnodes; i++)
			eqs_rhs[NodeShift[ii] + eqs_number[i]] += (*RHS)(i + ii * c_nnodes);
#endif
}

void CFiniteElementStd::AssembleTHJacobian()
{
	const unsigned c_dim = dim;
	const int c_nnodes = nnodes;
	const double dt = pcs->Tim->time_step_length;
	const double theta = pcs->m_num->ls_theta;
	const bool hasGravity =
		(coordinate_system) % 10 == 2 && FluidProp->CheckGravityCalculation();
	const double g_const = hasGravity ? gravity_constant : .0;
	const bool useSUPG = (pcs->m_num->ele_supg_method > 0);
	const bool useLumpedMass = (pcs->m_num->ele_mass_lumping > 0);
	const bool isTransient =
		(this->pcs->tim_type == FiniteElement::TIM_TRANSIENT);
	const bool isMatrixFlowInactive = pcs->deactivateMatrixFlow;
	const bool isMatrixElement = (MeshElement->GetDimension() == pcs->m_msh->GetMaxElementDim());

	Matrix t_transform_tensor(3, 3);
	if (dim > MediaProp->geo_dimension)
	{
		if (MeshElement->getTransformTensor() == NULL)
		{
			std::cout << "***Error: Geometric dimension in MMP is not "
						 "consistent with element."
					  << "\n";
			exit(0);
		}
		t_transform_tensor.LimitSize(dim, dim);
		MeshElement->getTransformTensor()->GetTranspose(t_transform_tensor);
	}

	Eigen::VectorXd nodal_p0(nnodes), nodal_p1(nnodes);
	Eigen::VectorXd nodal_T0(nnodes), nodal_T1(nnodes);
	for (int i = 0; i < nnodes; i++)
	{
		nodal_p0[i] = pcs->GetNodeValue(nodes[i], idxp0);
		nodal_p1[i] = pcs->GetNodeValue(nodes[i], idxp1);
		nodal_T0[i] = pcs->GetNodeValue(nodes[i], idxT0);
		nodal_T1[i] = pcs->GetNodeValue(nodes[i], idxT1);
	}

	// Calculate matrices
	const int offset_p = 0;
	const int offset_T = c_nnodes;
	(*StiffMatrix) = 0.0;  // Jacobian

//#define TH_DEBUG
#ifdef TH_DEBUG
	// for debugging
	Mass->LimitSize(nnodes, nnodes);
	Laplace->LimitSize(nnodes, nnodes);
	Advection->LimitSize(nnodes, nnodes);
	(*Mass) = .0;
	(*Laplace) = .0;
	(*Advection) = .0;
#endif
	double* tmp_k_tensor = MediaProp->PermeabilityTensor(Index);
	if (c_dim > MediaProp->geo_dimension)
	{
		Matrix local_tensor(c_dim, c_dim), temp_tensor(c_dim, c_dim),
			global_tensor(c_dim, c_dim);
		const unsigned c_ele_dim = ele_dim;
		for (size_t i = 0; i < c_ele_dim; i++)
			for (size_t j = 0; j < c_ele_dim; j++)
				local_tensor(i, j) = tmp_k_tensor[j + i * c_ele_dim];
		// cout << "K':" << endl; local_tensor.Write();
		local_tensor.multi(t_transform_tensor, temp_tensor);
		for (size_t i = 0; i < c_dim; i++)
			for (size_t j = 0; j < c_dim; j++)
				for (size_t k = 0; k < c_dim; k++)
					global_tensor(i, j) +=
						(*MeshElement->getTransformTensor())(i, k) *
						temp_tensor(k, j);
		// cout << "K:" << endl; global_tensor.Write();
		for (size_t i = 0; i < c_dim; i++)
			for (size_t j = 0; j < c_dim; j++)
				tmp_k_tensor[c_dim * i + j] = global_tensor(i, j);
	}
	double const* const k_tensor = tmp_k_tensor;
	const double Ss = MediaProp->StorageFunction(Index, unit, theta);
	double dummy[3] = {};
	double const* const lambda_tensor =
		MediaProp->HeatDispersionTensorNew(0, dummy);
	Eigen::MatrixXd k(c_dim, c_dim);
	for (unsigned i=0; i<c_dim; i++)
		for (unsigned j=0; j<c_dim; j++)
			k(i,j) = k_tensor[i*c_dim + j];
	Eigen::MatrixXd lambda(c_dim, c_dim);
	for (unsigned i=0; i<c_dim; i++)
		for (unsigned j=0; j<c_dim; j++)
			lambda(i,j) = lambda_tensor[i*c_dim + j];
	Eigen::Vector3d vec_g;
	vec_g << 0, 0, -g_const;

	//======================================================================
	Eigen::MatrixXd J_pp(c_nnodes, c_nnodes), J_pT(c_nnodes, c_nnodes);
	Eigen::MatrixXd J_Tp(c_nnodes, c_nnodes), J_TT(c_nnodes, c_nnodes);
	J_pp.setZero();
	J_pT.setZero();
	J_Tp.setZero();
	J_TT.setZero();

	//======================================================================
	// Mass lumping
	if (isTransient && useLumpedMass)
	{
		const double vol =
			MeshElement->GetVolume() * MeshElement->GetFluxArea();
		// Center of the reference element
		SetCenterGP();
		ComputeShapefct(1);
		double const* const c_shapefct = shapefct;
		Eigen::RowVectorXd N(c_nnodes);
		N.setZero();
		for (int i=0; i<c_nnodes; i++)
			N(i) = c_shapefct[i];
		const double gp_p1 = N * nodal_p1;
		const double gp_T1 = N * nodal_T1;
		double var[3] = {};
		var[0] = gp_p1;
		var[1] = gp_T1;
		const double rhocp = MediaProp->HeatCapacity(Index, theta, this, var);
		const double drho_w_dp = FluidProp->drhodP(var);
		const double drho_w_dT = FluidProp->drhodT(var);
		const double cp_w = FluidProp->SpecificHeatCapacity(var);
		const double porosity = MediaProp->Porosity(Index, theta);
		const double drhocp_dp = porosity * cp_w * drho_w_dp;
		const double drhocp_dT = porosity * cp_w * drho_w_dT;
		const double fkt = vol / (double)nnodes;
		J_TT.diagonal().setConstant(1/dt * rhocp * fkt);
		J_TT += fkt * drhocp_dT * (nodal_T1 - nodal_T0)/dt * N;
		J_Tp += fkt * drhocp_dp * (nodal_T1 - nodal_T0)/dt * N;
	}

	//======================================================================
	// Loop over Gauss points
	int gp_r, gp_s, gp_t;
	double var[3] = {};
	Eigen::RowVectorXd N(c_nnodes);
	Eigen::MatrixXd dN(c_dim, c_nnodes);
	Eigen::RowVectorXd W_T(c_nnodes), W_SUPG(c_nnodes);
	for (gp = 0; gp < nGaussPoints; gp++)
	{
		//---------------------------------------------------------
		//  Get local coordinates and weights
		//  Compute Jacobian matrix and its determinate
		//---------------------------------------------------------
		const double fkt = GetGaussData(gp, gp_r, gp_s, gp_t);
		// Compute geometry
		ComputeShapefct(1);
		ComputeGradShapefct(1);
		double const* const c_shapefct = shapefct;
		double const* const c_dshapefct = dshapefct;
		N.setZero();
		dN.setZero();
		for (int i=0; i<c_nnodes; i++)
			N(i) = c_shapefct[i];
		for (unsigned i = 0; i < c_dim; i++)
			for (int j = 0; j < c_nnodes; j++)
				dN(i,j) = c_dshapefct[i * c_nnodes + j];
		W_T = N;

		//---------------------------------------------------------
		//  Get state variables
		//---------------------------------------------------------
		// const double gp_p0 = N * nodal_p0;
		const double gp_T0 = N * nodal_T0;
		const double gp_p1 = N * nodal_p1;
		const double gp_T1 = N * nodal_T1;
		//const double gp_dp = gp_p1 - gp_p0;
		const double gp_dT = gp_T1 - gp_T0;
		//Eigen::VectorXd grad_p0 = dN * nodal_p0;
		Eigen::VectorXd grad_p1 = dN * nodal_p1;
		//Eigen::VectorXd grad_T0 = dN * nodal_T0;
		Eigen::VectorXd grad_T1 = dN * nodal_T1;
		if (pcs->m_num->nls_jacobian_level == 1)
			grad_T1.setZero();

		//---------------------------------------------------------
		//  Get material properties
		//---------------------------------------------------------
		var[0] = gp_p1;
		var[1] = gp_T1;
		// Fluid properties
		const double rho_w = FluidProp->Density(var);
		const double drho_w_dp = FluidProp->drhodP(var);
		const double drho_w_dT = FluidProp->drhodT(var);
		const double vis = FluidProp->Viscosity(var);
		const double dvis_dp = FluidProp->dViscositydP(var);
		const double dvis_dT = FluidProp->dViscositydT(var);
		const double cp_w = FluidProp->SpecificHeatCapacity(var);
		// Medium properties
		const double rhocp = MediaProp->HeatCapacity(Index, theta, this, var);
		const double porosity = MediaProp->Porosity(Index, theta);
		const double drhocp_dp = porosity * cp_w * drho_w_dp; //TODO d(cp)/dp
		const double drhocp_dT = porosity * cp_w * drho_w_dT;

		//---------------------------------------------------------
		//  Set velocity
		//---------------------------------------------------------
		Eigen::VectorXd grad_h1 = grad_p1;
		if (hasGravity)
			grad_h1 -= rho_w * vec_g;
		Eigen::VectorXd q = - k / vis * grad_h1;

		//-----------------------------------------
		// Derivatives of flow velocity and heat flux
		//-----------------------------------------
		Eigen::MatrixXd dq_dp = - k / vis * dN;
		if (hasGravity && drho_w_dp != 0.0)
			dq_dp.noalias() += - k / vis * drho_w_dp * vec_g * N;
		if (dvis_dp != 0.0)
			dq_dp.noalias() += - dvis_dp / vis * q * N;
		Eigen::MatrixXd dq_dT = - dvis_dT * q * N;
		if (hasGravity && drho_w_dT != 0.0)
			dq_dT.noalias() += - k / vis * drho_w_dT * vec_g * N;

		Eigen::MatrixXd djDiff_dT = - lambda * dN;
		Eigen::MatrixXd djAdv_dT = rhocp * q.transpose() * dN;
		if (dq_dT.size() > 0)
			djAdv_dT.noalias() += rhocp * grad_T1.transpose() * dq_dT;
		if (drho_w_dT != .0)
			djAdv_dT.noalias() += drhocp_dT * q.transpose() * grad_T1 * N;
		Eigen::MatrixXd djAdv_dp = rhocp * grad_T1.transpose() * dq_dp;
		if (drho_w_dp != .0)
			djAdv_dp.noalias() += drhocp_dp * q.transpose() * grad_T1 * N;

		//---------------------------------------------------------
		//  SUPG coefficients
		//---------------------------------------------------------
		if (useSUPG)
		{
			W_SUPG.setZero();
			double supg_tau = 0;
			CalcSUPGWeightingFunction(&q[0], gp, supg_tau, &W_SUPG[0]);
			W_T += supg_tau * W_SUPG;
		}

		//---------------------------------------------------------
		//  Assemble Jacobian
		//---------------------------------------------------------

		if (isTransient)
			J_pp.noalias() += 1/dt * fkt * N.transpose() * Ss * N;
		J_pp.noalias() += - fkt * dN.transpose() * dq_dp;
		if (dq_dT.size() > 0)
			J_pT.noalias() += - fkt * dN.transpose() * dq_dT;

		if (isTransient && !useLumpedMass)
			J_TT.noalias() += 1/dt * W_T.transpose() * (rhocp + gp_dT * drhocp_dT) * N * fkt;
		J_TT.noalias() += - dN.transpose() * djDiff_dT * fkt;
		J_TT.noalias() += W_T.transpose() * djAdv_dT * fkt;
		if (isTransient && !useLumpedMass)
			J_Tp.noalias() += 1/dt * W_T.transpose() * (gp_dT * drhocp_dp) * N * fkt;
		J_Tp.noalias() += W_T.transpose() * djAdv_dp * fkt;
	}

	if (isMatrixElement && isMatrixFlowInactive)
	{
		J_pp.setZero();
		J_pT.setZero();
		J_Tp.setZero();
	}

	for (int i=0; i<c_nnodes; i++)
	{
		for (int j=0; j<c_nnodes; j++)
		{
			(*StiffMatrix)(offset_p + i, offset_p + j) = J_pp(i, j);
			(*StiffMatrix)(offset_p + i, offset_T + j) = J_pT(i, j);
			(*StiffMatrix)(offset_T + i, offset_p + j) = J_Tp(i, j);
			(*StiffMatrix)(offset_T + i, offset_T + j) = J_TT(i, j);
		}
	}

	if (pcs->scaleUnknowns || pcs->scaleEQS)
	{
		// scale p by 1e6
		const double scale_p =
			1. / pcs->vec_scale_dofs[0] * pcs->vec_scale_eqs[0];
		for (long i = 0; i < c_nnodes; i++)
			for (long j = 0; j < c_nnodes; j++)
			{
				(*StiffMatrix)(i, j) *= scale_p;
				(*StiffMatrix)(i, j + c_nnodes) *= scale_p;
				//				(*StiffMatrix)(i+c_nnodes,j) *= scale_p;
			}
		// scale T
		const double scale_T =
			1. / pcs->vec_scale_dofs[1] * pcs->vec_scale_eqs[1];
		for (long i = 0; i < c_nnodes; i++)
			for (long j = 0; j < c_nnodes; j++)
			{
				(*StiffMatrix)(i + c_nnodes, j) *= scale_T;
				(*StiffMatrix)(i + c_nnodes, j + c_nnodes) *= scale_T;
				//				(*StiffMatrix)(i,j+c_nnodes) *= scale_T;
				//				(*StiffMatrix)(i+c_nnodes,j+c_nnodes) *=
				// scale_T;
			}
	}
}

}  // end namespace


#include "headers/Triangle.hpp"
#include "headers/Object.hpp"
#include "headers/Hittable.hpp"
#include <armadillo>
#include <vector>

using namespace arma;
using namespace std;

Triangle::Triangle (vec triangulo){

    mat transf;
    vec kd, ks, aux;

    transf << triangulo(16) << triangulo(17) << triangulo(18) << triangulo(19) << endr
	   << triangulo(20) << triangulo(21) << triangulo(22) << triangulo(23) << endr
           << triangulo(24) << triangulo(25) << triangulo(26) << triangulo(27) << endr
           << 0 << 0 << 0 << 1;

    this->setTransf(transf);

    aux << triangulo(0) << triangulo(1) << triangulo(2) << 1;
    aux = transf * aux;
    aux.shed_row(3);

    this->setA(aux);
//    this->setA(triangulo(0) << triangulo(1) << triangulo(2) << 1);
//    this->setA(transf * this->getA());
//    this->setA(this->getA().shed_row(3));

    aux << triangulo(3) << triangulo(4) << triangulo(5) << 1;
    aux = transf * aux;
    aux.shed_row(3);

    this->setB(aux);

//    this->setB(triangulo(3) << triangulo(4) << triangulo(5) << 1);
//    this->setB(transf * this->getB());
//    this->setB(this->getB().shed_row(3));

    aux << triangulo(6) << triangulo(7) << triangulo(8) << 1;
    aux = transf * aux;
    aux.shed_row(3);

    this->setC(aux);

//    this->setC(triangulo(6) << triangulo(7) << triangulo(8) << 1);
//    this->setC(transf * this->getC());
//    this->setC(this->getC().shed_row(3));

    kd << triangulo(9) << triangulo(10) << triangulo(11);
    ks << triangulo(12) << triangulo(13) << triangulo(14);

    this->setKd(kd);
    this->setKs(ks);

    this->setP(triangulo(15));

    this->setTipo(1);

    this->setCentro((this->getA() + this->getB() + this->getC())/3);

    this->setNormal(cross(this->getB() - this->getA(), this->getC() - this->getA()));
}

bool Triangle::colide(const vec &d, double &T, const vec &origem){

    vec tuv;
    mat matriz1, matriz2;

    matriz1.insert_cols(0, origem-d);
    matriz1.insert_cols(1, this->B - this->A);
    matriz1.insert_cols(2, this->C - this->A);

    if ((det(matriz1)>0.000001) || (det(matriz1)<-0.000001)){

        matriz2 = origem - this->A;

	tuv = matriz1.i() * matriz2;

	if ((tuv(1)>=0) && (tuv(1)<=1) && (tuv(2)>=0) && (tuv(2)<=1) && (tuv(1)+tuv(2)<=1)){

            T=tuv(0);
            setNormal(normalise(getNormal()));

            return true;
	}
    }

    return false;
}