/*
 * diagonalize.cc
 */

#include "llvm/Pass.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/Module.h"
#include "llvm/IR/Constants.h"
#include "llvm/IR/Instructions.h"
#include "llvm/Analysis/LoopPass.h"
#include "llvm/ADT/ValueMap.h"
#include "llvm/Support/PatternMatch.h"
#include "llvm/Support/InstIterator.h"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

using namespace llvm;
using namespace PatternMatch;
using namespace Eigen;

#define OP_IN_RANGE(_op, _start, _end) \
    (_op >= Instruction:: _start && _op <= Instruction:: _end)

namespace
{

class ADPass : public LoopPass
{
    Loop* loop;
    DominatorTree* DT;
    ICmpInst* loop_cond;
    BasicBlock* loop_body;
    BasicBlock* exit_block;
    Value* nr_iters;
    BasicBlock* dgen;
    ConstantInt* start_iter;
    PHINode* iter_var;
    ValueMap<PHINode*, size_t> phis;

    /* Track linear combinations of state variables. */
    typedef ValueMap<PHINode*, double> Coefficients;

public:
    static char ID;

    ADPass() : LoopPass(ID) {}

    virtual void getAnalysisUsage(AnalysisUsage& AU) const {
        AU.addRequired<DominatorTree>();
    }

    /*
     * Drive the optimization pass.
     * Return true iff the optimization was applied.
     */
    virtual bool runOnLoop(Loop* L, LPPassManager&) {
        loop = L;
        DT = &getAnalysis<DominatorTree>();
        phis.clear();

        if (loop->getBlocks().size() != 1
            || !(loop_body = loop->getBlocks().front())
            || loop->getSubLoops().size()
            || !(exit_block = loop->getUniqueExitBlock())
            || !loopFilter()
            || !extractIterator())
        {
            return false;
        }

        /* Assign a dimension to each state variable. */
        size_t phi_index = 0;
        phis.erase(iter_var);
        for (auto kv = phis.begin(); kv != phis.end(); ++kv) {
            phis[kv->first] = phi_index++;
        }

        /* Find the initial state and all linear dependence relations. */
        size_t nr_phis = phis.size();

        MatrixXd InitialState(nr_phis, 1);
        MatrixXd TransformationMatrix(nr_phis, nr_phis);
        TransformationMatrix << MatrixXd::Zero(nr_phis, nr_phis);
        for (auto kv = phis.begin(); kv != phis.end(); ++kv) {
            PHINode* PN = kv->first;
            size_t phi_label = phis[PN];
            InitialState(phi_label, 0) = toDouble(getPhiConstVal(PN));

            Coefficients coeffs;
            if (!trackUpdates(getPhiFeedbackVal(PN), coeffs)) {
                return false;
            }
            for (auto ckv = coeffs.begin(); ckv != coeffs.end(); ++ckv) {
                size_t target_phi = phis[ckv->first];
                TransformationMatrix(phi_label, target_phi) = ckv->second;
            }
        }

        /* Diagonalize the transformation matrix. */
        EigenSolver<MatrixXd> EigSolver(TransformationMatrix);
        MatrixXcd P = EigSolver.eigenvectors();
        MatrixXcd D = EigSolver.eigenvalues().asDiagonal();
        MatrixXcd Pinv = P.inverse();
        if (!checkSystem(TransformationMatrix, P, D, Pinv)) {
            return false;
        }

        /* Emit instructions to compute the closed form in a new block. */
        LLVMContext& ctx = loop_body->getContext();
        Function* parentFunc = exit_block->getParent();

        Module* mod = parentFunc->getParent();
        dgen = BasicBlock::Create(ctx, "dgen", parentFunc, exit_block);
        Type* numTy = Type::getDoubleTy(ctx);
        Function* powf = NULL;
        if (!(powf = mod->getFunction("llvm.pow.f64"))) {
            std::vector<Type*> powProto(2, numTy);
            FunctionType* powType = FunctionType::get(numTy, powProto, false);
            powf = Function::Create(powType, GlobalValue::ExternalLinkage,
                "llvm.pow.f64", mod);
            powf->setCallingConv(CallingConv::C);
        }

        /* P(D^n) = r * λ^n : ∀(r) ∈ row(P) */
        Value** PDn = new Value*[nr_phis * nr_phis];
        Value* iexpt = BinaryOperator::Create(Instruction::Sub, nr_iters,
           start_iter, "iexpt", dgen);
        iexpt = BinaryOperator::Create(Instruction::Add, iexpt,
           ConstantInt::get(Type::getInt32Ty(ctx), 1), "iexpt_adj", dgen);
        Value* exponent = CastInst::Create(Instruction::UIToFP, iexpt, numTy,
            "fexpt", dgen);
        for (size_t j = 0; j < nr_phis; ++j) {
            /* λ[j]^n */
            Value* exptargs[] = {
                toConstantFP(ctx, std::real(D(j, j))), exponent
            };
            Value* eigvexpt = CallInst::Create(powf,
                ArrayRef<Value*>(exptargs, 2), "eigvexpt", dgen);

            /* PDn[i][j] = P[i][j] * λ[j]^n */
            for (size_t i = 0; i < nr_phis; ++i) {
                size_t index = i * nr_phis + j;
                PDn[index] = BinaryOperator::Create(Instruction::FMul,
                    toConstantFP(ctx, std::real(P(i, j))), eigvexpt, "pdn",
                                 dgen);
            }
        }

        /* xf = P(D^n) * Pinv * x0 */
        Value** soln = new Value*[nr_phis];
        Value* zero = toConstantFP(ctx, 0.0);
        for (size_t i = 0; i < nr_phis; ++i) {
            soln[i] = zero;
            for (size_t j = 0; j < nr_phis; ++j) {
                /* dotp = <a, b> : a ∈ row(P(D^n)), b ∈ col(Pinv) */
                Value* dotp = zero;
                for (size_t k = 0; k < nr_phis; ++k) {
                    Value* ik_kj = BinaryOperator::Create(Instruction::FMul,
                        PDn[i * nr_phis + k],
                        toConstantFP(ctx, std::real(Pinv(k, j))),
                        "ik_kj", dgen);
                    dotp = BinaryOperator::Create(Instruction::FAdd,
                        ik_kj, dotp, "dotp", dgen);
                }

                /* xf[i] = ∑ P(D^n)Pinv[i][j] * x0[j] */
                Value* xj_prod = BinaryOperator::Create(Instruction::FMul,
                    dotp, toConstantFP(ctx, InitialState(j)), "pdpxj",
                    dgen);
                soln[i] = BinaryOperator::Create(Instruction::FAdd,
                    xj_prod, soln[i], "xf", dgen);
            }
        }

        delete[] PDn;

        /* Rewire edges headed in and out of the loop. */
        for (inst_iterator II = inst_begin(parentFunc),
                           E = inst_end(parentFunc); II != E; ++II)
        {
            Instruction* instr = &*II;
            if (!isa<BranchInst>(instr)) {
                continue;
            }

            BranchInst* BI = cast<BranchInst>(instr);
            for (unsigned k = 0; k < BI->getNumSuccessors(); ++k) {
                if (BI->getSuccessor(k) == loop_body) {
                    BI->setSuccessor(k, dgen);
                }
            }
        }
        BranchInst::Create(exit_block, dgen);

        /* Replace values leading into phi nodes. */
        for (auto kv = phis.begin(); kv != phis.end(); ++kv) {
            PHINode* loopPhi = kv->first;
            Value* incoming = getPhiFeedbackVal(loopPhi);
            Value* target = soln[kv->second];
            rewriteLiveValues(incoming, target);
        }
        loop_body->replaceSuccessorsPhiUsesWith(dgen);

        /* Delete the old loop. */
        loop_body->eraseFromParent();

        delete[] soln;

        return true;
    }

private:
    /*
     * Check whether the loop is linearizale.
     */
    bool loopFilter() {
        loop_cond = NULL;
        for (auto II = loop_body->begin(); II != loop_body->end(); ++II) {
            Instruction* instr = II;
            if (isa<ICmpInst>(instr)) {
                if (loop_cond) {
                    return false;
                } else {
                    loop_cond = cast<ICmpInst>(instr);
                }
            } else if (isa<PHINode>(instr)) {
                PHINode* PN = cast<PHINode>(instr);
                if (PN->getNumIncomingValues() != 2
                    || !DT->dominates(PN->getIncomingBlock(0), loop_body))
                {
                    return false;
                }
                Value* inLhs = PN->getIncomingValue(0);
                Value* inRhs = PN->getIncomingValue(1);
                if (!isa<ConstantInt>(inLhs) && !isConstant(inRhs)) {
                    return false;
                } else {
                    phis[PN] = 0;
                }
            } else if (isa<BinaryOperator>(instr)) {
                BinaryOperator* binop = cast<BinaryOperator>(instr);
                if (!(OP_IN_RANGE(binop->getOpcode(), Add, FDiv))) {
                    return false;
                }
            } else if (!isa<BranchInst>(instr)) {
                return false;
            }
        }
        return loop_cond != NULL;
    }

    /*
     * Given a loop in canonical form, extract the loop condition and the
     * starting iteration. Return true iff basic sanity checks pass.
     */
    bool extractIterator() {
        Value* loop_var = NULL;
        ICmpInst::Predicate IPred;
        if (!match(loop_cond, m_ICmp(IPred, m_Value(loop_var),
                                            m_Value(nr_iters)))
            || IPred != CmpInst::Predicate::ICMP_EQ
            || !isa<PHINode>(loop_var))
        {
            return false;
        }

        iter_var = cast<PHINode>(loop_var);
        if (!match(getPhiConstVal(iter_var), m_ConstantInt(start_iter))) {
            return false;
        }

        bool foundIncr = false;
        for (auto II = loop_body->begin(); II != loop_body->end(); ++II) {
            Instruction* instr = II;
            if (!isa<BinaryOperator>(II)) {
                continue;
            }

            BinaryOperator* binop = cast<BinaryOperator>(instr);
            if (foundIncr &&
                (binop->getOperand(0) == iter_var
                || binop->getOperand(1) == iter_var))
            {
                return false;
            }

            if (binop->getOpcode() == Instruction::Add
                && ((binop->getOperand(0) == iter_var
                      && isa<ConstantInt>(binop->getOperand(1))
                      && toInt(binop->getOperand(1)) == 1) ||
                    (binop->getOperand(1) == iter_var
                      && isa<ConstantInt>(binop->getOperand(0))
                      && toInt(binop->getOperand(0)) == 1)))
            {
                foundIncr = true;
            }
        }
        return foundIncr;
    }

    Value* getPhiConstVal(PHINode* PN) {
      if (isConstant(PN->getIncomingValue(0))) {
        return PN->getIncomingValue(0);
      }
      return PN->getIncomingValue(1);
    }

    Value* getPhiFeedbackVal(PHINode* PN) {
      if (isConstant(PN->getIncomingValue(0))) {
        return PN->getIncomingValue(1);
      }
      return PN->getIncomingValue(0);
    }

