/*
 * 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


#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 */


/*****************************************************************************
*
* 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 );
// }


//
//  geometry.cpp
//  autd3
//
//  Created by Seki Inoue on 6/8/16.
//
//

#include <stdio.h>
#include <map>
#include <Eigen/Geometry>
#include "autd3.hpp"
#include "privdef.hpp"

class Device {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Device(int device_id, Eigen::Vector3f position, Eigen::Vector3f euler_angles)
    : device_id(device_id), position(position), euler_angles(euler_angles) {
        Eigen::Quaternionf quo =
            Eigen::AngleAxisf(euler_angles.x(), Eigen::Vector3f::UnitZ()) *
            Eigen::AngleAxisf(euler_angles.y(), Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(euler_angles.z(), Eigen::Vector3f::UnitZ());

        transform_matrix = Eigen::Translation3f(position) * quo;
        z_direction = quo * Eigen::Vector3f(0, 0, 1);

        int index = 0;
        for (int y = 0; y < NUM_TRANS_Y; y++)
            for (int x = 0; x < NUM_TRANS_X; x++)
                if (!IS_MISSING_TRANSDUCER(x, y))
                    local_trans_positions.col(index++) = Eigen::Vector3f(x*TRANS_SIZE, y*TRANS_SIZE, 0);

        global_trans_positions = transform_matrix * local_trans_positions;
    };
    int device_id;
    Eigen::Vector3f position;
    Eigen::Matrix<float, 3, NUM_TRANS_IN_UNIT> local_trans_positions;
    Eigen::Matrix<float, 3, NUM_TRANS_IN_UNIT> global_trans_positions;
    Eigen::Vector3f euler_angles;
    Eigen::Vector3f z_direction;
    Eigen::Affine3f transform_matrix;
};

class autd::Geometry::impl {
public:
    std::vector<std::shared_ptr<Device> > devices;
    std::shared_ptr<Device> device(int transducer_id) {
        int eid = transducer_id/NUM_TRANS_IN_UNIT;
        return this->devices[eid];
    }
};

autd::GeometryPtr autd::Geometry::Create() {
    return GeometryPtr(new Geometry);
}

autd::Geometry::Geometry() {
    this->_pimpl = std::unique_ptr<impl>(new impl());
}

autd::Geometry::~Geometry() {

}

int autd::Geometry::AddDevice(Eigen::Vector3f position, Eigen::Vector3f euler_angles) {
    int device_id = this->_pimpl->devices.size();
    this->_pimpl->devices.push_back(std::shared_ptr<Device>(new Device(device_id, position, euler_angles)));
    return device_id;
}

void autd::Geometry::DelDevice(int device_id) {
    auto itr = this->_pimpl->devices.begin();
    while (itr != this->_pimpl->devices.end())
    {
        if((*itr)->device_id == device_id) itr = this->_pimpl->devices.erase(itr);
        else itr++;
    }
}

const int autd::Geometry::numDevices() {
    return this->_pimpl->devices.size();
}

const int autd::Geometry::numTransducers() {
    return this->numDevices()*NUM_TRANS_IN_UNIT;
}

const Eigen::Vector3f autd::Geometry::position(int transducer_id) {
    const int local_trans_id = transducer_id%NUM_TRANS_IN_UNIT;
    auto device = this->_pimpl->device(transducer_id);
    return device->global_trans_positions.col(local_trans_id);
}

const Eigen::Vector3f &autd::Geometry::direction(int transducer_id) {
    return this->_pimpl->devices[this->deviceIdForTransIdx(transducer_id)]->z_direction;
}

const int autd::Geometry::deviceIdForDeviceIdx(int device_idx) {
    return this->_pimpl->devices[device_idx]->device_id;
}

const int autd::Geometry::deviceIdForTransIdx(int transducer_id) {
    return this->_pimpl->device(transducer_id)->device_id;
}

float  autd::Geometry::frequency() {
    return FPGA_CLOCK / (640.0 + this->_freq_shift);
}

void   autd::Geometry::SetFrequency(float freq) {
    this->_freq_shift = std::min(std::max(FPGA_CLOCK / freq - 640, (float)std::numeric_limits<int8_t>::min()), (float)std::numeric_limits<int8_t>::max());
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

/// \file
/// Declares function RootFinder::toms748

#pragma once

#include <boost/math/tools/roots.hpp>
#include <functional>
#include <limits>

#include "DataStructures/DataVector.hpp"
#include "ErrorHandling/Exceptions.hpp"

namespace RootFinder {

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Finds the root of the function `f` with the TOMS_748 method.
 *
 * `f` is a unary invokable that takes a `double` which is the current value at
 * which to evaluate `f`. An example is below.
 *
 * \snippet Test_TOMS748.cpp double_root_find
 *
 * The TOMS_748 algorithm searches for a root in the interval [`lower_bound`,
 * `upper_bound`], and will throw if this interval does not bracket a root,
 * i.e. if `f(lower_bound) * f(upper_bound) > 0`.
 *
 * The arguments `f_at_lower_bound` and `f_at_upper_bound` are optional, and
 * are the function values at `lower_bound` and `upper_bound`. These function
 * values are often known because the user typically checks if a root is
 * bracketed before calling `toms748`; passing the function values here saves
 * two function evaluations.
 *
 * See the [Boost](http://www.boost.org/) documentation for more details.
 *
 * \requires Function `f` is invokable with a `double`
 *
 * \throws `std::domain_error` if the bounds do not bracket a root.
 * \throws `convergence_error` if the requested tolerance is not met after
 *                            `max_iterations` iterations.
 */
template <typename Function>
double toms748(const Function& f, const double lower_bound,
               const double upper_bound, const double f_at_lower_bound,
               const double f_at_upper_bound, const double absolute_tolerance,
               const double relative_tolerance,
               const size_t max_iterations = 100) {
  ASSERT(relative_tolerance > std::numeric_limits<double>::epsilon(),
         "The relative tolerance is too small.");

  boost::uintmax_t max_iters = max_iterations;

  // This solver requires tol to be passed as a termination condition. This
  // termination condition is equivalent to the convergence criteria used by the
  // GSL
  auto tol = [absolute_tolerance, relative_tolerance](double lhs, double rhs) {
    return (fabs(lhs - rhs) <=
            absolute_tolerance +
                relative_tolerance * fmin(fabs(lhs), fabs(rhs)));
  };
  // clang-tidy: internal boost warning, can't fix it.
  auto result = boost::math::tools::toms748_solve(  // NOLINT
      f, lower_bound, upper_bound, f_at_lower_bound, f_at_upper_bound, tol,
      max_iters);
  if (max_iters >= max_iterations) {
    throw convergence_error(
        "toms748 reached max iterations without converging");
  }
  return result.first + 0.5 * (result.second - result.first);
}

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Finds the root of the function `f` with the TOMS_748 method,
 * where function values are not supplied at the lower and upper
 * bounds.
 */
template <typename Function>
double toms748(const Function& f, const double lower_bound,
               const double upper_bound, const double absolute_tolerance,
               const double relative_tolerance,
               const size_t max_iterations = 100) {
  return toms748(f, lower_bound, upper_bound, f(lower_bound), f(upper_bound),
                 absolute_tolerance, relative_tolerance, max_iterations);
}