    /*
     * Find a linear combination of state variables which generate @parent.
     * Return true iff a valid list of coefficients is found.
     */
    bool trackUpdates(Value* parent, Coefficients& coeffs, bool root = true) {
        if (isa<Constant>(parent)) {
            /*
             * If a state variable is set to a constant during each iteration,
             * the compiler should lift it out of the loop before we get here.
             */
            return !root;
        } else if (isa<PHINode>(parent)) {
            PHINode* PN = cast<PHINode>(parent);
            coeffs[PN] = 1;
            return phis.count(PN) == 1;
        } else if (isa<BinaryOperator>(parent)) {
            BinaryOperator* binop = cast<BinaryOperator>(parent);
            int opcode = binop->getOpcode();
            Value *LHS = binop->getOperand(0),
                  *RHS = binop->getOperand(1);

            Coefficients lhsCoeffs, rhsCoeffs;
            if (!trackUpdates(LHS, lhsCoeffs, false)
                || !trackUpdates(RHS, rhsCoeffs, false))
            {
                return false;
            }

            double scalar;
            if (OP_IN_RANGE(opcode, Add, FSub)) {
                /* Add instructions shouldn't operate on scalars. */
                if (isScalar(lhsCoeffs) || isScalar(rhsCoeffs)) {
                    return false;
                }
            } else {
                /* Mul instructions should only have one scalar operand. */
                if (!(isScalar(lhsCoeffs) ^ isScalar(rhsCoeffs))) {
                    return false;
                }

                /* Div instructions cannot have scalar numerators. */
                if (OP_IN_RANGE(opcode, UDiv, FDiv) && isScalar(lhsCoeffs)) {
                    return false;
                }

                scalar = toDouble(isScalar(lhsCoeffs) ? LHS : RHS);
            }

            /* Merge the two sets of coefficients. */
            for (auto kv = phis.begin(); kv != phis.end(); ++kv) {
                PHINode* PN = kv->first;
                double lcoeff = lhsCoeffs.lookup(PN);
                double rcoeff = rhsCoeffs.lookup(PN);

                /* Adding nil entries to 'coeffs' breaks isScalar(). */
                if (lcoeff == 0.0 && rcoeff == 0.0) {
                    continue;
                }

                if (OP_IN_RANGE(opcode, Add, FAdd)) {
                    coeffs[PN] = lcoeff + rcoeff;
                } else if (OP_IN_RANGE(opcode, Sub, FSub)) {
                    coeffs[PN] = lcoeff - rcoeff;
                } else if (OP_IN_RANGE(opcode, Mul, FMul)) {
                    coeffs[PN] = scalar * (lcoeff + rcoeff);
                } else {
                    coeffs[PN] = (1 / scalar) * (lcoeff + rcoeff);
                }
            }
            return true;
        }
        return false;
    }

    bool isConstant(Value* V) {
      return isa<ConstantInt>(V) || isa<ConstantFP>(V);
    }

    int64_t toInt(Value* V) {
        return cast<ConstantInt>(V)->getValue().getSExtValue();
    }

    double toDouble(Value* V) {
        if (isa<ConstantInt>(V)) {
            return double(toInt(V));
        } else if (isa<ConstantFP>(V)) {
            return cast<ConstantFP>(V)->getValueAPF().convertToDouble();
        }
        return 0.0;
    }

    Value* toConstantFP(LLVMContext& ctx, double n) {
        return ConstantFP::get(ctx, APFloat(n));
    }

    bool isScalar(Coefficients& coeffs) {
        return coeffs.size() == 0;
    }

    /*
     * Verify that A == PDP^-1 within an acceptable margin of error.
     */
    bool checkSystem(MatrixXd& A, MatrixXcd& P,
                     MatrixXcd& D, MatrixXcd& Pinv)
    {
        MatrixXcd PDPi = P*D*Pinv;
        const double epsilon = 25 * std::numeric_limits<double>::epsilon();
        for (int i = 0; i < P.rows(); ++i) {
            for (int j = 0; j < P.cols(); ++j) {
                if (std::imag(P(i, j)) != 0.0) {
                    return false;
                }
                if (std::abs(A(i, j) - PDPi(i, j)) > epsilon) {
                    return false;
                }
            }
        }
        return true;
    }

    /*
     * Some instructions may use the results of the loop. Rewire these
     * instructions so that they use the values from the @dgen basic block.
     */
    void rewriteLiveValues(Value* oldval, Value* target) {
        for (auto UI = oldval->use_begin(); UI != oldval->use_end(); ++UI) {
            User* user = *UI;

            /* Don't bother updating values in @loop_body. It dies soon. */
            if (isPartialComputation(user)) {
                continue;
            }

            if (isa<PHINode>(user)) {
                PHINode* userPhi = cast<PHINode>(user);
                for (unsigned i=0; i < userPhi->getNumIncomingValues(); ++i) {
                    if (userPhi->getIncomingValue(i) == oldval) {
                        userPhi->setIncomingValue(i, target);
                        userPhi->setIncomingBlock(i, dgen);
                    }
                }
                continue;
            }

            for (auto op = user->op_begin(); op != user->op_end(); ++op) {
                Use& phiUse = *op;
                Value* operand = phiUse.get();
                if (operand == oldval) {
                    phiUse.set(target);
                }
            }
        }
    }

    /*
     * Check if the target value is defined in the loop body.
     */
    bool isPartialComputation(Value* target) {
        for (auto II = loop_body->begin(); II != loop_body->end(); ++II) {
            Instruction* instr = II;
            if (instr == target) {
                return true;
            }
        }
        return false;
    }
};

char ADPass::ID = 0;

}

static RegisterPass<ADPass> X("auto-diagonalize",
    "Diagonalize linear dynamical systems", false, false);


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

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

//local
#include "../math/extent.hpp"
#include "common.hpp"

namespace eig = Eigen;

namespace tsdf{

template<typename Scalar>
inline eig::Matrix<Scalar,3,3,eig::ColMajor>
compute_covariance_camera_space(Scalar voxel_size, const eig::Matrix<Scalar,4,4>& camera_pose,
		Scalar gaussian_covariance_scale = 1.0f) {
	eig::Matrix<Scalar,3,3,eig::ColMajor> camera_rotation_matrix = camera_pose.block(0, 0, 3, 3);
	eig::Matrix<Scalar,3,3,eig::ColMajor> covariance_voxel_sphere_world_space = eig::Matrix<Scalar,3,3>::Identity() *
			(voxel_size * gaussian_covariance_scale);
	eig::Matrix<Scalar,3,3,eig::ColMajor> covariance_camera_space =
			camera_rotation_matrix * covariance_voxel_sphere_world_space * camera_rotation_matrix.transpose();
	return covariance_camera_space;
}

template<typename Scalar>
inline bool compute_sampling_bounds(
		math::Extent2d& extent,
		const eig::Matrix<Scalar,2,1>& bounds_max,
		const eig::Matrix<Scalar,2,1>& voxel_image,
		const eig::Matrix<unsigned short, eig::Dynamic, eig::Dynamic>& depth_image) {
	// compute sampling bounds
	int x_sample_start = static_cast<int>(voxel_image(0) - bounds_max(0));
	int x_sample_end = static_cast<int>(std::ceil(voxel_image(0) + bounds_max(0) + 1.0f));
	int y_sample_start = static_cast<int>(voxel_image(1) - bounds_max(1));
	int y_sample_end = static_cast<int>(std::ceil(voxel_image(1) + bounds_max(1) + 1.0f));

	// check that at least some samples within sampling range fall within the depth image
	if (x_sample_start >= depth_image.cols() || x_sample_end <= 0
			|| y_sample_start >= depth_image.rows() || y_sample_end <= 0) {
		return false;
	}

	// limit sampling bounds to image bounds
	extent.x_start = std::max(0, x_sample_start);
	extent.x_end = std::min(static_cast<int>(depth_image.cols()), x_sample_end);
	extent.y_start = std::max(0, y_sample_start);
	extent.y_end = std::min(static_cast<int>(depth_image.rows()), y_sample_end);
	return true;
}

template<typename Scalar>
inline bool compute_sampling_bounds_inclusive(
		math::Extent2d& extent,
		const eig::Matrix<Scalar,2,1>& bounds_max,
		const eig::Matrix<Scalar,2,1>& voxel_image,
		const eig::Matrix<unsigned short, eig::Dynamic, eig::Dynamic>& depth_image) {
	// compute sampling bounds
	extent.x_start = static_cast<int>(voxel_image(0) - bounds_max(0));
	extent.x_end = static_cast<int>(std::ceil(voxel_image(0) + bounds_max(0) + 1.0f));
	extent.y_start = static_cast<int>(voxel_image(1) - bounds_max(1));
	extent.y_end = static_cast<int>(std::ceil(voxel_image(1) + bounds_max(1) + 1.0f));

	//TODO: potential speedup -- remove check here and make function void -- we're already checking for "out-of-bounds" voxels
	// check that at least some samples within sampling range fall within the depth image
	if (extent.x_start >= depth_image.cols() || extent.x_end <= 0
			|| extent.y_start >= depth_image.rows() || extent.y_end <= 0) {
		return false;
	}
	return true;
}

template<typename Scalar>
inline
Scalar compute_voxel_EWA_image_space(
		const math::Extent2d& sampling_bounds,
		const eig::Matrix<Scalar,2,1>& voxel_image,
		const eig::Matrix<Scalar,3,1>& voxel_camera,
		const eig::Matrix<Scalar,2,2>& ellipse_matrix,
		const Scalar& squared_radius_threshold,
		const Scalar& depth_unit_ratio,
		const Scalar& narrow_band_half_width,
		const eig::Matrix<unsigned short, eig::Dynamic, eig::Dynamic>& depth_image){
	Scalar weights_sum = static_cast<Scalar>(0.0);
	Scalar depth_sum = static_cast<Scalar>(0.0);

	// collect sample readings
	for (int x_sample = sampling_bounds.x_start; x_sample < sampling_bounds.x_end; x_sample++) {
		for (int y_sample = sampling_bounds.y_start; y_sample < sampling_bounds.y_end; y_sample++) {
			eig::Matrix<Scalar,2,1> sample_centered;
			sample_centered <<
					static_cast<Scalar>(x_sample) - voxel_image(0),
					static_cast<Scalar>(y_sample) - voxel_image(1);
			Scalar dist_sq = sample_centered.transpose() * ellipse_matrix * sample_centered;
			//TODO: potential speedup -- remove check
			if (dist_sq > squared_radius_threshold) {
				continue;
			}
			Scalar weight = std::exp(static_cast<Scalar>(-0.5) * dist_sq);
			Scalar surface_depth = static_cast<Scalar>(depth_image(y_sample, x_sample)) * depth_unit_ratio;
			if (surface_depth <= static_cast<Scalar>(0.0)) {
				continue;
			}
			depth_sum += weight * surface_depth;
			weights_sum += weight;
		}
	}
	if (depth_sum <= static_cast<Scalar>(0.0)) {
		return static_cast<Scalar>(1.0);
	}

	Scalar final_depth = depth_sum / weights_sum;

	// signed distance from surface to voxel along camera axis
	// TODO: try with "along ray" and compare. Newcombe et al. in KinectFusion claim there won't be a difference...
	Scalar signed_distance = final_depth - voxel_camera[2];

	return compute_TSDF_value(signed_distance, narrow_band_half_width);
}

template<typename Scalar>
inline
Scalar compute_voxel_EWA_voxel_space(
		const math::Extent2d& sampling_bounds,
		const eig::Matrix<Scalar,2,1>& voxel_image,
		const eig::Matrix<Scalar,3,1>& voxel_camera,
		const eig::Matrix<Scalar,2,2>& ellipse_matrix,
		const Scalar& squared_radius_threshold,
		const Scalar& depth_unit_ratio,
		const Scalar& narrow_band_half_width,
		const eig::Matrix<unsigned short, eig::Dynamic, eig::Dynamic>& depth_image){
	Scalar weights_sum = static_cast<Scalar>(0.0);
	Scalar TSDF_sum = static_cast<Scalar>(0.0);

	// collect sample readings
	for (int x_sample = sampling_bounds.x_start; x_sample < sampling_bounds.x_end; x_sample++) {
		for (int y_sample = sampling_bounds.y_start; y_sample < sampling_bounds.y_end; y_sample++) {
			eig::Matrix<Scalar,2,1> sample_centered;
			sample_centered <<
					static_cast<Scalar>(x_sample) - voxel_image(0),
					static_cast<Scalar>(y_sample) - voxel_image(1);
			Scalar dist_sq = sample_centered.transpose() * ellipse_matrix * sample_centered;
			if (dist_sq > squared_radius_threshold) {
				continue;
			}
			Scalar weight = std::exp(static_cast<Scalar>(-0.5) * dist_sq);
			Scalar surface_depth = static_cast<Scalar>(depth_image(y_sample, x_sample)) * depth_unit_ratio;
			if (surface_depth <= static_cast<Scalar>(0.0)) {
				continue;
			}
			Scalar signed_distance = surface_depth - voxel_camera[2];
			TSDF_sum += weight * compute_TSDF_value(signed_distance, narrow_band_half_width);
			weights_sum += weight;
		}
	}

	if (weights_sum == static_cast<Scalar>(0.0)) {
		return static_cast<Scalar>(1.0);
	}

	return TSDF_sum / weights_sum;
}

template<typename Scalar>
inline
Scalar compute_voxel_EWA_voxel_space_inclusive(
		const math::Extent2d& sampling_bounds,
		const eig::Matrix<Scalar,2,1>& voxel_image,
		const eig::Matrix<Scalar,3,1>& voxel_camera,
		const eig::Matrix<Scalar,2,2>& ellipse_matrix,
		const Scalar& squared_radius_threshold,
		const Scalar& depth_unit_ratio,
		const Scalar& narrow_band_half_width,
		const eig::Matrix<uint16_t, eig::Dynamic, eig::Dynamic>& depth_image){
	Scalar weights_sum = static_cast<Scalar>(0.0);
	Scalar TSDF_sum = static_cast<Scalar>(0.0);

	// collect sample readings
	for (int x_sample = sampling_bounds.x_start; x_sample < sampling_bounds.x_end; x_sample++) {
		for (int y_sample = sampling_bounds.y_start; y_sample < sampling_bounds.y_end; y_sample++) {
			eig::Matrix<Scalar,2,1> sample_centered;
			sample_centered <<
					static_cast<Scalar>(x_sample) - voxel_image(0),
					static_cast<Scalar>(y_sample) - voxel_image(1);
			Scalar dist_sq = sample_centered.transpose() * ellipse_matrix * sample_centered;
			//TODO: potential speedup -- remove range checking
			if (dist_sq > squared_radius_threshold) {
				continue;
			}
			Scalar weight = std::exp(static_cast<Scalar>(-0.5) * dist_sq);

			if (y_sample < 0 || y_sample >= depth_image.rows() ||
					x_sample < 0 || x_sample >= depth_image.cols()) {
				TSDF_sum += weight;  // (* 1.0)
			} else {
				Scalar surface_depth = static_cast<Scalar>(depth_image(y_sample, x_sample)) * depth_unit_ratio;
				if (surface_depth <= static_cast<Scalar>(0.0)) {
					continue;
				}
				Scalar signed_distance = surface_depth - voxel_camera[2];
				TSDF_sum += weight * compute_TSDF_value(signed_distance, narrow_band_half_width);
			}
			weights_sum += weight;
		}
	}

	//TODO: potential speedup -- is it even possible for this condition to be true?
	if (weights_sum == static_cast<Scalar>(0.0)) {
		return static_cast<Scalar>(1.0);
	}
	return TSDF_sum / weights_sum;
}

}//namespace tsdf