namespace detail {
template <typename Function>
DataVector toms748_impl(const Function& f, const DataVector& lower_bound,
                        const DataVector& upper_bound,
                        const DataVector& f_at_lower_bound,
                        const DataVector& f_at_upper_bound,
                        const double absolute_tolerance,
                        const double relative_tolerance,
                        const size_t max_iterations,
                        const bool function_values_are_supplied) {
  ASSERT(relative_tolerance > std::numeric_limits<double>::epsilon(),
         "The relative tolerance is too small.");
  // This solver requires tol to be passed as a termination condition. This
  // termination condition is equivalent to the convergence criteria used by the
  // GSL
  auto tol = [absolute_tolerance, relative_tolerance](const double lhs,
                                                      const double rhs) {
    return (fabs(lhs - rhs) <=
            absolute_tolerance +
                relative_tolerance * fmin(fabs(lhs), fabs(rhs)));
  };
  DataVector result_vector{lower_bound.size()};
  for (size_t i = 0; i < result_vector.size(); ++i) {
    // toms748_solver modifies the max_iters after the root is found to the
    // number of iterations that it took to find the root, so we reset it to
    // max_iterations after each root find.
    boost::uintmax_t max_iters = max_iterations;
    auto result = function_values_are_supplied
                      ?
                      // clang-tidy: internal boost warning, can't fix it.
                      boost::math::tools::toms748_solve(  // NOLINT
                          [&f, i](double x) { return f(x, i); }, lower_bound[i],
                          upper_bound[i], f_at_lower_bound[i],
                          f_at_upper_bound[i], tol, max_iters)
                      :
                      // clang-tidy: internal boost warning, can't fix it.
                      boost::math::tools::toms748_solve(  // NOLINT
                          [&f, i](double x) { return f(x, i); }, lower_bound[i],
                          upper_bound[i], tol, max_iters);
    if (max_iters >= max_iterations) {
      throw convergence_error(
          "toms748 reached max iterations without converging");
    }
    result_vector[i] = result.first + 0.5 * (result.second - result.first);
  }
  return result_vector;
}
}  // namespace detail

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Finds the root of the function `f` with the TOMS_748 method on each
 * element in a `DataVector`.
 *
 * `f` is a binary invokable that takes a `double` as its first argument and a
 * `size_t` as its second. The `double` is the current value at which to
 * evaluate `f`, and the `size_t` is the current index into the `DataVector`s.
 * Below is an example of how to root find different functions by indexing into
 * a lambda-captured `DataVector` using the `size_t` passed to `f`.
 *
 * \snippet Test_TOMS748.cpp datavector_root_find
 *
 * For each index `i` into the DataVector, the TOMS_748 algorithm searches for a
 * root in the interval [`lower_bound[i]`, `upper_bound[i]`], and will throw if
 * this interval does not bracket a root,
 * i.e. if `f(lower_bound[i], i) * f(upper_bound[i], i) > 0`.
 *
 * See the [Boost](http://www.boost.org/) documentation for more details.
 *
 * \requires Function `f` be callable with a `double` and a `size_t`
 *
 * \throws `std::domain_error` if, for any index, the bounds do not bracket a
 * root.
 * \throws `convergence_error` if, for any index, the requested tolerance is not
 * met after `max_iterations` iterations.
 */
template <typename Function>
DataVector toms748(const Function& f, const DataVector& lower_bound,
                   const DataVector& upper_bound,
                   const double absolute_tolerance,
                   const double relative_tolerance,
                   const size_t max_iterations = 100) {
  return detail::toms748_impl(f, lower_bound, upper_bound, DataVector{},
                              DataVector{}, absolute_tolerance,
                              relative_tolerance, max_iterations, false);
}

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Finds the root of the function `f` with the TOMS_748 method on each
 * element in a `DataVector`, where function values are supplied at the lower
 * and upper bounds.
 *
 * Supplying function values is an optimization that saves two
 * function calls per point.  The function values are often available
 * because one often checks if the root is bracketed before calling `toms748`.
 */
template <typename Function>
DataVector toms748(const Function& f, const DataVector& lower_bound,
                   const DataVector& upper_bound,
                   const DataVector& f_at_lower_bound,
                   const DataVector& f_at_upper_bound,
                   const double absolute_tolerance,
                   const double relative_tolerance,
                   const size_t max_iterations = 100) {
  return detail::toms748_impl(f, lower_bound, upper_bound, f_at_lower_bound,
                              f_at_upper_bound, absolute_tolerance,
                              relative_tolerance, max_iterations, true);
}

}  // namespace RootFinder


/**
 * Created on Tue Jan 04 2022
 *
 * Shane Flandermeyer, shaneflandermeyer@gmail.com
 *
 * A simple example usage of the CFAR detection object when the input is
 * gaussian noise with a time-varying variance. This example is adapted from
 * https://www.mathworks.com/help/phased/ug/constant-false-alarm-rate-cfar-detection.html
 */

#include "cfar1d.h"
#include "matrix_utils.h"

#include <random>
#include <Eigen/Dense>
#include <matplot/matplot.h>

using namespace plasma;
using namespace matplot;
using namespace Eigen;
int main() {

  //! CFAR parameters
  auto method = "CA";
  size_t num_guard_cells = 2;
  size_t num_train_cells = 200;
  auto pfa = 1e-3;
  CFARDetector cfar{num_train_cells, num_guard_cells, pfa};

  //! Generate data
  // The input data is a square-law input signal with increasing noise power
  auto noise_power = pow(10, -10 / 10);
  size_t num_points = 10e3;
  // std::default_random_engine gen;
  MatrixXcd rsamp(num_points, 1);
  MatrixXd x(rsamp.rows(), rsamp.cols());
  // Start with samples from a complex standard normal distribution
  std::mt19937 gen{1000};
  std::normal_distribution<> normal{0, 1};
  for (size_t i = 0; i < num_points; ++i)
    rsamp(i) = std::complex<double>(normal(gen), normal(gen));
  // Create the square-law input signal
  ArrayXd ramp = ArrayXd::LinSpaced(num_points, 1, 10);
  x = abs2(sqrt(noise_power * ramp / 2) * rsamp.array()).matrix();
  // x = hcat(x,x);

  //! Write to a file
  // std::vector<double> filevec(x.data(), x.data() + x.size());
  // write_binary<double>("/home/shane/bin.dat",filevec);

  //! Do CFAR
  DetectionReport det = cfar.detect(x);

  //! Figures
  // Input data
  std::vector<double> xvec(x.data(), x.data() + x.size());
  std::vector<double> threshvec(det.threshold.data(),
                                det.threshold.data() + det.threshold.size());
  std::vector<size_t> indices(det.indices.col(0).data(),
                              det.indices.col(0).data() +
                                  det.indices.col(0).size());
  std::vector<double> detvec(det.indices.size());
  for (size_t i = 0; i < det.indices.size(); ++i)
    detvec[i] = x(det.indices(i));

  std::cout << "Number of Detections: " << det.num_detections << std::endl;

  plot(xvec);
  hold(true);
  plot(threshvec);
  plot(indices, detvec, "o");
  hold(false);
  show();
  return 0;
}


#include <iostream>

#include <unistd.h>
#include <cuda.h>
#include <cuda_runtime_api.h>

#include <blitz.h>

using namespace blitz;
// M K
Shape left_shape(2);
// K N
Shape right_shape(2);
// M N
Shape output_shape(2);