//////////////////////////////////////////////////////////////////////////////
// non_parametric::kolmogorov_smirnov::check_convergence.hpp                //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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_STATISTICS_DETAIL_NON_PARAMETRIC_KOLMOGOROV_SMIRNOV_CHECK_CONVERGENCE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_KOLMOGOROV_SMIRNOV_CHECK_CONVERGENCE_HPP_ER_2009

#include <boost/mpl/copy.hpp>
#include <boost/mpl/back_inserter.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/statistics/stats.hpp>

#include <boost/statistics/detail/non_parametric/empirical_distribution/ordered_sample.hpp>
#include <boost/statistics/detail/non_parametric/kolmogorov_smirnov/statistic.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace empirical_distribution{

    // Usage:
    //     check_convergence<> check;
    //     check(n_loops,n,n_factor,distribution,random_generator,os);
    // Generates random samples of size { k = n^p : p=1,...,n_loops} and for
    // each computes the kolmogorov-smirnov statistic. The results are passed
    // to os.
    template<
    	typename T1 = double,
    	typename MoreFeatures = boost::accumulators::stats<>
    >
    struct check_convergence
    {

    	typedef T1 value_type;
        typedef kolmogorov_smirnov::tag::kolmogorov_smirnov tag_;
    	typedef boost::accumulators::stats<tag_> stats_;
    	typedef boost::mpl::push_back<boost::mpl::_,boost::mpl::_> op_;

       	typedef typename boost::mpl::copy<
        	MoreFeatures,boost::mpl::back_inserter< stats_ >
    	>::type mpl_features;

    	public:

    	check_convergence(){}

        template<typename D,typename G>
        struct traits{
            typedef typename D::value_type val_;
            typedef typename G::distribution_type random_;
            typedef typename random_::result_type sample_type;

            typedef boost::accumulators::accumulator_set<
            	sample_type,
                typename mpl_features::type
            > acc_;
        };

        template<typename D,typename G>
        typename traits<D,G>::acc_
        operator()(
            long n_loops,
            long n, 			// n *= n_factor at each loop
            long n_factor,
            const D& dist,
            G& gen,
            std::ostream& os
        )const{
            return (*this)(
            	boost::mpl::void_(),
            	n_loops,
                n,
                n_factor,
                dist,
                gen,
                default_fun<D>(),
                os
            );
        }

        template<
            typename Args,
            typename D,
            typename G,
            typename F,
            typename Out
        >
        typename traits<D,G>::acc_
        operator()(
            const Args& args,
            long n_loops,
            long n, // n *= n_factor at each loop
            long n_factor,
            const D& dist,
            G& gen,
            const F& fun,
            Out& out
        )const{
            typedef traits<D,G> traits_;
            typedef typename traits_::val_ val_;
            typedef typename traits_::random_ random_;
            typedef typename traits_::sample_type sample_type;
            typedef typename traits_::acc_ acc_;

            acc_ acc(args);

            for(long i1 = 0; i1<n_loops; i1++){
                for(long i2 = 0; i2< n; i2++){
                    sample_type x = gen();
                    acc(x);
                } // grows sample by n

                fun(acc,dist,out);
                n *= n_factor;
            }

            return acc;
        }

        template<typename D>
    	struct default_fun{

            typedef boost::numeric::bounds<value_type> bounds_;

            default_fun() : ks0( bounds_::highest() ){}

            template<typename AccSet>
            void operator()(const AccSet& acc,const D& d,std::ostream& os)const{
                namespace ns = kolmogorov_smirnov;
            	value_type ks1 = ns::statistic<value_type>( acc, d );
                bool ok = ks1 - ks0 < bounds_::smallest();
                os
                    << '('
                    << boost::accumulators::extract::count(acc)
                    << ','
                    << ks1
                    << ','
                    << ok
                    << ')'
                    << std::endl;
            }

            private:
            mutable value_type ks0;

    	};

    };

}// empirical_distribution
}// detail
}// statistics
}// boost

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_SINC_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_SINC_HPP_INCLUDED

#include <nt2/trigonometric/functions/sinc.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/sin.hpp>
#include <nt2/include/functions/simd/is_eqz.hpp>
#include <nt2/include/constants/one.hpp>
#include <boost/simd/sdk/config.hpp>

#if !defined(BOOST_SIMD_NO_DENORMALS)
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/constants/eps.hpp>
#endif

#if !defined(BOOST_SIMD_NO_INFINITIES)
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/include/functions/simd/is_inf.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( sinc_, boost::simd::tag::simd_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      result_type r1 =  nt2::sin(a0)/a0;

      #if !defined(BOOST_SIMD_NO_DENORMALS)
      r1 = nt2::if_else ( nt2::lt(nt2::abs(a0), nt2::Eps<A0>())
                        , nt2::One<A0>()
                        , r1
                        );
      #else
      r1 = nt2::if_else(nt2::is_eqz(a0), nt2::One<result_type>(), r1);
      #endif

      #if !defined(BOOST_SIMD_NO_INFINITIES)
      r1 = nt2::if_zero_else(nt2::is_inf(a0), r1);
      #endif

      return r1;
    }
  };
} }

#endif


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ATANH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ATANH_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/log1p.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/raw.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( atanh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      A0 absa0 = bs::abs(a0);
      A0 t =  absa0+absa0;
      A0 z1 = oneminus(absa0);
      return bitwise_xor(bitofsign(a0),
                         Half<A0>()*log1p((absa0 < Half<A0>())
                                          ? fma(t, absa0/z1, t)
                                          : t/z1)
                        );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( atanh_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                         )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &,  A0  a0) const BOOST_NOEXCEPT
    {
      return std::atanh(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( atanh_
                          , (typename A0)
                          , bd::cpu_
                          , bs::raw_tag
                          , bd::scalar_< bd::floating_<A0> >
                         )
  {
    BOOST_FORCEINLINE A0 operator() (const raw_tag &,  A0  a0) const BOOST_NOEXCEPT
    {
       return  Half<A0>()*log(inc(a0)/oneminus(a0));
    }
  };

} } }

#endif


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

#include "math/regression.h"
#include "math/statistics.h"
#include "sensors/sensor_log.h"
#include <boost/range/iterator_range.hpp>
#include <cassert>
#include <gnuplot-iostream.h>
#include <iostream>

using namespace vulcan;

using LaserIt = sensors::SensorLog::laser_iterator;

void estimate_sensor_noise(const std::string& laserName, LaserIt begin, LaserIt end);

int main(int argc, char** argv)
{
    sensors::SensorLog log(argv[1]);

    if (log.sizeFrontLaser() == 0) {
        return -1;
    }

    if (log.sizeBackLaser() == 0) {
        return -1;
    }

    estimate_sensor_noise("front", log.beginFrontLaser(), log.endFrontLaser());
    estimate_sensor_noise("back", log.beginBackLaser(), log.endBackLaser());

    return 0;
}

void estimate_sensor_noise(const std::string& laserName, LaserIt begin, LaserIt end)
{
    using ValueVec = std::vector<double>;
    std::vector<ValueVec> values(begin->ranges.size());

    for (auto& scan : boost::make_iterator_range(begin, end)) {
        assert(values.size() == scan.ranges.size());
        for (std::size_t n = 0; n < scan.ranges.size(); ++n) {
            if ((scan.ranges[n] > 0.0f) && (scan.ranges[n] < 40.0f)) {
                values[n].push_back(scan.ranges[n]);
            }
        }
    }

    std::vector<double> means;
    std::vector<double> variances;

    for (auto& v : values) {
        means.push_back(math::mean(v.begin(), v.end()));
        variances.push_back(std::sqrt(math::variance(v.begin(), v.end())));
    }

    std::vector<Point<float>> meanVsVar;
    for (std::size_t n = 0; n < means.size(); ++n) {
        // Toss out high std dev as they must have hit something dynamic
        //         if(variances[n] < 0.2)
        //         {
        meanVsVar.emplace_back(means[n], variances[n]);
        //         }
    }

    auto line = math::total_least_squares(meanVsVar.begin(), meanVsVar.end());
    std::cout << "Line fit:" << line << '\n';

    Gnuplot plot;
    plot << "plot '-' using 1:2 with points title 'Laser: " << laserName << "'\n";
    plot.send1d(boost::make_tuple(means, variances));
}


#ifndef EXPSUM_REDUCTION_BALANCED_TRUNCATION_HPP
#define EXPSUM_REDUCTION_BALANCED_TRUNCATION_HPP

#include <cassert>

#include <armadillo>

#include "expsum/reduction/cholesky_quasi_cauchy.hpp"
#include "expsum/reduction/coneig_sym_rrd.hpp"

namespace expsum
{

template <typename T>
class balanced_truncation
{
public:
    using size_type    = arma::uword;
    using value_type   = T;
    using real_type    = typename arma::get_pod_type<T>::result;
    using complex_type = std::complex<real_type>;

    using vector_type         = arma::Col<value_type>;
    using index_vector_type   = arma::uvec;
    using real_vector_type    = arma::Col<real_type>;
    using complex_vector_type = arma::Col<complex_type>;

    using matrix_type         = arma::Mat<value_type>;
    using real_matrix_type    = arma::Mat<real_type>;
    using complex_matrix_type = arma::Mat<complex_type>;

private:
    size_type size_;
    vector_type exponent_;
    vector_type weight_;