void compare_cpu_gpu(size_t size, float* output_cpu, float* output_gpu) {
  for (size_t i = 0; i < size; ++i) {
    if (output_cpu[i] > output_gpu[i] + 1e-3 ||
      output_cpu[i] < output_gpu[i] - 1e-3) {
      std::cout << "Index: " << i << ", CPU: " << output_cpu[i] <<
        ", GPU: " << output_gpu[i] << std::endl;
    }
  }
}

void multiply(size_t m, size_t n, size_t k, BLITZ_ALGORITHM algorithm, string trans) {
  bool left_trans, right_trans;
  // set shapes
  if (trans == "nn") {
    left_trans = false;
    right_trans = false;
    left_shape[0] = m;
    left_shape[1] = k;
    right_shape[0] = k;
    right_shape[1] = n;
    output_shape[0] = m;
    output_shape[1] = n;
  } else if (trans == "tn") {
    left_trans = true;
    right_trans = false;
    left_shape[0] = k;
    left_shape[1] = m;
    right_shape[0] = k;
    right_shape[1] = n;
    output_shape[0] = m;
    output_shape[1] = n;
  } else if (trans == "nt") {
    left_trans = false;
    right_trans = true;
    left_shape[0] = m;
    left_shape[1] = k;
    right_shape[0] = n;
    right_shape[1] = k;
    output_shape[0] = m;
    output_shape[1] = n;
  } else {
    std::cerr << "No such transform method!" << std::endl;
    exit(1);
  }
  // set up cpu
  CPUTensor<float> left_cpu(left_shape);
  CPUTensor<float> right_cpu(right_shape);
  CPUTensor<float> output_cpu(output_shape);
  // set up gpu
  GPUTensor<float> left_gpu(left_shape);
  GPUTensor<float> right_gpu(right_shape);
  GPUTensor<float> output_gpu(output_shape);
  CPUTensor<float> output_copy(output_shape);
  // init values
  Backend<CPUTensor, float>::UniformDistributionFunc(&left_cpu, 0.0, 1.0);
  Backend<CPUTensor, float>::UniformDistributionFunc(&right_cpu, 0.0, 1.0);
  cudaMemcpy(left_gpu.data(), left_cpu.data(),
    left_cpu.size() * sizeof(float), cudaMemcpyHostToDevice);
  cudaMemcpy(right_gpu.data(), right_cpu.data(),
    right_cpu.size() * sizeof(float), cudaMemcpyHostToDevice);
  // transpose
  Backend<CPUTensor, float>::MatrixMultiplyFunc(&left_cpu, &right_cpu, &output_cpu, left_trans, right_trans, 1.0, 0.0, algorithm);
  Backend<GPUTensor, float>::MatrixMultiplyFunc(&left_gpu, &right_gpu, &output_gpu, left_trans, right_trans, 1.0, 0.0, algorithm);
  // copy from gpu to cpu
  cudaMemcpy(output_copy.data(), output_gpu.data(),
    output_gpu.size() * sizeof(float), cudaMemcpyDeviceToHost);
  compare_cpu_gpu(output_cpu.size(), output_cpu.data(), output_copy.data());
}

int main(int argc, char** argv) {
  const size_t NUM_ARGS = 5;
  // M N K
  if (argc != NUM_ARGS + 1) {
    std::cerr << "Not matchable args!" << std::endl;
    exit(1);
  }
  std::string kernel = std::string(argv[1]);
  std::string trans = std::string(argv[2]);
  const size_t M = atoi(argv[3]);
  const size_t N = atoi(argv[4]);
  const size_t K = atoi(argv[5]);
  // run
  multiply(M, N, K, BlitzParseAlgorithm(kernel), trans);
  return 0;
}


// -----------------------------------------------------------------------------
// Filename:    Coord_Process.hpp
// Revision:    None
// Date:        2018/09/24 - 10:10
// Author:      Haixiang HOU
// Email:       hexid26@outlook.com
// Website:     [NULL]
// Notes:       [NULL]
// -----------------------------------------------------------------------------
// Copyright:   2018 (c) Haixiang
// License:     GPL
// -----------------------------------------------------------------------------
// Version [1.0]
// Calculate the coordinates of senders/receivers and sample points.

#include <boost/asio.hpp>
#include <boost/atomic.hpp>
#include <boost/thread.hpp>
#include <chrono>
#include <cmath>
#include <fstream>
#include <iostream>

#include "define.hpp"

class Coord_Process {
  private:
    boost::atomic_int saved_files;
    float dis_farest_point;                            // 成像点到圆心的距离
    unsigned long long pre_malloc_points_per_triangle; // 预先用来保存单个三角形点坐标的空间大小
    uint16_t point_sum_per_line;                       // 成像点每行的个数，图为正方形
    float coord_step;                                  // 当前成像分辨率下坐标标准方向上的步进
    void initialize(uint16_t num);
    void calc_ele_coords();
    void calc_all_triangles();
    inline float change_coord_to_int(float num, float step);
    void calc_sample_points_coords_by_ele_snd_id(uint16_t triangle_id);
    void save_sample_points_in_triangle(uint16_t ele_snd_id);

  public:
    float ele_coord_array_x[ELE_NO], ele_coord_array_y[ELE_NO]; //振元坐标
    float *triangle_vertex_x, *triangle_vertex_y;               // 所有三角形顶点坐标
    unsigned long long temp_sample_points_coords_cnt[ELE_NO];        //每个发射源扫描区的点数
    uint16_t **temp_sample_points_coords;              // 二维数组[ELE_NO][采样点数]
    void calc_all_sample_points_coords(int thread_sum);         // 多线程计算所有扫描区中，点的坐标
    void save_all_triangles(int thread_sum);                    // 多线程保存文件
    void print_ele_coords();
    void print_triangles_vertex();
    void print_sample_points_info();
    Coord_Process(uint16_t num);
    ~Coord_Process();
};

Coord_Process::Coord_Process(uint16_t num) { initialize(num); }

// num 是成像分辨率单维点数
void Coord_Process::initialize(uint16_t num) {
    dis_farest_point = RADIUS - 0.01; // 距离探头 10mm 的不成像
    point_sum_per_line = num;
    coord_step = IMAGE_WIDTH / (point_sum_per_line - 1);
    pre_malloc_points_per_triangle =
        (unsigned long long)(num * num * RADIUS * RADIUS * tan(PI / 18) / IMAGE_WIDTH / IMAGE_WIDTH);
    printf("Points per triangle: %llu\nPre malloc memory: %.2fGB\n", pre_malloc_points_per_triangle,
           pre_malloc_points_per_triangle * ELE_NO * 2 * 2 / 1024.0 / 1024.0 / 1024.0);
    calc_ele_coords();
    triangle_vertex_x = (float *)calloc(3 * sizeof(float), ELE_NO);
    triangle_vertex_y = (float *)calloc(3 * sizeof(float), ELE_NO);
    temp_sample_points_coords = new uint16_t *[ELE_NO];
    for (uint16_t ele_snd_id = 0; ele_snd_id < ELE_NO; ele_snd_id++) {
        temp_sample_points_coords[ele_snd_id] =
            (uint16_t *)calloc(2 * sizeof(uint16_t), pre_malloc_points_per_triangle);
    }
    calc_all_triangles();
}