    index_vector_type ipiv_;
    vector_type work_;
    real_vector_type rwork_;

public:
    template <typename VecP, typename VecW>
    typename std::enable_if<(arma::is_basevec<VecP>::value &&
                             arma::is_basevec<VecW>::value),
                            void>::type
    run(const VecP& p, const VecW& w, real_type tol);

    void resize(size_type n)
    {
        if (exponent_.size() < n)
        {
            exponent_.set_size(n);
            weight_.set_size(n);
            ipiv_.set_size(n);
            work_.set_size(std::max(n * (n + 6), n * (3 * n + 1)));
            rwork_.set_size(arma::is_complex<T>::value ? 3 * n : n);
        }
    }

    size_type size() const
    {
        return size_;
    }

    //
    // @return Vector view to the exponents.
    //
    auto exponents() const -> decltype(exponent_.head(size_))
    {
        return exponent_.head(size_);
    }
    //
    // @return Vector view to the weights.
    //
    auto weights() const -> decltype(weight_.head(size_))
    {
        return weight_.head(size_);
    }

private:
    //
    // Compute eigenvalue decomposition of (n x n) symmetric matrix A.
    //
    // @A On input, matrix A, on exit, A is overwritten.
    // @d On exit, eigenvalues of matrix A
    //
    // --- for real matrix
    //     workspace size (lwork): n * n
    static void diagonalize(real_matrix_type& A, real_matrix_type& X,
                            real_vector_type& d, real_type* work,
                            size_type lwork, real_type* /*dummy*/)
    {
        char jobz = 'V';
        char uplo = 'U';

        auto n_     = arma::blas_int(A.n_rows);
        auto lwork_ = static_cast<arma::blas_int>(lwork);
        auto info_  = arma::blas_int();

        arma::lapack::syev(&jobz, &uplo, &n_, A.memptr(), &n_, d.memptr(), work,
                           &lwork_, &info_);
        if (info_)
        {
            std::ostringstream msg;
            msg << "[s/d]SYEV error: failed with info " << info_;
            throw std::logic_error(msg.str());
        }

        X = A;
    }

    // --- for complex matrix
    //     workspace size (lwork): n * n
    //     real workspace size: 2 * n
    static void diagonalize(complex_matrix_type& A, complex_matrix_type& X,
                            complex_vector_type& d, complex_type* work,
                            size_type lwork, real_type* rwork)
    {
        char jobvl = 'N'; // Do not compute left eigenvectors of A
        char jobvr = 'V'; // Compute right eigenvectors of A

        complex_type vl[2]; // dummy

        auto n_     = arma::blas_int(A.n_rows);
        auto ldvl_  = arma::blas_int(1);
        auto lwork_ = static_cast<arma::blas_int>(lwork);
        auto info_  = arma::blas_int();

        complex_type* work_ = work + A.n_elem;

        arma::lapack::cx_geev(&jobvl, &jobvr, &n_, A.memptr(), &n_, d.memptr(),
                              &vl[0], &ldvl_, X.memptr(), &n_, work_, &lwork_,
                              rwork, &info_);
        if (info_)
        {
            std::ostringstream msg;
            msg << "[c/z]GEEV error: failed with info " << info_;
            throw std::logic_error(msg.str());
        }

        for (size_type j = 0; j < X.n_cols; ++j)
        {
            auto xj          = X.col(j);
            const auto t     = arma::dot(xj, xj);
            const auto scale = value_type(1) / std::sqrt(t);
            xj *= scale;
        }
    }
};

template <typename T>
template <typename VecP, typename VecW>
typename std::enable_if<(arma::is_basevec<VecP>::value &&
                         arma::is_basevec<VecW>::value),
                        void>::type
balanced_truncation<T>::run(const VecP& p, const VecW& w, real_type tol)
{
    using cholesky_rrd = cholesky_quasi_cauchy<value_type>;

    assert(p.n_elem == w.n_elem);

    const auto n = p.n_elem;
    resize(n);
    if (n <= size_type(1))
    {
        // Quick return
        return;
    }

    value_type* ptr_X = work_.memptr();
    value_type* ptr_a = ptr_X + n * n;
    real_type* ptr_d  = reinterpret_cast<real_type*>(exponent_.memptr());

    //
    // Set the factors that defines the quasi-Cauchy matrix
    //
    //   P(i, j) = a[i] * b[j] /(x[i] + y[j]),
    //   a[i] = sqrt(w[i]),
    //   b[i] = sqrt(conj(p[i])),
    //   x[i] = p[i],
    //   y[i] = conj(p[i]),
    //
    // which is the controllability Gramian matrix of the system.
    //
    vector_type a(ptr_a + 0 * n, n, false, true);
    vector_type b(ptr_a + 1 * n, n, false, true);
    vector_type x(ptr_a + 2 * n, n, false, true);
    vector_type y(ptr_a + 3 * n, n, false, true);
    vector_type work1(ptr_a + 4 * n, n, false, true);
    vector_type work2(ptr_a + 5 * n, n, false, true);

    a = arma::sqrt(w);
    b = arma::conj(a);
    x = p;
    y = arma::conj(x);

    //
    // Rank-revealing Cholesky factorization of Gramian matrix given as
    //
    // P = X * D^2 * X.t()
    //
    // Cholesky factorization can be computed accurately using the Gaussian
    // elimination with complete pivoting (GECP).
    //
    index_vector_type ipiv(ipiv_.memptr(), n, false, true);
    size_ = cholesky_rrd::pivot_order(a, b, x, y, tol, ipiv, work1);

    matrix_type X1(ptr_X, n, size_, false, true);
    real_vector_type d(ptr_d, size_, false, true);

    cholesky_rrd::factorize(a, b, x, y, X1, d, work1, work2);
    cholesky_rrd::apply_row_permutation(X1, ipiv, work1);

    //
    // Compute the state-space transformation matrix.
    //
    // First compute eigenvalue decomposition of L * Q * L.t(), where Q is
    // observability Gramian matrix of the system, which can be obtained as
    // Q = P.st()
    //
    // Let us define
    //
    //   G = D * X.st() * X * D,
    //
    // then eigenvalue decomposition
    //
    //   L * Q * L.t() = G.t() * G = X * S^2 * X.t().
    //
    // As described by Haut and Beylkin (2011), the eigenvectors of G.t() * G
    // become con-eigenvectors of matrix P, i.e.,
    //
    //   P = conj(X) * S * X.t().
    //
    // where the matrix X is (complex) orthogonal, X.t() * X = I
    //
    // The matrix conj(X) is the desired transformation matrix of the system.
    //
    // `coneig_sym_rrd` computes only the con-eigenvalues greater than the
    // target accuracy `tol` and corresponding con-eigenvectors.
    //
    // NOTE: Required memory for workspace
    //
    // work size : 2 * n * (n + 1)
    // rwork size:
    //    if `T` is real type   : n
    //    if `T` is complex type: 3 * n
    //
    const auto k = coneig_sym_rrd<T>::run(X1, d, tol, ptr_a, rwork_.memptr());
    //
    // Apply transformation matrix
    //
    // A1 = Xk.t() * diagmat(a) * conj(Xk)
    // b1 = Xk.t() * b
    // c1 = b.st() * conj(Xk) = b1.st()
    //
    // where Xk = X1.head_cols(k) that satisfies X_k.st() * X_k = I_k
    //
    auto Xk = X1.head_cols(k);
    matrix_type A1(ptr_a, k, k, false, true);
    vector_type p_(exponent_.memptr(), k, false, true);
    vector_type w_(weight_.memptr(), k, false, true);
    A1 = Xk.t() * arma::diagmat(p) * arma::conj(Xk);
    w_ = Xk.t() * arma::sqrt(w);

    //
    // Compute eigenvalue decomposition of the (k x k) matrix, A1. Since A1 is
    // real/complex symmetric matrix, the eigen decomposition has the form
    //
    //   A1 = X2 * D * X2.st(), (X2.st() * X2 = I).
    //
    matrix_type X2(ptr_X, k, k, false, true);
    diagonalize(A1, X2, p_, ptr_a + n * n, n * n, rwork_.memptr());

    //
    // Apply the state space transformation by X2,
    //
    // A2 = X2.st() * A1 * X2 = D
    // b2 = X2.st() * b1
    // c2 = b1 * X2 = b2.st()
    //
    // Finally parameters for truncated exponential sum can be obtained as
    //
    // p' = A2 = diag(D)
    // w' = c2 % b2 = square(b2)
    //
    size_     = k;
    A1.col(0) = w_;
    w_        = X2.st() * A1.col(0);
    w_        = arma::square(w_);

    return;
}

} // namespace: expsum

#endif /* EXPSUM_REDUCTION_BALANCED_TRUNCATION_HPP */


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *
 * planeRegistration.hpp
 *
 *  Created on: Jan 28, 2019
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#ifndef PLANEREGISTRATION_HPP_
#define PLANEREGISTRATION_HPP_

#include <vector>
#include "mrob/SE3.hpp"
#include <Eigen/StdVector>
#include "mrob/plane.hpp"
#include "mrob/optimizer.hpp"
#include "mrob/time_profiling.hpp"

#include <unordered_map>
#include <memory>

namespace mrob{

/**
 * class PlaneRegistration introduced a class for the alignment of
 * planes.
 */
class PlaneRegistration: public OptimizerDense{

  public:
    // XXX is this mode used anymore? deprecated?
    enum TrajectoryMode{SEQUENCE=0, INTERPOLATION};
    // XXX Solve method is almost deprecated
    //enum SolveModeGrad{GRADIENT_DESCENT_NAIVE=0, GRADIENT_DESCENT_INCR, STEEPEST, HEAVYBALL, MOMENTUM, MOMENTUM_SEQ, BENGIOS_NAG, GRADIENT_DESCENT_BACKTRACKING, BFGS};
    enum SolveMode{INITIALIZE=0,
                   GRADIENT,
                   GRADIENT_BENGIOS_NAG,
                   GRADIENT_ALL_POSES,
                   GN_HESSIAN,
                   GN_CLAMPED_HESSIAN,
                   LM_SPHER,
                   LM_ELLIP};

  public:
    PlaneRegistration();
    //PlaneRegistration(uint_t numberPlanes , uint_t numberPoses);
    ~PlaneRegistration();

    // Function from the parent class Optimizer
    virtual matData_t calculate_error() override;
    virtual void calculate_gradient_hessian() override;
    virtual void update_state() override;
    virtual void bookkeep_state() override;
    virtual void update_state_from_bookkeep() override;

    // Specific methods
    void set_number_planes_and_poses(uint_t numPlanes, uint_t numPoses);
    uint_t get_number_planes() const {return numberPlanes_;};
    uint_t get_number_poses() const {return numberPoses_;};

    /**
     * solve() calculates the poses on trajectory such that the minimization objective
     * is met: J = sum (lamda_min_plane)
     */
    uint_t solve(SolveMode mode, bool singleIteration = false);
    /**
     * solve_interpolate() calculates the poses on trajectory such that the minimization objective
     * is met: J = sum (lamda_min_plane), and the trajectory is described as an interpolation from I to T_f
     */
    uint_t solve_interpolate_gradient(bool singleIteration = false);
    //Gradient for all poses
    uint_t solve_gradient_all_poses(bool singleIteration = false);
    /**
     * solve_interpolate_hessian() calculates the poses on trajectory such that the minimization objective
     * is met: J = sum (lamda_min_plane), and the trajectory is described as an interpolation from I to T_f
     * using second order methods with Hessian. Very similar to solve_interpolate
     */
    //uint_t solve_interpolate_hessian(bool singleIteration = false);
    /**
     * Initialization_solve give a first guess on all poses by using classical point-point
     * methods SVD-based to calculate an initial condition closer to the true solution
     */
    uint_t solve_initialize();
    /**
     * Solve quaternion plane uses a paramteric representation for each plane, a quaternion,
     * and optimizes both the plane parameters and the trajectory variables
     */
    uint_t solve_quaternion_plane();
    /**
     * reset_solution, resets the current calculated solution while maintainting all data (planes)
     * This function is intended for comparing different solvers without replicating data
     */
    void reset_solution();
    double get_current_error() const;
    /**
     * Get trajectory returns a SE3 transformations,
     */
    SE3 get_trajectory(uint_t time);