Coord_Process::~Coord_Process() {
    free(triangle_vertex_x);
    free(triangle_vertex_y);
    for (uint16_t index = 0; index < point_sum_per_line; index++) {
        free(temp_sample_points_coords[index]);
    }
}

// 计算所有发射源的坐标
void Coord_Process::calc_ele_coords() {
    float raduis = RADIUS;
    float ele_angle_offset = (2 * PI * 43.4695 / (256 - 1)) / 360; //阵元间隔角度
    float start_ele_angle = 2 * PI * (45 - 43.4695) / 360;         //第一个阵元角度
    /* for (int start_ele_id = 0; start_ele_id < 256; start_ele_id++) {
        ele_coord_array_x[start_ele_id] =
            raduis * cos(start_ele_angle + start_ele_id * ele_angle_offset);
        ele_coord_array_y[start_ele_id] =
            -raduis * sin(start_ele_angle + start_ele_id * ele_angle_offset);
    } */
    // 把上面的 for 循环融入到下面会产生误差，但是上面的结果不符合可90度旋转原理
    for (int start_ele_id = 0; start_ele_id < ELE_NO; start_ele_id++) {
        ele_coord_array_x[start_ele_id] = raduis * cos(start_ele_angle + (start_ele_id % 256) * ele_angle_offset +
                                                       (int)(start_ele_id / 256) * PI / 4);
        ele_coord_array_y[start_ele_id] = -raduis * sin(start_ele_angle + (start_ele_id % 256) * ele_angle_offset +
                                                        (int)(start_ele_id / 256) * PI / 4);
    }
}

// 计算所有发射源扫描区(三角形)的三点坐标
void Coord_Process::calc_all_triangles() {
    float radius = RADIUS;
    float temp_point[3][2];   // 3个点，每个点2个坐标值
    uint16_t high_id, low_id; // 最高点和最低点的 id
    for (uint16_t start_ele_id = 0; start_ele_id < ELE_NO; start_ele_id++) {
        temp_point[0][0] = ele_coord_array_x[start_ele_id];
        temp_point[0][1] = ele_coord_array_y[start_ele_id];
        temp_point[1][0] = sin(PI / 18) * radius *
                           sqrtf(ele_coord_array_y[start_ele_id] * ele_coord_array_y[start_ele_id] / (radius * radius));
        temp_point[1][1] = -(temp_point[0][0] * temp_point[1][0]) / temp_point[0][1];
        temp_point[2][0] = -temp_point[1][0];
        temp_point[2][1] = -temp_point[1][1];
        // 排序

        for (uint16_t index = 0; index < 3; index++) {
            if (temp_point[index][1] == std::max(std::max(temp_point[0][1], temp_point[1][1]), temp_point[2][1])) {
                high_id = index;
            }
        }
        for (uint16_t index = 0; index < 3; index++) {
            if (temp_point[index][1] == std::min(std::min(temp_point[0][1], temp_point[1][1]), temp_point[2][1])) {
                low_id = index;
            }
        }

        triangle_vertex_x[3 * start_ele_id] = temp_point[high_id][0];
        triangle_vertex_y[3 * start_ele_id] = temp_point[high_id][1];
        triangle_vertex_x[3 * start_ele_id + 1] = temp_point[3 - high_id - low_id][0];
        triangle_vertex_y[3 * start_ele_id + 1] = temp_point[3 - high_id - low_id][1];
        triangle_vertex_x[3 * start_ele_id + 2] = temp_point[low_id][0];
        triangle_vertex_y[3 * start_ele_id + 2] = temp_point[low_id][1];
    }
    return;
}

// 把三角形三点坐标转移到整数空间
inline float Coord_Process::change_coord_to_int(float num, float step) {
    return (num + (point_sum_per_line - 1) / 2 * step) / step;
}

// 扫描单个三角形中的点
void Coord_Process::calc_sample_points_coords_by_ele_snd_id(uint16_t triangle_id) {
    float high_point_x = triangle_vertex_x[3 * triangle_id];
    float mid_point_x = triangle_vertex_x[3 * triangle_id + 1];
    float low_point_x = triangle_vertex_x[3 * triangle_id + 2];
    float high_point_y = triangle_vertex_y[3 * triangle_id];
    float mid_point_y = triangle_vertex_y[3 * triangle_id + 1];
    float low_point_y = triangle_vertex_y[3 * triangle_id + 2];
    high_point_x = change_coord_to_int(high_point_x, coord_step);
    mid_point_x = change_coord_to_int(mid_point_x, coord_step);
    low_point_x = change_coord_to_int(low_point_x, coord_step);
    high_point_y = change_coord_to_int(high_point_y, coord_step);
    mid_point_y = change_coord_to_int(mid_point_y, coord_step);
    low_point_y = change_coord_to_int(low_point_y, coord_step);

    unsigned long long sample_points_sum = 0;

    float temp1, temp2;
    int min_horizontal_id, max_horizontal_id;
    for (int row_id = std::max((int)(low_point_y + 1), 0); row_id <= std::min((int)mid_point_y, point_sum_per_line - 1);
         row_id++) {
        temp1 = ((float)row_id - low_point_y) * (mid_point_x - low_point_x) / (mid_point_y - low_point_y) + low_point_x;
        temp2 =
            ((float)row_id - low_point_y) * (high_point_x - low_point_x) / (high_point_y - low_point_y) + low_point_x;
        min_horizontal_id = (int)(std::min(temp1, temp2) + 1);
        max_horizontal_id = (int)(std::max(temp1, temp2));
        min_horizontal_id = std::max(min_horizontal_id, 0);
        min_horizontal_id = std::min(min_horizontal_id, point_sum_per_line - 1);
        max_horizontal_id = std::min(max_horizontal_id, point_sum_per_line - 1);
        max_horizontal_id = std::max(max_horizontal_id, 0);
        for (uint16_t index = min_horizontal_id; index <= max_horizontal_id; index++) {
            if (sqrtf(((float)index - point_sum_per_line / 2) * ((float)index - point_sum_per_line / 2) +
                      ((float)row_id - point_sum_per_line / 2) * ((float)row_id - point_sum_per_line / 2)) *
                    coord_step >
                dis_farest_point) {
                continue;
            }
            temp_sample_points_coords[triangle_id][2 * sample_points_sum] = (uint16_t)index;
            temp_sample_points_coords[triangle_id][2 * sample_points_sum + 1] = (uint16_t)row_id;
            sample_points_sum++;
        }
    }

    for (int row_id = std::max((int)(mid_point_y + 1), 0);
         row_id <= std::min((int)high_point_y, point_sum_per_line - 1); row_id++) {
        temp1 =
            ((float)row_id - high_point_y) * (high_point_x - mid_point_x) / (high_point_y - mid_point_y) + high_point_x;
        temp2 =
            ((float)row_id - low_point_y) * (high_point_x - low_point_x) / (high_point_y - low_point_y) + low_point_x;
        min_horizontal_id = (int)(std::min(temp1, temp2) + 1);
        max_horizontal_id = (int)(std::max(temp1, temp2));
        min_horizontal_id = std::max(min_horizontal_id, 0);
        min_horizontal_id = std::min(min_horizontal_id, point_sum_per_line - 1);
        max_horizontal_id = std::min(max_horizontal_id, point_sum_per_line - 1);
        max_horizontal_id = std::max(max_horizontal_id, 0);
        for (uint16_t index = min_horizontal_id; index <= max_horizontal_id; index++) {
            if (sqrtf(((float)index - point_sum_per_line / 2.0) * ((float)index - point_sum_per_line / 2.0) +
                      ((float)row_id - point_sum_per_line / 2.0) * ((float)row_id - point_sum_per_line / 2.0)) *
                    coord_step >
                dis_farest_point) {
                continue;
            }
            temp_sample_points_coords[triangle_id][2 * sample_points_sum] = (uint16_t)index;
            temp_sample_points_coords[triangle_id][2 * sample_points_sum + 1] = (uint16_t)row_id;
            sample_points_sum++;
        }
    }
    temp_sample_points_coords_cnt[triangle_id] = sample_points_sum;
}

// 多线程扫描所有的(2048)三角形，thread_sum 通过进程参数设置
void Coord_Process::calc_all_sample_points_coords(int thread_sum) {
    boost::asio::thread_pool thread_pool_calc(thread_sum);
    for (uint16_t ele_snd_id = 0; ele_snd_id < ELE_NO; ele_snd_id++) {
        boost::asio::post(thread_pool_calc,
                          boost::bind(&Coord_Process::calc_sample_points_coords_by_ele_snd_id, this, ele_snd_id));
    }
    thread_pool_calc.join();

    // for (uint16_t ele_snd_id = 0; ele_snd_id < ELE_NO; ele_snd_id++) {
    //     calc_sample_points_coords_by_ele_snd_id(ele_snd_id);
    // }
}

// 打印所有探头的坐标
void Coord_Process::print_ele_coords() {
    for (uint16_t index = 0; index < ELE_NO; index++) {
        printf("[%04d] = %10.8f, %10.8f; [%04d] = %10.8f, %10.8f; [%04d] = %10.8f, %10.8f; [%04d] = %10.8f, %10.8f;\n",
               index, ele_coord_array_x[index], ele_coord_array_y[index], index + ELE_NO / 4,
               ele_coord_array_x[index + ELE_NO / 4], ele_coord_array_y[index + ELE_NO / 4], index + ELE_NO * 2 / 4,
               ele_coord_array_x[index + ELE_NO * 2 / 4], ele_coord_array_y[index + ELE_NO * 2 / 4],
               index + ELE_NO * 3 / 4, ele_coord_array_x[index + ELE_NO * 3 / 4],
               ele_coord_array_y[index + ELE_NO * 3 / 4]);
    }
    return;
}

void Coord_Process::print_triangles_vertex() {
    for (uint16_t triangle_id = 0; triangle_id < ELE_NO; triangle_id++) {
        if (triangle_id % 64 == 0) {
            printf(
                "[[%10.8f, %10.8f, %10.8f, %10.8f],[%10.8f, %10.8f, %10.8f, "
                "%10.8f]]\n",
                triangle_vertex_x[3 * triangle_id], triangle_vertex_x[3 * triangle_id + 1],
                triangle_vertex_x[3 * triangle_id + 2], triangle_vertex_x[3 * triangle_id],
                triangle_vertex_y[3 * triangle_id], triangle_vertex_y[3 * triangle_id + 1],
                triangle_vertex_y[3 * triangle_id + 2], triangle_vertex_y[3 * triangle_id]);
        }
    }
}

void Coord_Process::print_sample_points_info() {
    unsigned long long point_sum = 0;
    for (uint16_t index = 0; index < ELE_NO; index++) {
        printf("Tri_%04u :: Sample point sum = %llu\n", index, temp_sample_points_coords_cnt[index]);
        point_sum += temp_sample_points_coords_cnt[index];
    }
}

void Coord_Process::save_sample_points_in_triangle(uint16_t ele_snd_id) {
    char *file_name = new char[17];
    sprintf(file_name, "Tri/Tri_%04d.txt", ele_snd_id);
    std::ofstream f_stream(file_name, std::fstream::out);
    std::string content = "";
    for (unsigned long long index = 0; index < temp_sample_points_coords_cnt[ele_snd_id]; index++) {
        content += std::to_string(temp_sample_points_coords[ele_snd_id][2 * index]) + "," +
                   std::to_string(temp_sample_points_coords[ele_snd_id][2 * index + 1]) + "\n";
    }
    f_stream << content;
    f_stream.close();
    saved_files++;
}

void Coord_Process::save_all_triangles(int thread_sum) {
    boost::asio::thread_pool thread_pool_save(40);
    saved_files = 0;
    for (uint16_t ele_snd_id = 0; ele_snd_id < ELE_NO; ele_snd_id++) {
        boost::asio::post(thread_pool_save,
                          boost::bind(&Coord_Process::save_sample_points_in_triangle, this, ele_snd_id));
    }
    while (saved_files < ELE_NO) {
        printf("\r%06.3f%%", (float)saved_files / ELE_NO * 100);
        fflush(stdout);
        sleep(1);
    }
    printf("\r%06.3f%%\n", (float)saved_files / ELE_NO * 100);
}


#include <chrono>
#include <iomanip>
#include <thread>
#include <eigen3/Eigen/Dense>

#include "common/plugin.hpp"
#include "common/switchboard.hpp"
#include "common/data_format.hpp"
#include "common/threadloop.hpp"

#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/navigation/AHRSFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>  // Used if IMU combined is off.
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h>
#include <boost/smart_ptr/shared_ptr.hpp>
#include <boost/smart_ptr/make_shared.hpp>

// IMU sample time to live in seconds
#define IMU_TTL 5

using PimUniquePtr = std::unique_ptr<gtsam::PreintegrationType>;
using ImuBias = gtsam::imuBias::ConstantBias;
using namespace ILLIXR;

typedef struct {
	double timestamp;
	Eigen::Matrix<double, 3, 1> wm;
	Eigen::Matrix<double, 3, 1> am;
} imu_type;

class imu_integrator : public threadloop {
public:
	imu_integrator(std::string name_, phonebook* pb_)
		: threadloop{name_, pb_}
		, sb{pb->lookup_impl<switchboard>()}
		, _m_imu_cam{sb->subscribe_latest<imu_cam_type>("imu_cam")}
		, _m_in{sb->subscribe_latest<imu_integrator_seq>("imu_integrator_seq")}
		, _m_imu_integrator_input{sb->subscribe_latest<imu_integrator_input>("imu_integrator_input")}
		, _m_imu_raw{sb->publish<imu_raw_type>("imu_raw")}
		, _seq_expect(1)
	{}

	virtual skip_option _p_should_skip() override {
		auto in = _m_in->get_latest_ro();
		if (!in || in->seq == _seq_expect-1) {
			// No new data, sleep to keep CPU utilization low
			std::this_thread::sleep_for(std::chrono::milliseconds{1});
			return skip_option::skip_and_yield;
		} else {
			if (in->seq != _seq_expect) {
				_stat_missed = in->seq - _seq_expect;
			} else {
				_stat_missed = 0;
			}
			_stat_processed++;
			_seq_expect = in->seq+1;
			return skip_option::run;
		}
	}