    SE3 get_last_pose() {return trajectory_->back();}

    /**
     * Sets the trajectory (current solution) by addint the last pose
     */
    void set_last_pose(SE3 &last);

    /**
     * add_plane adds a plane structure already initialized and filled with data
     */
    void add_plane(uint_t id, std::shared_ptr<Plane> &plane);
    /**
     * add new plane is an ALTERNATIVE (for py) to the above function
     * which creates a new plane inside the class and then points are added one by one
     *
     */
    void add_new_plane(uint_t id);
    /**
     * add a point to the new plane. An ALTERNATIVE (for py) to add points into the sover
     * class (instead of adding the point fully as in add_plane())
     *
     */
    void plane_push_back_point(uint_t id, uint_t t, Mat31 &point);

    uint_t calculate_total_number_points();
    std::shared_ptr<Plane> & get_plane(uint_t id);

    std::unordered_map<uint_t, std::shared_ptr<Plane>>& get_all_planes() {return planes_;};

    void set_alpha_parameter(double alpha) {alpha_ = alpha;};
    void set_beta_parameter(double beta) {beta_ = beta;};

    double calculate_poses_rmse(std::vector<SE3> & groundTruth) const;

    void print(bool plotPlanes = true) const;

    /**
     * print evaluate looks for degenerate cases, such as planes normal vectors,
     * Hessian rank, det of all normals, etc. Basically this function tries to answer
     * if the problem is ill-conditioned
     *
     * Returns: 0) current error
     *          1) number of iters,
     *          2) determinant
     *          3) number of negative eigenvalues
     *          4) conditioning number
     */
    std::vector<double> print_evaluate();

    /**
     * add point_cloud requires a complete set of points observed at a given time
     * stamp (XXX now only an integer) and fills in the registration structure.
     */
    void add_point_cloud_planes(uint_t time, std::vector<Mat31>& points, std::vector<uint_t>& point_ids);
    /**
     * get_point_cloud gets all raw point, according to the current time index
     * from trajectory. It does not distinguish between planes.
     */
    std::vector<Mat31> get_point_cloud(uint_t time);
    std::vector<Mat31> get_point_plane_ids(uint_t time);

  protected:
    // flag for detecting when is has been solved
    uint_t numberPlanes_, numberPoses_, numberPoints_;
    uint_t isSolved_;
    PlaneRegistration::TrajectoryMode trajMode_ {};
    uint_t time_{};
    std::unordered_map<uint_t, std::shared_ptr<Plane>> planes_;
    std::shared_ptr<std::vector<SE3>> trajectory_;
    SE3 bookept_trajectory_;//last pose is stored/bookept
    double tau_ {};//variable for weighting the number of poses in traj
    uint_t solveIters_ {};

    // 1st order parameters methods if used
    PlaneRegistration::SolveMode solveMode_;
    std::vector<Mat61> previousState_;
    double c1_, c2_;    //parameters for the Wolfe conditions DEPRECATED?
    double alpha_, beta_;

    //2nd order data (if used) TODO remove since they are defined in parent class
    Mat61 gradient__;
    Mat6 hessian__;

    // time profiling
    TimeProfiling time_profiles_;
    double initial_error_ {}; // for benchmark purposes

    // alternative structure to keep planes. This is only for the python bindings

};

}// namespace
#endif /* PLANEREGISTRATION_HPP_ */


// This file is part of SWGANH which is released under the MIT license.
// See file LICENSE or go to http://swganh.com/LICENSE

#include <anh/utility_python.h>
#include <glm/glm.hpp>
#include <glm/gtx/quaternion.hpp>

#include <boost/python.hpp>
using namespace boost::python;
namespace anh
{
namespace utilities
{
void define_class_glm_vec3()
{
	class_<glm::vec3>("vector3",
		"Stores a direction vector in three-dimensional space", no_init)
		.def(init<glm::float_t, glm::float_t, glm::float_t>())
		.def(init<const glm::vec3&>())
        .def_readwrite("x", &glm::vec3::x)
        .def_readwrite("y", &glm::vec3::y)
        .def_readwrite("z", &glm::vec3::z)
		.def("__len__", &utility::constant_len_len<glm::vec3, 3>)
		.def("__getitem__", &utility::constant_len_get_item<glm::vec3, 3, glm::float_t>)
		.def("__setitem__", &utility::constant_len_set_item<glm::vec3, 3, glm::float_t>)
		.def(self == self)
		.def(self != self)
		.def(self + self)
		.def(self - self)
		.def(self * glm::float_t())
		.def(glm::float_t() * self)
		.def(self += self)
		.def(self -= self)
		.def(self *= glm::float_t())
		.def(self /= glm::float_t());
		//.def(self_ns::str(self));
}
void define_class_glm_quat()
{
    class_<glm::quat>("quat",
		"Stores a quaternion", no_init)
		.def(init<glm::float_t, glm::float_t, glm::float_t, glm::float_t>())
		.def(init<const glm::quat&>())
        .def_readwrite("x", &glm::quat::x)
        .def_readwrite("y", &glm::quat::y)
        .def_readwrite("z", &glm::quat::z)
        .def_readwrite("w", &glm::quat::w)
		.def("__len__", &utility::constant_len_len<glm::quat, 4>)
		.def("__getitem__", &utility::constant_len_get_item<glm::quat, 4, glm::float_t>)
		.def("__setitem__", &utility::constant_len_set_item<glm::quat, 4, glm::float_t>)
		.def(self == self)
		.def(self != self)
		/*.def(self + self)
		.def(self - self)*/
		.def(self * glm::float_t())
		.def(glm::float_t() * self)
		/*.def(self += self)
		.def(self -= self)*/
		.def(self *= glm::float_t())
		.def(self /= glm::float_t());
		//.def(self_ns::str(self));
}
}} // namespace anh::utilities


#pragma once
#include <Eigen/Eigen>
#include <nlohmann/json.hpp>

class TFIsing
{
private:
	int n_;
	double J_;
	double h_;
public:
	TFIsing(int n, double J, double h)
		: n_(n), J_(J), h_(h)
	{
	}

	nlohmann::json params() const
	{
		return nlohmann::json
		{
			{"name", "TFIsing"},
			{"n", n_},
			{"J", J_},
			{"h", h_}
		};
	}

	template<class State>
	typename State::Scalar operator()(const State& smp) const
	{
		typename State::Scalar s = 0.0;
		//Nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			s += J_*smp.sigmaAt(i)*smp.sigmaAt((i+1)%n_);
			s += h_*smp.ratio(i);
		}
		return s;
	}

	std::vector< std::array<int, 1> > offDiagonals(const Eigen::VectorXi& s) const
	{
		(void)s;
		std::vector< std::array<int, 1> > res;
		for(int i = 0; i < n_; i++)
		{
			res.push_back(std::array<int, 1>{i});
		}
		return res;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		std::map<uint32_t, double> res;
		for(int i = 0; i < n_; i++)
		{
			int s1 = (col >> i) & 1;
			int s2 = (col >> ((i+1) % n_)) & 1;
			long long int x = (1 << i);
			res[col ^ x] += h_;
			res[col] += J_*(1-2*s1)*(1-2*s2);
		}
		return res;
	}
};


#ifndef _UBLAS_
#define _UBLAS_

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

#endif

#ifndef _KMEANS_
#define _KMEANS_

using namespace boost::numeric::ublas;
using namespace boost::numeric;

double dist_xc /* データ点xとクラスタ中心c_jとの距離 */
(const ublas::matrix_row<const ublas::matrix<double> > x,
 const ublas::matrix<double> c,
 const unsigned int j)
{
  double sum = 0;
  for(unsigned int d = 0; d < x.size(); ++d){
    sum += (x[d] - c(j,d)) * (x[d] - c(j,d));
  }
  return sqrt(sum);
}

double dist_cc /* クラスタ中心間の距離を計算 */
(const ublas::matrix<double> mat1, const unsigned int r1,
 const ublas::matrix<double> mat2, const unsigned int r2)
{
  if(mat1.size2() != mat2.size2()){
    return -1;
  }else{
    double sum = 0;
    for(unsigned int i = 0; i < mat1.size2(); ++i){
      sum += (mat1(r1, i) - mat2(r2, i)) * (mat1(r1, i) - mat2(r2, i));
    }
    return sqrt(sum);
  }
}

#endif


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

#include <CTraitsTest.h>
#include <gtest/gtest.h>
#include <mrpt/math/CSparseMatrix.h>
#include <mrpt/random.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::math;
using namespace std;

template class mrpt::CTraitsTest<mrpt::math::CSparseMatrix>;

void generateRandomSparseMatrix(
	size_t N, size_t M, size_t nEntries, CSparseMatrix& MAT)
{
	MAT.clear();

	MAT.setRowCount(N);
	MAT.setColCount(M);

	for (size_t i = 0; i < nEntries; i++)
	{
		MAT.insert_entry(
			mrpt::random::getRandomGenerator().drawUniform32bit() % N,
			mrpt::random::getRandomGenerator().drawUniform32bit() % M,
			mrpt::random::getRandomGenerator().drawGaussian1D(0, 1));
	}

	// Return already compressed:
	MAT.compressFromTriplet();
}

void do_test_init_to_unit(size_t N)
{
	CMatrixDouble dense1;
	dense1.setIdentity(N);

	CSparseMatrix SM(dense1);

	CMatrixDouble dense_out;
	SM.get_dense(dense_out);

	EXPECT_TRUE(dense_out == dense1) << "Failed with N=" << N << "\n";
}

TEST(SparseMatrix, InitFromDenseUnit)
{
	do_test_init_to_unit(1);
	do_test_init_to_unit(10);
	do_test_init_to_unit(100);
}

void do_test_init_random(size_t N)
{
	CMatrixDouble dense1(N, N);
	mrpt::random::getRandomGenerator().drawGaussian1DMatrix(dense1);
	CSparseMatrix SM(dense1);
	CMatrixDouble dense_out;
	SM.get_dense(dense_out);
	EXPECT_TRUE(dense_out == dense1) << "Failed with N=" << N << "\n";
}

TEST(SparseMatrix, InitFromDenseRandom)
{
	do_test_init_random(1);
	do_test_init_random(10);
	do_test_init_random(100);
}

TEST(SparseMatrix, InitFromTriplet)
{
	CSparseMatrix SM;
	CMatrixDouble D(10, 20);

	SM.insert_entry(2, 2, 4.0);
	D(2, 2) = 4.0;
	SM.insert_entry(6, 8, -2.0);
	D(6, 8) = -2.0;

	SM.setRowCount(10);
	SM.setColCount(20);

	CMatrixDouble dense_out1;
	SM.get_dense(dense_out1);

	SM.compressFromTriplet();

	CMatrixDouble dense_out2;
	SM.get_dense(dense_out2);

	EXPECT_TRUE(dense_out1 == dense_out2);
}

TEST(SparseMatrix, InitFromSparse)
{
	CMatrixDouble D(4, 5);
	mrpt::math::CSparseMatrixTemplate<double> S(4, 5);
	D(1, 2) = 2.0;
	S(1, 2) = 2.0;

	D(3, 1) = -7.0;
	S(3, 1) = -7.0;

	CSparseMatrix SM(S);
	CMatrixDouble dense_out;
	SM.get_dense(dense_out);
	EXPECT_TRUE(dense_out == D) << "Dense: \n"
								<< D << "Sparse:\n"
								<< dense_out << endl;
}

TEST(SparseMatrix, InitFromRandom)
{
	CSparseMatrix SM;
	generateRandomSparseMatrix(100, 100, 25, SM);
	generateRandomSparseMatrix(20, 10, 15, SM);
}