	void _p_one_iteration() override {
		const imu_cam_type *datum = _m_imu_cam->get_latest_ro();
		double timestamp_in_seconds = (double(datum->dataset_time) / NANO_SEC);

		imu_type data;
        data.timestamp = timestamp_in_seconds;
        data.wm = (datum->angular_v).cast<double>();
        data.am = (datum->linear_a).cast<double>();
		_imu_vec.emplace_back(data);

		clean_imu_vec(timestamp_in_seconds);
        propagate_imu_values(timestamp_in_seconds, datum->time);
	}

private:
	const std::shared_ptr<switchboard> sb;

	// IMU Data, Sequence Flag, and State Vars Needed
	std::unique_ptr<reader_latest<imu_cam_type>> _m_imu_cam;
	std::unique_ptr<reader_latest<imu_integrator_seq>> _m_in;
	std::unique_ptr<reader_latest<imu_integrator_input>> _m_imu_integrator_input;

	// Write IMU Biases for PP
	std::unique_ptr<writer<imu_raw_type>> _m_imu_raw;

	std::vector<imu_type> _imu_vec;
  	PimUniquePtr pim_ = NULL;

	[[maybe_unused]] double last_cam_time = 0;
	double last_imu_offset = 0;
	long long _seq_expect, _stat_processed, _stat_missed;

	// Remove IMU values older than 'IMU_TTL' from the imu buffer
	void clean_imu_vec(double timestamp) {
		auto imu_iterator = _imu_vec.begin();

		// Since the vector is ordered oldest to latest, keep deleting until you
		// hit a value less than 'IMU_TTL' seconds old
		while (imu_iterator != _imu_vec.end()) {
			if (timestamp-(*imu_iterator).timestamp < IMU_TTL) {
				break;
			}

			imu_iterator = _imu_vec.erase(imu_iterator);
		}
	}

	// Timestamp we are propagating the biases to (new IMU reading time)
	void propagate_imu_values(double timestamp, time_type real_time) {
		const imu_integrator_input *input_values = _m_imu_integrator_input->get_latest_ro();
		if (input_values == NULL) {
			return;
		}

		ImuBias imu_bias = ImuBias(input_values->biasAcc, input_values->biasGyro);
		if (pim_ == NULL) {
			boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> params =
          			boost::make_shared<gtsam::PreintegratedCombinedMeasurements::Params>(input_values->params.n_gravity);

			params->setGyroscopeCovariance(std::pow(input_values->params.gyro_noise, 2.0) * Eigen::Matrix3d::Identity());
  			params->setAccelerometerCovariance(std::pow(input_values->params.acc_noise, 2.0) * Eigen::Matrix3d::Identity());
  			params->setIntegrationCovariance(std::pow(input_values->params.imu_integration_sigma, 2.0) * Eigen::Matrix3d::Identity());
			params->biasAccCovariance = std::pow(input_values->params.acc_walk, 2.0) * Eigen::Matrix3d::Identity();
			params->biasOmegaCovariance = std::pow(input_values->params.gyro_walk, 2.0) * Eigen::Matrix3d::Identity();

			pim_ = std::make_unique<gtsam::PreintegratedCombinedMeasurements>(params, imu_bias);
			last_imu_offset = input_values->t_offset;
		}

#ifndef NDEBUG
		if (input_values->last_cam_integration_time > last_cam_time) {
			std::cout << "New slow pose has arrived!\n";
			last_cam_time = input_values->last_cam_integration_time;
		}
#endif
		pim_->resetIntegrationAndSetBias(imu_bias);

		double time_begin = input_values->last_cam_integration_time + last_imu_offset;
		double time_end = timestamp + input_values->t_offset;

		std::vector<imu_type> prop_data = select_imu_readings(_imu_vec, time_begin, time_end);
		if (prop_data.size() < 2) {
			return;
		}

   		ImuBias prev_bias = pim_->biasHat();
		ImuBias bias = pim_->biasHat();

#ifndef NDEBUG
		std::cout << "Integrating over " << prop_data.size() << " IMU samples\n";
#endif

		for (unsigned i = 0; i < prop_data.size()-1; i++) {
			const gtsam::Vector3& measured_acc = prop_data.at(i).am;
			const gtsam::Vector3& measured_omega = prop_data.at(i).wm;

			// Delta T should be in seconds
			const double& delta_t = prop_data.at(i+1).timestamp - prop_data.at(i).timestamp;
			pim_->integrateMeasurement(measured_acc, measured_omega, delta_t);

			prev_bias = bias;
			bias = pim_->biasHat();
		}

		gtsam::NavState navstate_lkf(gtsam::Pose3(gtsam::Rot3(input_values->quat), input_values->position), input_values->velocity);
		gtsam::NavState navstate_k = pim_->predict(navstate_lkf, imu_bias);
		gtsam::Pose3 out_pose = navstate_k.pose();

#ifndef NDEBUG
		std::cout << "Base Position (x, y, z) = "
				<< input_values->position(0) << ", "
				<< input_values->position(1) << ", "
				<< input_values->position(2) << std::endl;

		std::cout << "New  Position (x, y, z) = "
				<< out_pose.x() << ", "
				<< out_pose.y() << ", "
				<< out_pose.z() << std::endl;
#endif

		_m_imu_raw->put(new imu_raw_type{
			prev_bias.gyroscope(),
			prev_bias.accelerometer(),
			bias.gyroscope(),
			bias.accelerometer(),
			out_pose.translation(), // Position
			navstate_k.velocity(), // Velocity
			out_pose.rotation().toQuaternion(), // Eigen Quat
			real_time
		});
	}

	// Select IMU readings based on timestamp similar to how OpenVINS selects IMU values to propagate
	std::vector<imu_type> select_imu_readings(const std::vector<imu_type>& imu_data, double time_begin, double time_end) {
		std::vector<imu_type> prop_data;
		if (imu_data.size() < 2) {
			return prop_data;
		}

		for (unsigned i = 0; i < imu_data.size()-1; i++) {

			// If time_begin comes inbetween two IMUs (A and B), interpolate A forward to time_begin
			if (imu_data.at(i+1).timestamp > time_begin && imu_data.at(i).timestamp < time_begin) {
				imu_type data = interpolate_imu(imu_data.at(i), imu_data.at(i+1), time_begin);
				prop_data.push_back(data);
				continue;
			}

			// IMU is within time_begin and time_end
			if (imu_data.at(i).timestamp >= time_begin && imu_data.at(i+1).timestamp <= time_end) {
				prop_data.push_back(imu_data.at(i));
				continue;
			}

			// IMU is past time_end
			if (imu_data.at(i+1).timestamp > time_end) {
				imu_type data = interpolate_imu(imu_data.at(i), imu_data.at(i+1), time_end);
				prop_data.push_back(data);
				break;
			}
		}

		// Loop through and ensure we do not have an zero dt values
		// This would cause the noise covariance to be Infinity
		for (size_t i = 0; i < prop_data.size()-1; i++) {
			if (std::abs(prop_data.at(i+1).timestamp-prop_data.at(i).timestamp) < 1e-12) {
				prop_data.erase(prop_data.begin()+i);
				i--;
			}
		}

		return prop_data;
	}

	// For when an integration time ever falls inbetween two imu measurements (modeled after OpenVINS)
	static imu_type interpolate_imu(const imu_type imu_1, imu_type imu_2, double timestamp) {
		imu_type data;
		data.timestamp = timestamp;

		double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp);
		data.am = (1 - lambda) * imu_1.am + lambda * imu_2.am;
		data.wm = (1 - lambda) * imu_1.wm + lambda * imu_2.wm;

		return data;
	}
};

PLUGIN_MAIN(imu_integrator)


#ifndef _MST_HXX_
#define _MST_HXX_

#include <utility>
#include <boost/shared_array.hpp>

struct UnionFind {

    struct Entry {
        int parent;
        int depth;
        int size;
    };

    int N;
    boost::shared_array<Entry> partitions;

    UnionFind(int N = 0);

    int Find(int elem) const;
    int Depth(int elem) const { return partitions[Find(elem)].depth; }
    int ComponentSize(int elem) const { return partitions[Find(elem)].size; }
    void Merge(int e1, int e2);
    int Size() const { return N; }
};

inline UnionFind::UnionFind(int N)
    : N(N)
{
    partitions = boost::shared_array<Entry>(new Entry[N]);
    for (int i = 0; i < N; ++i) {
        partitions[i].parent = i;
        partitions[i].depth = 1;
        partitions[i].size = 1;
    }
}

inline int UnionFind::Find(int elem) const {
    int parent = partitions[elem].parent;
    while (parent != elem) {
        elem = parent;
        parent = partitions[elem].parent;
    }
    return elem;
}

inline void UnionFind::Merge(int e1, int e2) {
    int p1 = Find(e1);
    int p2 = Find(e2);
    if (p1 == p2)
        return;
    int d1 = partitions[p1].depth;
    int d2 = partitions[p2].depth;
    if (d1 < d2) {
        partitions[p1].parent = p2;
        partitions[p2].size += partitions[p1].size;
    } else if (d2 < d1) {
        partitions[p2].parent = p1;
        partitions[p1].size += partitions[p2].size;
    } else {
        partitions[p2].parent = p1;
        partitions[p1].size += partitions[p2].size;
        partitions[p1].depth++;
    }
}

#endif


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * ****************************************************************************
 * Fitting things to data.
 * ****************************************************************************
 */

#ifndef VISIONCORE_MATH_FITTING_HPP
#define VISIONCORE_MATH_FITTING_HPP

#include <VisionCore/Platform.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/LU>

#include <VisionCore/Math/Statistics.hpp>
#include <VisionCore/Types/Hypersphere.hpp>

namespace vc
{

namespace math
{

/**
 * Fitting plane from 3D points.
 */
template<typename T>
class PlaneFitting
{
public:
    typedef MultivariateStats<Eigen::Matrix<T,3,1>> StatsT;
    typedef typename StatsT::VectorType VectorT;
    typedef typename StatsT::CovarianceType CovarianceMatrixT;
    typedef Eigen::Hyperplane<T,3> PlaneT;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    PlaneFitting() { reset(); }

    inline void reset() { stats.reset(); }
    inline std::size_t count() const { return stats.count(); }

    inline void operator()(const VectorT& x)
    {
        stats(x);
    }

    bool getPlane(PlaneT& p, T& curvature) const
    {
        return getPlane(stats, p, curvature);
    }

private:
    bool getPlane(const StatsT& ss, PlaneT& p, T& curvature) const
    {
        if(ss.count() < 3)
        {
            return false;
        }

        const VectorT mean_point = ss.mean();
        const CovarianceMatrixT cm = ss.covariance();

        getPlane(cm, mean_point, p, curvature);

        return true;
    }

    void getPlane(const CovarianceMatrixT& cm, const VectorT& mean_point, PlaneT& p, T& curvature) const
    {
        using Eigen::numext::abs;

        Eigen::SelfAdjointEigenSolver<CovarianceMatrixT> es(cm);

        const T eigen_value = es.eigenvalues()(0);
        const VectorT eigen_vector = es.eigenvectors().col(0);

        p.normal() = eigen_vector;

        T eig_sum = cm.coeff(0) + cm.coeff(4) + cm.coeff(8);
        if(eig_sum != T(0.0))
        {
            curvature = abs(eigen_value / eig_sum);
        }
        else
        {
            curvature = T(0.0);
        }

        // Hessian form (D = nc . p_plane (centroid here) + p)
        p.offset() = T(1.0) * eigen_vector.dot(mean_point);
    }

    math::MultivariateStats<VectorT> stats;
};

/**
 * Circle from 3 points.
 */
template<typename T>
EIGEN_DEVICE_FUNC static inline types::CircleT<T> circleFrom3Points(const Eigen::Matrix<T,2,1>& p1, const Eigen::Matrix<T,2,1>& p2, const Eigen::Matrix<T,2,1>& p3)
{
    T ma = (p2(1) - p1(1))/(p2(0) - p1(0));
    T mb = (p3(1) - p2(1))/(p3(0) - p2(0));

    T cx = (ma*mb * (p1(1) - p3(1)) + mb * (p1(0) + p2(0)) - ma * (p2(0) + p3(0)))/(2 * (mb - ma));
    T cy = (mb*p3(1)+(mb-ma)*p2(1)-ma*p1(1)+p3(0)-p1(0))/(2*(mb-ma));

    types::CircleT<T> ret;

    ret.coeff() << cx , cy;
    ret.radius() = ret(p1);

    return ret;
}

// ****************** AFFINE FROM 3D POINT CORRESPONDANCES ***********

/**
 * Establish transform from 3D corresponding points.
 */
template<typename T>
class TransformationFrom3DPointPairs
{
public:
    typedef Eigen::Matrix<T,3,1> VectorT;
    typedef Eigen::Matrix<T,3,3> MatrixT;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    inline void reset()
    {
        sample_count = 0;
        accumulated_weight_ = 0.0;
        mean1.fill(0);
        mean2.fill(0);
        covariance.fill(0);
    }

    inline std::size_t count() const { return sample_count; }

    inline void operator()(const VectorT& p1, const VectorT& p2, T weight = T(1.0))
    {
        if(weight == T(0.0))
            return;

        ++sample_count;
        accumulated_weight_ += weight;
        T alpha = weight/accumulated_weight_;

        VectorT diff1 = p1 - mean1, diff2 = p2 - mean2;
        covariance = (T(1.0) - alpha)*(covariance + alpha * (diff2 * diff1.transpose()));

        mean1 += alpha * (diff1);
        mean2 += alpha * (diff2);
    }

    inline Eigen::Affine3f getTransformation ()
    {
        Eigen::JacobiSVD<MatrixT> svd (covariance, Eigen::ComputeFullU | Eigen::ComputeFullV);
        const MatrixT& u = svd.matrixU(),& v = svd.matrixV();

        MatrixT s;
        s.setIdentity();
        if(u.determinant()*v.determinant() < T(0.0f))
        {
            s(2,2) = T(-1.0);
        }

        MatrixT r = u * s * v.transpose();
        VectorT t = mean2 - r*mean1;

        Eigen::Transform<T,3,Eigen::Affine> ret(Eigen::Matrix<T,4,4>::Zero());
        ret.block<3,3>(0,0) = r;
        ret.block<3,1>(0,3) = t;

        return ret;
    }
private:
    std::size_t sample_count;
    T accumulated_weight_;
    VectorT mean1, mean2;
    MatrixT covariance;
};

}

}

#endif // VISIONCORE_MATH_FITTING_HPP


/**
 * @file clempiricflux.cc
 * @brief NPDE exam problem summer 2019 "CLEmpiricFlux" code
 * @author Oliver Rietmann
 * @date 18.07.2019
 * @copyright Developed at ETH Zurich
 */