using TMatrixSMOperator = void (*)(
	const CSparseMatrix& M1, const CSparseMatrix& M2, CSparseMatrix& res);
using TMatrixDenseOperator = void (*)(
	const CMatrixDouble& M1, const CMatrixDouble& M2, CMatrixDouble& res);

void do_matrix_op_test(
	size_t nRows1, size_t nCols1, size_t nNonZeros1, size_t nRows2,
	size_t nCols2, size_t nNonZeros2, TMatrixSMOperator op1,
	TMatrixDenseOperator op2)
{
	CSparseMatrix SM1, SM2;
	generateRandomSparseMatrix(nRows1, nCols1, nNonZeros1, SM1);
	generateRandomSparseMatrix(nRows2, nCols2, nNonZeros2, SM2);

	CSparseMatrix SM_res;
	(*op1)(SM1, SM2, SM_res);

	// Check:
	CMatrixDouble D1, D2, Dres;
	SM1.get_dense(D1);
	SM2.get_dense(D2);
	SM_res.get_dense(Dres);

	CMatrixDouble RES;
	(*op2)(D1, D2, RES);

	const double err = (RES - Dres).array().abs().maxCoeff();

	EXPECT_TRUE(err < 1e-10) << "M1:\n"
							 << D1 << "M2:\n"
							 << D2 << "Real op result:\n"
							 << RES << "SM result:\n"
							 << Dres << "ERR:\n"
							 << (RES - Dres);
}

void op_sparse_add(
	const CSparseMatrix& M1, const CSparseMatrix& M2, CSparseMatrix& res)
{
	res = M1 + M2;
}
void op_dense_add(
	const CMatrixDouble& M1, const CMatrixDouble& M2, CMatrixDouble& res)
{
	res = M1 + M2;
}

TEST(SparseMatrix, Op_Add)
{
	do_matrix_op_test(1, 1, 0, 1, 1, 0, &op_sparse_add, &op_dense_add);
	do_matrix_op_test(1, 1, 1, 1, 1, 1, &op_sparse_add, &op_dense_add);
	do_matrix_op_test(2, 2, 1, 2, 2, 1, &op_sparse_add, &op_dense_add);
	do_matrix_op_test(10, 20, 33, 10, 20, 33, &op_sparse_add, &op_dense_add);
	do_matrix_op_test(11, 21, 34, 11, 21, 34, &op_sparse_add, &op_dense_add);
}

void op_sparse_multiply_AB(
	const CSparseMatrix& M1, const CSparseMatrix& M2, CSparseMatrix& res)
{
	res = M1 * M2;
}
void op_dense_multiply_AB(
	const CMatrixDouble& M1, const CMatrixDouble& M2, CMatrixDouble& res)
{
	if (M1.isSquare() && M2.isSquare()) res = M1 * M2;
	else
		res = M1.asEigen() * M2.asEigen();
}

TEST(SparseMatrix, Op_Multiply_AB)
{
	do_matrix_op_test(
		1, 1, 0, 1, 1, 0, &op_sparse_multiply_AB, &op_dense_multiply_AB);
	do_matrix_op_test(
		1, 1, 1, 1, 1, 1, &op_sparse_multiply_AB, &op_dense_multiply_AB);
	do_matrix_op_test(
		2, 2, 1, 2, 2, 1, &op_sparse_multiply_AB, &op_dense_multiply_AB);
	do_matrix_op_test(
		10, 20, 33, 20, 15, 33, &op_sparse_multiply_AB, &op_dense_multiply_AB);
	do_matrix_op_test(
		8, 34, 100, 34, 3, 100, &op_sparse_multiply_AB, &op_dense_multiply_AB);
}

TEST(SparseMatrix, CholeskyDecomp)
{
	CSparseMatrix SM(10, 10);
	const auto COV1 = mrpt::random::getRandomGenerator()
						  .drawDefinitePositiveMatrix<CMatrixDouble>(6, 0.2);
	const auto COV2 = mrpt::random::getRandomGenerator()
						  .drawDefinitePositiveMatrix<CMatrixDouble>(4, 0.2);

	SM.insert_submatrix(0, 0, COV1);
	SM.insert_submatrix(6, 6, COV2);
	SM.compressFromTriplet();

	CSparseMatrix::CholeskyDecomp Chol(SM);

	const CMatrixDouble L = Chol.get_L();  // lower triangle

	// Compare with the dense matrix implementation:
	CMatrixDouble D;
	SM.get_dense(D);

	CMatrixDouble Ud;  // Upper triangle
	D.chol(Ud);

	const double err = (Ud.transpose() - L.asEigen()).array().abs().mean();
	EXPECT_TRUE(err < 1e-8);
}


#pragma once
#include <Eigen/Geometry>
#include "mtao/types.hpp"
#include "mtao/colvector_loop.hpp"

namespace mtao { namespace geometry {
    template <typename T, int D> using BBox = Eigen::AlignedBox<T,D>;
    template <typename T, int D>
        auto bounding_box(const mtao::ColVectors<T,D>& V) {
            BBox<T,D> bb;
            if(V.cols() > 0) {
                bb.extend(V.rowwise().minCoeff());
                bb.extend(V.rowwise().maxCoeff());

            }
            return bb;
        }
    template <typename T, int D>
        auto bounding_box_slow(const mtao::ColVectors<T,D>& V) {
            BBox<T,D> bb;
            if(V.cols() > 0) {
                bb.extend(V.rowwise().minCoeff());
                bb.extend(V.rowwise().maxCoeff());
                for(auto&& p: colvector_loop(V)) {
                    bb.extend(p);
                }
            }

            return bb;
        }

    //expand by multiplying by a scale
    template <typename T, int D>
        BBox<T,D> expand_bbox(BBox<T,D> bb, T scale) {
            using Vec = mtao::Vector<T,D>;
            Vec s = (scale-1) * (T(.5) * bb.sizes());
            bb.min() -= s;
            bb.max() += s;
            return bb;
        }
    //expand by adding a factor
    template <typename T, int D>
        BBox<T,D> offset_bbox(BBox<T,D> bb, T offset) {
            bb.min().array() -= offset;
            bb.max().array() += offset;
            return bb;
        }
}}


/*
 * test_image_io.cpp
 *
 *  Created on: Mar 26, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 */
#define BOOST_TEST_MODULE test_image_io

//libraries
#include <boost/test/unit_test.hpp>

//test data
#include "data/test_data_image_io.hpp"

//test targets
#include "../src/math/typedefs.hpp"
#include "../src/image_io/png_eigen.hpp"
#include "common.hpp"

BOOST_AUTO_TEST_CASE(test_image_read01) {
	math::MatrixXus depth_image;
	bool image_read = read_image_helper(depth_image, "zigzag_depth_00064.png");
	BOOST_REQUIRE(image_read);
	BOOST_REQUIRE_EQUAL(depth_image.rows(), 480);
	BOOST_REQUIRE_EQUAL(depth_image.cols(), 640);
	BOOST_REQUIRE_EQUAL(depth_image(0, 0), (unsigned short )1997);
	BOOST_REQUIRE_EQUAL(depth_image(479, 0), (unsigned short )1997);
	BOOST_REQUIRE_EQUAL(depth_image(479, 639), (unsigned short ) 5154);
	math::MatrixXus sample = depth_image.block(40, 60, 1, 20);
	BOOST_REQUIRE(sample.isApprox(test_data::depth_00064_sample));
}


/**
 * \author Norihiro Watanabe
 * \date   2013-09-06
 *
 * \copyright
 * Copyright (c) 2013, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/LICENSE.txt
 */

#include <gtest/gtest.h>

#include <limits>
#include <algorithm>
#include <vector>

#ifdef OGS_USE_EIGEN
#include <Eigen/Eigen>
#endif

#include "MeshLib/Elements/Quad.h"
#include "NumLib/Fem/ShapeFunction/ShapeQuad4.h"
#include "NumLib/Fem/CoordinatesMapping/ShapeMatrices.h"
#include "NumLib/Fem/CoordinatesMapping/NaturalCoordinatesMapping.h"

#include "../TestTools.h"

using namespace NumLib;

#ifdef OGS_USE_EIGEN

namespace
{
class NumLibFemNaturalCoordinatesMappingQuad4Test : public ::testing::Test
{
 public:
    // Matrix types
    static const unsigned dim = 2;
    static const unsigned e_nnodes = 4;
    typedef Eigen::Matrix<double, e_nnodes, 1> NodalVector;
    typedef Eigen::Matrix<double, dim, e_nnodes, Eigen::RowMajor> DimNodalMatrix;
    typedef Eigen::Matrix<double, dim, dim, Eigen::RowMajor> DimMatrix;
    // Shape data type
    typedef ShapeMatrices<NodalVector,DimNodalMatrix,DimMatrix> ShapeMatricesType;
    // Natural coordinates mapping type
    typedef NaturalCoordinatesMapping<MeshLib::Quad, ShapeQuad4, ShapeMatricesType> NaturalCoordsMappingType;

 public:
    NumLibFemNaturalCoordinatesMappingQuad4Test()
    {
        // create four quad elements used for testing
        naturalQuad   = createNaturalShapeQuad();
        irregularQuad = createIrregularShapeQuad();
        clockwiseQuad = createClockWiseQuad();
        zeroAreaQuad  = createZeroAreaQuad();

        // for destructor
        vec_eles.push_back(naturalQuad);
        vec_eles.push_back(irregularQuad);
        vec_eles.push_back(clockwiseQuad);
        vec_eles.push_back(zeroAreaQuad);
        for (auto e : vec_eles)
            for (unsigned i=0; i<e->getNNodes(true); i++)
                vec_nodes.push_back(e->getNode(i));
    }

    ~NumLibFemNaturalCoordinatesMappingQuad4Test()
    {
        for (auto itr = vec_nodes.begin(); itr!=vec_nodes.end(); ++itr )
            delete *itr;
        for (auto itr = vec_eles.begin(); itr!=vec_eles.end(); ++itr )
            delete *itr;
    }

    // quad having shape identical to that in natural coordinates
    MeshLib::Quad* createNaturalShapeQuad()
    {
        MeshLib::Node** nodes = new MeshLib::Node*[e_nnodes];
        nodes[0] = new MeshLib::Node( 1.0,  1.0,  0.0);
        nodes[1] = new MeshLib::Node(-1.0,  1.0,  0.0);
        nodes[2] = new MeshLib::Node(-1.0, -1.0,  0.0);
        nodes[3] = new MeshLib::Node( 1.0, -1.0,  0.0);
        return new MeshLib::Quad(nodes);
    }

    // quad having irregular or skew shape
    MeshLib::Quad* createIrregularShapeQuad()
    {
        MeshLib::Node** nodes = new MeshLib::Node*[e_nnodes];
        nodes[0] = new MeshLib::Node(-0.5, -0.5,  0.0);
        nodes[1] = new MeshLib::Node( 0.6, -0.6,  0.0);
        nodes[2] = new MeshLib::Node( 0.5,  0.4,  0.0);
        nodes[3] = new MeshLib::Node(-0.3,  0.1,  0.0);
        return new MeshLib::Quad(nodes);
    }

    // invalid case: clock wise node ordering
    MeshLib::Quad* createClockWiseQuad()
    {
        MeshLib::Node** nodes = new MeshLib::Node*[e_nnodes];
        nodes[0] = new MeshLib::Node( 1.0,  1.0,  0.0);
        nodes[3] = new MeshLib::Node(-1.0,  1.0,  0.0);
        nodes[2] = new MeshLib::Node(-1.0, -1.0,  0.0);
        nodes[1] = new MeshLib::Node( 1.0, -1.0,  0.0);
        return new MeshLib::Quad(nodes);
    }