#include "clempiricflux.h"

#include <Eigen/Core>
#include <cassert>

namespace CLEmpiricFlux {

/**
 * @brief Bisection algorithm for root finding of an increasing function.
 *
 * @param g continuous changing sign in the interval [v, w]
 * @param v lower bound of the interval containg the root
 * @param w upper bound of the interval containg the root
 * @param tol error tolerance for stopping criterion
 * @return approximate root x in [v, w]
 */
/* SAM_LISTING_BEGIN_8 */
template <typename FUNCTOR>
double findRoots(double v, double w, FUNCTOR &&g, double tol = 1.0E-6) {
  double x = v;  // approximate root
  const double len = w - v;
  constexpr static const int maxN = 1000;
  double gv = g(v), gw = g(w);
  // Ensure that function changes sign
  assert(gv * gw <= 0);
  for (int N = 0; (std::abs(w - v) > tol * len) && N < maxN; N++) {
    x = (v + w) / 2.0;
    const double gx = g(x);
    if (gv * gx < 0.0) {
      // Sign change in left half of [v,w]
      w = x;
      gw = gx;
    } else {
      // Sign change in right half of [v,w]
      v = x;
      gv = gx;
    }
  }
  return x;
}
/* SAM_LISTING_END_8 */

GodunovFlux::GodunovFlux(const UniformCubicSpline &f) : _f(f){};

/* SAM_LISTING_BEGIN_9 */
double GodunovFlux::operator()(double v, double w) const {
  double result;
  //====================
  // Your code goes here
  //====================
  return result;
}

/* SAM_LISTING_END_9 */

}  // namespace CLEmpiricFlux


#include "scope.h"

#include "math_lib/transpose.h"

#include <boost/qvm/map_mat_mat.hpp>
#include <boost/qvm/map_mat_vec.hpp>
#include <boost/qvm/map_vec_mat.hpp>
#include <boost/qvm/vec_operations.hpp>

namespace pagoda
{
Scope::Scope() : m_position{0, 0, 0}, m_size{0, 0, 0}, m_rotation(boost::qvm::diag_mat(Vec3F{1, 1, 1})) {}

Scope::Scope(const Vec3F &pos, const Vec3F &size, const Mat3x3F &rot) : m_position(pos), m_size(size), m_rotation(rot)
{
}

Scope::Scope(const std::array<Vec3F, 8> &boxPoints)
{
	auto origin = boxPoints[static_cast<int>(BoxPoints::LowerBottomLeft)];
	auto lbr = boxPoints[static_cast<int>(BoxPoints::LowerBottomRight)];
	auto ltl = boxPoints[static_cast<int>(BoxPoints::LowerTopLeft)];
	auto hbl = boxPoints[static_cast<int>(BoxPoints::HigherBottomLeft)];
	auto xAxis = lbr - origin;
	auto yAxis = ltl - origin;
	auto zAxis = boost::qvm::cross(xAxis, yAxis);

	m_position = origin;
	m_size = Vec3F{boost::qvm::mag(xAxis), boost::qvm::mag(yAxis), boost::qvm::mag(origin - hbl)};
	boost::qvm::col<0>(m_rotation) = boost::qvm::normalized(xAxis);
	boost::qvm::col<1>(m_rotation) = boost::qvm::normalized(yAxis);
	boost::qvm::col<2>(m_rotation) = boost::qvm::normalized(zAxis);
}

Vec3F Scope::GetPosition() const { return m_position; }

void Scope::SetPosition(const Vec3F &pos) { m_position = pos; }

Vec3F Scope::GetSize() const { return m_size; }

void Scope::SetSize(const Vec3F &size) { m_size = size; }

Mat3x3F Scope::GetRotation() const { return m_rotation; }

void Scope::SetRotation(const Mat3x3F &rotation) { m_rotation = rotation; }

Mat3x3F Scope::GetInverseRotation() const { return boost::qvm::transposed(m_rotation); }

Vec3F Scope::GetXAxis() const { return boost::qvm::col<0>(m_rotation); }
Vec3F Scope::GetYAxis() const { return boost::qvm::col<1>(m_rotation); }
Vec3F Scope::GetZAxis() const { return boost::qvm::col<2>(m_rotation); }

Vec3F Scope::GetAxis(const std::string &axisName) const
{
	if (axisName == "x")
	{
		return GetXAxis();
	}
	else if (axisName == "y")
	{
		return GetYAxis();
	}
	CRITICAL_ASSERT_MSG(axisName == "z", "Axis name must be one of x, y or z.");
	return GetZAxis();
}

Vec3F Scope::GetAxis(char axisName) const
{
	switch (axisName)
	{
		case 'x':
			return GetXAxis();
		case 'y':
			return GetYAxis();
		case 'z':
			return GetZAxis();
		default:
			CRITICAL_ASSERT_MSG(false, "Axis must be one of x, y or z.");
	}
	return Vec3F();
}

Plane<float> Scope::GetXYPlane() const { return Plane<float>::FromPointAndNormal(m_position, GetZAxis()); }
Plane<float> Scope::GetXZPlane() const { return Plane<float>::FromPointAndNormal(m_position, GetYAxis()); }
Plane<float> Scope::GetYZPlane() const { return Plane<float>::FromPointAndNormal(m_position, GetXAxis()); }

Vec3F Scope::LocalPointInWorld(const Vec3F &localPoint) const
{
	return m_position + GetXAxis() * X(localPoint) + GetYAxis() * Y(localPoint) + GetZAxis() * Z(localPoint);
}

Vec3F Scope::GetLocalPoint(const BoxPoints &p) const
{
	uint32_t index = static_cast<uint32_t>(p);
	Vec3F localPoint;
	X(localPoint) = ((index & (1 << 0)) != 0 ? X(m_size) : 0);
	Y(localPoint) = ((index & (1 << 1)) != 0 ? Y(m_size) : 0);
	Z(localPoint) = ((index & (1 << 2)) != 0 ? Z(m_size) : 0);
	return localPoint;
}

Vec3F Scope::GetWorldPoint(const BoxPoints &p) const { return LocalPointInWorld(GetLocalPoint(p)); }

std::array<Vec3F, 8> Scope::GetWorldPoints() const
{
	std::array<Vec3F, 8> boxPoints;
	for (int i = 0; i < 8; ++i)
	{
		boxPoints[i] = GetWorldPoint(static_cast<BoxPoints>(i));
	}
	return boxPoints;
}

Vec3F Scope::GetLocalVector(const Vec3F &worldVector) const
{
	return Vec3F{boost::qvm::dot(GetXAxis(), worldVector), boost::qvm::dot(GetYAxis(), worldVector),
	             boost::qvm::dot(GetZAxis(), worldVector)};
}

Vec3F Scope::GetWorldVector(const Vec3F &localVector) const
{
	return GetXAxis() * X(localVector) + GetYAxis() * Y(localVector) + GetZAxis() * Z(localVector);
}

Vec3F Scope::GetCenterPointInWorld() const { return LocalPointInWorld(GetCenterPointInLocal()); }

Vec3F Scope::GetCenterPointInLocal() const
{
	return 0.5f * (GetLocalPoint(BoxPoints::LowerBottomLeft) + GetLocalPoint(BoxPoints::HigherTopRight));
}
}  // namespace pagoda