    // invalid case: zero area
    MeshLib::Quad* createZeroAreaQuad()
    {
        MeshLib::Node** nodes = new MeshLib::Node*[e_nnodes];
        nodes[0] = new MeshLib::Node( 1.0,  1.0,  0.0);
        nodes[1] = new MeshLib::Node(-1.0,  1.0,  0.0);
        nodes[2] = new MeshLib::Node(-1.0,  1.0,  0.0);
        nodes[3] = new MeshLib::Node( 1.0,  1.0,  0.0);
        return new MeshLib::Quad(nodes);
    }

    static const double r[dim];
    static const double exp_N[e_nnodes];
    static const double exp_dNdr[e_nnodes*dim];
    static const double eps;

    std::vector<const MeshLib::Node*> vec_nodes;
    std::vector<const MeshLib::Quad*> vec_eles;
    MeshLib::Quad* naturalQuad;
    MeshLib::Quad* irregularQuad;
    MeshLib::Quad* clockwiseQuad;
    MeshLib::Quad* zeroAreaQuad;

}; // NumLibFemNaturalCoordinatesMappingQuad4Test

const double NumLibFemNaturalCoordinatesMappingQuad4Test::r[dim] = {0.5, 0.5};
const double NumLibFemNaturalCoordinatesMappingQuad4Test::exp_N[e_nnodes] = {0.5625, 0.1875, 0.0625, 0.1875};
const double NumLibFemNaturalCoordinatesMappingQuad4Test::exp_dNdr[e_nnodes*dim] = {0.375, -0.375, -0.125, 0.125, 0.375, 0.125, -0.125, -0.375};
const double NumLibFemNaturalCoordinatesMappingQuad4Test::eps = std::numeric_limits<double>::epsilon();

} // namespace

TEST_F(NumLibFemNaturalCoordinatesMappingQuad4Test, CheckFieldSpecification_N)
{
    ShapeMatricesType shape(dim, e_nnodes);

    //only N
    NaturalCoordsMappingType::computeShapeMatrices<ShapeMatrixType::N>(*naturalQuad, r, shape);
    ASSERT_FALSE(shape.N.isZero());
    ASSERT_TRUE(shape.dNdr.isZero());
    ASSERT_TRUE(shape.J.isZero());
    ASSERT_TRUE(shape.detJ == .0);
    ASSERT_TRUE(shape.invJ.isZero());
    ASSERT_TRUE(shape.dNdx.isZero());
}

TEST_F(NumLibFemNaturalCoordinatesMappingQuad4Test, CheckFieldSpecification_DNDR)
{
    ShapeMatricesType shape(dim, e_nnodes);

    // dNdr
    NaturalCoordsMappingType::computeShapeMatrices<ShapeMatrixType::DNDR>(*naturalQuad, r, shape);
    ASSERT_TRUE(shape.N.isZero());
    ASSERT_FALSE(shape.dNdr.isZero());
    ASSERT_TRUE(shape.J.isZero());
    ASSERT_TRUE(shape.detJ == .0);
    ASSERT_TRUE(shape.invJ.isZero());
    ASSERT_TRUE(shape.dNdx.isZero());
}

TEST_F(NumLibFemNaturalCoordinatesMappingQuad4Test, CheckFieldSpecification_N_J)
{
    ShapeMatricesType shape(dim, e_nnodes);

    // N_J
    shape.setZero();
    NaturalCoordsMappingType::computeShapeMatrices<ShapeMatrixType::N_J>(*naturalQuad, r, shape);
    ASSERT_FALSE(shape.N.isZero());
    ASSERT_FALSE(shape.dNdr.isZero());
    ASSERT_FALSE(shape.J.isZero());
    ASSERT_FALSE(shape.detJ == .0);
    ASSERT_TRUE(shape.invJ.isZero());
    ASSERT_TRUE(shape.dNdx.isZero());
}

TEST_F(NumLibFemNaturalCoordinatesMappingQuad4Test, CheckFieldSpecification_DNDR_J)
{
    ShapeMatricesType shape(dim, e_nnodes);

    // dNdr, J
    NaturalCoordsMappingType::computeShapeMatrices<ShapeMatrixType::DNDR_J>(*naturalQuad, r, shape);
    ASSERT_TRUE(shape.N.isZero());
    ASSERT_FALSE(shape.dNdr.isZero());
    ASSERT_FALSE(shape.J.isZero());
    ASSERT_FALSE(shape.detJ == .0);
    ASSERT_TRUE(shape.invJ.isZero());
    ASSERT_TRUE(shape.dNdx.isZero());
}

TEST_F(NumLibFemNaturalCoordinatesMappingQuad4Test, CheckFieldSpecification_DNDX)
{
    ShapeMatricesType shape(dim, e_nnodes);

    // DNDX
    shape.setZero();
    NaturalCoordsMappingType::computeShapeMatrices<ShapeMatrixType::DNDX>(*naturalQuad, r, shape);
    ASSERT_TRUE(shape.N.isZero());
    ASSERT_FALSE(shape.dNdr.isZero());
    ASSERT_FALSE(shape.J.isZero());
    ASSERT_FALSE(shape.detJ == .0);
    ASSERT_FALSE(shape.invJ.isZero());
    ASSERT_FALSE(shape.dNdx.isZero());
}

TEST_F(NumLibFemNaturalCoordinatesMappingQuad4Test, CheckFieldSpecification_ALL)
{
    ShapeMatricesType shape(dim, e_nnodes);

    // ALL
    shape.setZero();
    NaturalCoordsMappingType::computeShapeMatrices(*naturalQuad, r, shape);
    ASSERT_FALSE(shape.N.isZero());
    ASSERT_FALSE(shape.dNdr.isZero());
    ASSERT_FALSE(shape.J.isZero());
    ASSERT_FALSE(shape.detJ == .0);
    ASSERT_FALSE(shape.invJ.isZero());
    ASSERT_FALSE(shape.dNdx.isZero());
}

TEST_F(NumLibFemNaturalCoordinatesMappingQuad4Test, CheckNaturalShape)
{
    // identical to natural coordinates
    ShapeMatricesType shape(dim, e_nnodes);

    NaturalCoordsMappingType::computeShapeMatrices(*naturalQuad, r, shape);
    double exp_J[]= {1.0, 0.0, 0.0, 1.0};

    ASSERT_ARRAY_NEAR(exp_N, shape.N.data(), shape.N.size(), eps);
    ASSERT_ARRAY_NEAR(exp_dNdr, shape.dNdr.data(), shape.dNdr.size(), eps);
    ASSERT_ARRAY_NEAR(exp_J, shape.J.data(), shape.J.size(), eps);
    ASSERT_ARRAY_NEAR(exp_J, shape.invJ.data(), shape.invJ.size(), eps);
    ASSERT_NEAR(1.0, shape.detJ, eps);
    ASSERT_ARRAY_NEAR(exp_dNdr, shape.dNdx.data(), shape.dNdx.size(), eps);
}

TEST_F(NumLibFemNaturalCoordinatesMappingQuad4Test, CheckIrregularShape)
{
    // irregular shape
    ShapeMatricesType shape(dim, e_nnodes);

    NaturalCoordsMappingType::computeShapeMatrices(*irregularQuad, r, shape);
//        std::cout << shape;
    double exp_J[]= {-0.5125, 0.0, -0.0625, -0.35};
    double exp_invJ[]= {-1.9512195121951219, 0.0, 0.3484320557491290, -2.8571428571428572};
    double exp_dNdx[]= {-0.73170731707317072, 0.73170731707317072, 0.243902439024390, -0.24390243902439029, -0.940766550522648, -0.48780487804878048, 0.313588850174216, 1.1149825783972125};

    ASSERT_ARRAY_NEAR(exp_N, shape.N.data(), shape.N.size(), eps);
    ASSERT_ARRAY_NEAR(exp_dNdr, shape.dNdr.data(), shape.dNdr.size(), eps);
    ASSERT_ARRAY_NEAR(exp_J, shape.J.data(), shape.J.size(), eps);
    ASSERT_ARRAY_NEAR(exp_invJ, shape.invJ.data(), shape.invJ.size(), eps);
    ASSERT_NEAR(0.179375, shape.detJ, eps);
    ASSERT_ARRAY_NEAR(exp_dNdx, shape.dNdx.data(), shape.dNdx.size(), eps);
}

TEST_F(NumLibFemNaturalCoordinatesMappingQuad4Test, CheckClockwise)
{
    // clockwise node ordering, which is invalid)
    ShapeMatricesType shape(dim, e_nnodes);

    NaturalCoordsMappingType::computeShapeMatrices(*clockwiseQuad, r, shape);
    //std::cout << shape;
    double exp_J[]= {0.0, 1.0, 1.0, 0.0};
    // Inverse of the Jacobian matrix doesn't exist
    double exp_invJ[dim*dim]= {0.0};
    double exp_dNdx[dim*e_nnodes]= {0.0};

    ASSERT_ARRAY_NEAR(exp_N, shape.N.data(), shape.N.size(), eps);
    ASSERT_ARRAY_NEAR(exp_dNdr, shape.dNdr.data(), shape.dNdr.size(), eps);
    ASSERT_ARRAY_NEAR(exp_J, shape.J.data(), shape.J.size(), eps);
    ASSERT_ARRAY_NEAR(exp_invJ, shape.invJ.data(), shape.invJ.size(), eps);
    ASSERT_NEAR(-1.0, shape.detJ, eps);
    ASSERT_ARRAY_NEAR(exp_dNdx, shape.dNdx.data(), shape.dNdx.size(), eps);
}

TEST_F(NumLibFemNaturalCoordinatesMappingQuad4Test, CheckZeroArea)
{
    // zero area
    ShapeMatricesType shape(dim, e_nnodes);

    NaturalCoordsMappingType::computeShapeMatrices(*zeroAreaQuad, r, shape);
    //std::cout << shape;
    double exp_J[]= {1.0, 0.0, 0.0, 0.0};
    // Inverse of the Jacobian matrix doesn't exist
    double exp_invJ[dim*dim]= {0.0};
    double exp_dNdx[dim*e_nnodes]= {0.0};

    ASSERT_ARRAY_NEAR(exp_N, shape.N.data(), shape.N.size(), eps);
    ASSERT_ARRAY_NEAR(exp_dNdr, shape.dNdr.data(), shape.dNdr.size(), eps);
    ASSERT_ARRAY_NEAR(exp_J, shape.J.data(), shape.J.size(), eps);
    ASSERT_ARRAY_NEAR(exp_invJ, shape.invJ.data(), shape.invJ.size(), eps);
    ASSERT_NEAR(0.0, shape.detJ, eps);
    ASSERT_ARRAY_NEAR(exp_dNdx, shape.dNdx.data(), shape.dNdx.size(), eps);
}

#endif //OGS_USE_EIGEN


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

#define BOOST_MATH_MAX_SERIES_ITERATION_POLICY 10000000

#include <boost/math/special_functions/hypergeometric_1f1.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <map>
#include <boost/math/tools/test_data.hpp>
#include <boost/random.hpp>
#include "mp_t.hpp"

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;

struct hypergeometric_1f1_gen
{
   mp_t operator()(mp_t a1, mp_t a2, mp_t z)
   {
      std::cout << a1 << " " << a2 << " " << z << std::endl;
      mp_t result = boost::math::detail::hypergeometric_1f1_generic_series(a1, a2, z, boost::math::policies::policy<>());
      std::cout << a1 << " " << a2 << " " << z << " " << result << std::endl;
      return result;
   }
};

struct hypergeometric_1f1_gen_2
{
   mp_t operator()(mp_t a1, mp_t a2, mp_t z)
   {
      mp_t result = boost::math::detail::hypergeometric_1f1_generic_series(a1, a2, z, boost::math::policies::policy<>());
      std::cout << a1 << " " << a2 << " " << z << " " << result << std::endl;
      if (fabs(result) > (std::numeric_limits<double>::max)())
      {
         std::cout << "Discarding result as too large\n";
         throw std::domain_error("");
      }
      if (static_cast<double>(result) == 1)
      {
         std::cout << "Discarding result as unity\n";
         throw std::domain_error("");  // uninteresting result.
      }
      return result;
   }
};

int main(int, char* [])
{
   parameter_info<mp_t> arg1, arg2, arg3;
   test_data<mp_t> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for 2F0:\n";

   std::string line;
   bool cont;

#if 1
   std::vector<mp_t> v;
   random_ns::mt19937 rnd;
   random_ns::uniform_real_distribution<float> ur_a(0, 1);

   mp_t p = ur_a(rnd);
   p *= 1e6;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e5;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e4;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e3;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e2;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e-5;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e-12;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e-30;
   v.push_back(p);
   v.push_back(-p);

   for (unsigned i = 0; i < v.size(); ++i)
   {
      for (unsigned j = 0; j < v.size(); ++j)
      {
         for (unsigned k = 0; k < v.size(); ++k)
         {
            arg1 = make_single_param(v[i]);
            arg2 = make_single_param(v[j] * 3 / 2);
            arg3 = make_single_param(v[k] * 5 / 4);
            data.insert(hypergeometric_1f1_gen_2(), arg1, arg2, arg3);
         }
      }
   }

#else

   do {
      get_user_parameter_info(arg1, "a1");
      get_user_parameter_info(arg2, "a2");
      get_user_parameter_info(arg3, "z");
      data.insert(hypergeometric_1f1_gen(), arg1, arg2, arg3);
      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   } while (cont);

#endif
   std::cout << "Enter name of test data file [default=hypergeometric_1f1.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "hypergeometric_1f1.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, line.c_str());

   return 0;
}


/*****************************************************************************
*
* Functions for calculating the gap
*
* Copyright (C) 2018 by Hidemaro Suwa
* e-mail:suwamaro@phys.s.u-tokyo.ac.jp
*
*****************************************************************************/

#include "calc_gap.h"
#include "rpa_util.h"
#include <armadillo>

cx_double calc_intensity_square(int L, hoppings const& ts, double mu, double U, double delta, double qx, double qy, cx_double omega, bool zz){
  double k1 = 2. * M_PI / (double)L;

  cx_double A = 0, B = 0, C = 0, D = 0;
  /* Summing up at all the wavevectors */
  for(int x=-L/2; x < L/2; x++){
    double kx = k1 * x;
    for(int y=-L/2; y < L/2; y++){
      double ky = k1 * y;
      add_to_sus_mat2( ts, mu, A, B, C, D, qx, qy, 0, kx, ky, 0, delta, omega, zz );	    }
  }

  int n_sites = L * L;
  A *= 2. / (double)n_sites;
  B *= 2. / (double)n_sites;
  C *= 2. / (double)n_sites;
  D *= 2. / (double)n_sites;

  /* RPA */
  arma::cx_mat chi0_mat(2,2);
  chi0_mat(0,0) = A;   // (A, A) correlation
  chi0_mat(0,1) = B;   // (A, B)
  chi0_mat(1,0) = C;   // (B, A)
  chi0_mat(1,1) = D;   // (B, B)

  /* Transverse = < \sigma^- \sigma^+ >; Longitudinal (zz) = < \sigma^z \sigma^z > */
  /* Note that 2 < \sigma^- \sigma^+ > = < \sigma^z \sigma^z > (U -> 0 for the SU(2) case) */
  double factor_channel = 1.0;
  if ( zz ) { factor_channel = 0.5; }

  arma::cx_mat denom = arma::eye<arma::cx_mat>(2,2) - factor_channel * U * chi0_mat;
  arma::cx_mat chi_mat = chi0_mat * arma::inv(denom);

  // // for check
  // chi_mat = chi0_mat;

  // sigma-to-spin factor
  double factor_operator = 0.5;
  cx_double chi = factor_operator * factor_operator * ( chi_mat(0,0) - chi_mat(1,0) - chi_mat(0,1) + chi_mat(1,1) );

  return chi;
}

// cx_double calc_intensity_square2(int L, double t, double mu, double U, double delta, double qx, double qy, cx_double omega, bool zz){
//   double k1 = 2. * M_PI / (double)L;
//   double e_eps = 1e-12;
//   cx_double polarization = 0;

//   /* Summing up over all the wavevectors in the magnetic BZ. */
//   for(int x=-L/2; x < L/2; x++){
//     double kx = k1 * x;

//     for(int y=-L/2; y < L/2; y++){
//       double ky = k1 * y;

//       double e_free = energy_free_electron( t, mu, kx, ky );
//       if ( e_free < mu + e_eps ) {
// 	double factor = 1.;
// 	if ( std::abs( e_free - mu ) < e_eps ) {
// 	  factor = 0.5;
// 	}

// 	double kx2 = wave_vector_in_BZ( kx + qx );
// 	double ky2 = wave_vector_in_BZ( ky + qy );
// 	double e_free2 = energy_free_electron( t, mu, kx2, ky2 );

// 	double Ek = eigenenergy_HF_plus(e_free, delta);
// 	double Ek2 = eigenenergy_HF_plus(e_free2, delta);

// 	double element = 0;
// 	if ( zz ) {
// 	  element = 1. - (e_free * e_free2 + delta * delta) / (Ek * Ek2);
// 	} else {
// 	  element = 1. - (e_free * e_free2 - delta * delta) / (Ek * Ek2);
// 	}

// 	cx_double denom = 1. / (omega - Ek2 - Ek) + 1. / ( - std::conj(omega) - Ek2 - Ek);
// 	polarization += - factor * element / denom;
//       }
//     } /* end for y */
//   } /* end for x */

//   int nsites = L * L;
//   polarization /= (double)(2*nsites);
//   return polarization / ( 1. - U * polarization );
// }


//
// Copyright Antoine Leblanc 2010 - 2015
// Distributed under the MIT license.
//
// http://nauths.fr
// http://github.com/nicuveo
// mailto://antoine.jp.leblanc@gmail.com
//

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Includes

#include <cstdlib>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "nauths/mml/mml.hh"

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Local code

namespace
{

  typedef mml::Shape<mml::Num> Shape;
  typedef mml::Point<mml::Num> Point;
  typedef mml::Line<mml::Num> Line;
  typedef mml::Point<mml::Num> Vector;
  typedef mml::Polygon<mml::Num> Polygon;

  typedef mml::Tessellation<mml::Num> Tessellation;

  typedef std::map<int, Shape> ShapeMap;

  Line oriented(const Line& l)
  {
    return (l.x0() == l.x1()
            ? (l.y0() < l.y1()
               ? l
               : -l)
            : (l.x0() < l.x1()
               ? l
               : -l));
  }

  Line
  common_line(const Polygon& p1, const Polygon& p2)
  {
    // ugly
    for (const Line& l1 : p1.lines())
      for (const Line& l2 : p2.lines())
        if (oriented(l1) == oriented(l2))
          return oriented(l1);
    BOOST_CHECK(false);
    return Line();
  }

  void check_pair(const Polygon& p1, const Polygon& p2)
  {
    const Line& common = common_line(p1, p2);
    Line centers(p1.center(), p2.center());
    mml::Real n1;
    mml::Real n2;
    mml::Real d1;
    mml::Real d2;
    mml::il::lines_coeff(common, centers, n1, n2, d1, d2);

    BOOST_CHECK_NE(0, d1);
    BOOST_CHECK_NE(0, d2);
    BOOST_CHECK_LE(0, std::abs(n1));
    BOOST_CHECK_LE(0, std::abs(n2));
    BOOST_CHECK_LE(std::abs(n1), std::abs(d1));
    BOOST_CHECK_LE(std::abs(n2), std::abs(d2));

    double real = n2 / d2;
    double theoretical = mml::tiling::ratio(p1.size(), p2.size());
    double diff = std::abs(theoretical - real);

    // std::cout << "real: " << std::setw(10) << real << " theo: " << theoretical << std::endl;
    BOOST_CHECK_LT(diff / theoretical, 0.05);
  }

  void check_tiling(mml::TilingType type)
  {
    Shape ref(Shape::square(20000));
    Tessellation tess(type, ref, 5000);
    Tessellation::iterator it;
    ShapeMap m;

    for (it = tess.begin(false, true); it != tess.end(); ++it)
    {
      m[it.index()] = *it;
      for (const Tessellation::Link& nid : it.links())
      {
        ShapeMap::iterator it2 = m.find(nid.first);
        if (it2 != m.end())
          check_pair(m[it.index()].polygon(), it2->second.polygon());
      }
    }
  }

}

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Implementation

BOOST_AUTO_TEST_SUITE(tiling_ratios)

BOOST_AUTO_TEST_CASE(TRIANGULAR_TILING)
{
  check_tiling(mml::tiling::TRIANGULAR);
}

BOOST_AUTO_TEST_CASE(SQUARE_TILING)
{
  check_tiling(mml::tiling::SQUARE);
}

BOOST_AUTO_TEST_CASE(HEXAGONAL_TILING)
{
  check_tiling(mml::tiling::HEXAGONAL);
}

BOOST_AUTO_TEST_CASE(SNUB_HEXAGONAL_TILING)
{
  check_tiling(mml::tiling::SNUB_HEXAGONAL);
}

BOOST_AUTO_TEST_CASE(REFLECTED_SNUB_HEXAGONAL_TILING)
{
  check_tiling(mml::tiling::REFLECTED_SNUB_HEXAGONAL);
}

BOOST_AUTO_TEST_CASE(TRI_HEXAGONAL_TILING)
{
  check_tiling(mml::tiling::TRI_HEXAGONAL);
}

BOOST_AUTO_TEST_CASE(ELONGATED_TRIANGULAR_TILING)
{
  check_tiling(mml::tiling::ELONGATED_TRIANGULAR);
}

BOOST_AUTO_TEST_CASE(SNUB_SQUARE_TILING)
{
  check_tiling(mml::tiling::SNUB_SQUARE);
}

BOOST_AUTO_TEST_CASE(RHOMBITRIHEXAGONAL_TILING)
{
  check_tiling(mml::tiling::RHOMBITRIHEXAGONAL);
}

BOOST_AUTO_TEST_CASE(TRUNCATED_SQUARE_TILING)
{
  check_tiling(mml::tiling::TRUNCATED_SQUARE);
}

BOOST_AUTO_TEST_CASE(TRUNCATED_HEXAGONAL_TILING)
{
  check_tiling(mml::tiling::TRUNCATED_HEXAGONAL);
}

BOOST_AUTO_TEST_CASE(TRUNCATED_TRIHEXAGONAL_TILING)
{
  check_tiling(mml::tiling::TRUNCATED_TRIHEXAGONAL);
}

BOOST_AUTO_TEST_SUITE_END()


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SIGNIFICANTS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SIGNIFICANTS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/divides.hpp>
#include <boost/simd/function/simd/iceil.hpp>
#include <boost/simd/function/simd/if_zero_else.hpp>
#include <boost/simd/function/simd/is_eqz.hpp>
#include <boost/simd/function/simd/is_gtz.hpp>
#include <boost/simd/function/simd/log10.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/round.hpp>
#include <boost/simd/function/simd/tenpower.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_invalid.hpp>
#endif

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

//   WARNING AN "IMPLEMENT_IF" WAS PRESENT IN THE ORIGINAL FILE

   BOOST_DISPATCH_OVERLOAD( significants_
                          , (typename A0, typename A1, typename X)
                          , bd::cpu_
                          , bs::pack_< bd::floating_<A0>, X>
                          , bs::pack_< bd::integer_<A1>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const  A1&  a1) const BOOST_NOEXCEPT
      {
        BOOST_ASSERT_MSG( assert_all(is_gtz(a1))
                        , "Number of significant digits must be positive"
                        );
        using iA0 =  bd::as_integer_t<A0>;
        iA0 exp = a1 - iceil(log10(abs(a0)));
        A0 fac = tenpower(exp);
        A0 scaled = round(a0*fac);
  #ifndef BOOST_SIMD_NO_INVALIDS
        A0 r = if_else(is_invalid(a0), a0, scaled/fac);
  #else
        A0 r =  scaled/fac;
  #endif
        return if_zero_else(is_eqz(a0), r);
      }
   };

} } }

#endif