/*------------------------------------------------------------------------------
 *	Copyright (c) 2023 by Bai Bing (seread@163.com)
 *	See COPYING file for copying and redistribution conditions.
 *
 *  Alians IT Studio.
 *----------------------------------------------------------------------------*/

#include <algorithm>
#include <array>
#include <cfloat>
#include <chrono>
#include <climits>
#include <cmath>
#include <cstdlib>
#include <execution>
#include <fstream>
#include <functional>
#include <future>
#include <iomanip>
#include <list>
#include <map>
#include <memory>
#include <numeric>
#include <stdexcept>
#include <thread>
#include <utility>
#include <vector>

#include "core/FunCheck.h"
#include "core/Time.h"
#include "utils/Misc.h"
#include "ASTriangle.h"
#include "ASPolygon.h"
#include "ASGridInterpolator.h"
#include "ASGridInterpolatorBlock.h"
#include "interpolator/ASInterpolatorSupport.h"
#include "interpolator/ASInterpolateMethod.h"
#include "linalg/lstsq.h"

#define _LIKELY(x)  (x)

namespace ais
{
    GridInterpolator::GridInterpolator() : maxIteration_(2000), residual_(0.0001), fillValue_(0), zMean_(std::nan("1")),
                                           dxSize_(0), dySize_(0), alpha_(std::nan("1")), beta_(std::nan("1")), flagSucceed_(false)
    {
    }

    GridInterpolator::~GridInterpolator()
    {
    }

    GridInterpolator::GridInterpolator(const std::vector<ais::PointXYZ> &randomPoints, // random sample points
                                       const std::vector<ais::Point> &areaPoints,      // result grid area corners points
                                       std::shared_ptr<ais::FMatrix> targetMatrix,     // output result values
                                       const std::vector<ais::BreakLine> &breaklines,  // break lines
                                       const std::vector<ais::Fault> &faults           // faults
                                       ) : randomPoints_(randomPoints), areaPoints_(areaPoints), targetMatrixPtr_(targetMatrix), breaklines_(breaklines), faults_(faults)
    {
        generate_grid_params(targetMatrixPtr_);

        // data statistics included point positions' distance, so it should be called after coordinates_points
        if(randomPoints.size() < 1000)
            generate_data_statistics();
    }

    void GridInterpolator::set_area_points(const std::vector<ais::Point> &points)
    {
        if (points.size() >= 2)
        {
            areaPoints_.clear();
            areaPoints_ = points;

            // update internal vars
            auto xs = getAxisValues(areaPoints_, Axis3DType::X);
            auto ys = getAxisValues(areaPoints_, Axis3DType::Y);
            xMinMax_ = getMinMax(xs);
            yMinMax_ = getMinMax(ys);
        }
    }

    void GridInterpolator::start(InterpolationMethod method)
    {
        // before start interpolation, checkout required params
        if (targetMatrixPtr_->size() == 0 || areaPoints_.empty())
            THROW_INVALID_ARGUMENT("Grid interpolator require completed target matrix and sample points");

        // fault may affect the constraint value, do it first
        build_fault_impact_matrix(targetMatrixPtr_, pointInFaultMatrixPtr_, faultAffectMatrixPtr_);

        // build the constraint matrix
        build_constraint_matrix(targetMatrixPtr_, constrainMatrixPtr_);

        // update edge nodes by nearest neighbors or fill value
        update_edge_node(targetMatrixPtr_);

        // check the grid size, if the grid size is too smell to get a smooth result,
        // try to estimate a grid with bigger grid size
        detect_estimate_factor();
        scale_estimate();

        // estimate the target matrix for better output
        if (estimateMatrixPtr_.get())
            estimate_gradients_surface(targetMatrixPtr_);

        // update nan nodes with fill value
        update_nan_node(targetMatrixPtr_);

        // update whole matrix
        start_interpolation(targetMatrixPtr_, constrainMatrixPtr_, pointInFaultMatrixPtr_, faultAffectMatrixPtr_, method);

        flagEnd_ = true;
    }

    std::string GridInterpolator::report()
    {
        std::stringstream ss;
        ss << "Iteration: " << iteration_ << " times, interpolations " << (flagSucceed_ ? "Succeed" : "Failed") << std::endl;
        if (!faults_.empty())
        {
            ss << "No data node: " << faultNodesCount_ << std::endl;
        }
        ss << "Iteration execution time: " << iteration_time() << std::endl;
        ss << "Result information: " << std::endl;
        ss << "---------------------------------------" << std::endl;
        ss << header_info();
        ss << std::endl;

        return ss.str();
    }

    std::string GridInterpolator::points_info()
    {
        std::stringstream ss;

        auto pCount = randomPoints_.size();
        ss << "Total " << pCount << " simple points : " << std::endl;
        ss << "x, y, z" << std::endl;

        if (pCount <= 10)
        {
            for (int i = 0; i < pCount; i++)
            {
                auto &p = randomPoints_[i];
                ss << p.x << ", " << p.y << ", " << p.z << std::endl;
            }
        }
        else
        {
            for (int i = 0; i <= 5; i++)
            {
                auto &p = randomPoints_[i];
                ss << p.x << ", " << p.y << ", " << p.z << std::endl;
            }
            ss << "...";
            for (auto pi = randomPoints_.cend() - 6; pi != randomPoints_.cend(); pi++)
            {
                auto &p = *pi;
                ss << p.x << ", " << p.y << ", " << p.z << std::endl;
            }
        }

        return ss.str();
    }

    std::string GridInterpolator::header_info(std::shared_ptr<ais::FMatrix> dataMatrixPtr)
    {
        if (!dataMatrixPtr.get())
            dataMatrixPtr = targetMatrixPtr_;

        auto shape = dataMatrixPtr->shape();

        std::stringstream ss;
        ss << shape.cols << " " << shape.rows << std::endl;
        ss << std::fixed << std::setprecision(4);
        ss << xMinMax_.first << " " << xMinMax_.second << std::endl;
        ss << yMinMax_.first << " " << yMinMax_.second << std::endl;
        ss << zMinMax_.first << " " << zMinMax_.second << std::endl;

        return ss.str();
    }

    std::string GridInterpolator::data_info(std::shared_ptr<ais::FMatrix> dataMatrixPtr)
    {
        if (!dataMatrixPtr.get())
            dataMatrixPtr = targetMatrixPtr_;

        auto shape = dataMatrixPtr->shape();

        std::string out_string;
        out_string.reserve(256 * 1024 * 1024);
        for (size_t r = 0; r < shape.rows; r++)
        {
            std::string line;
            line.reserve(16 * 1024 * 1024);
            for (size_t c = 0; c < shape.cols; c++)
            {
                line += std::to_string(dataMatrixPtr->at(r, c)) + " ";
            }

            out_string += line + "\n\n";
        }

        return out_string;
    }

    std::string GridInterpolator::iteration_time()
    {
        std::stringstream ss;

        auto durSecs = std::chrono::duration_cast<std::chrono::milliseconds>(endTime_ - startTime_).count() / 1000.0;
        auto hours = int(durSecs / 3600);
        auto minutes = int((durSecs - hours * 3600) / 60);
        auto seconds = durSecs - hours * 3600 - minutes * 60;
        ss << hours << ":" << minutes << ":" << std::setprecision(3) << seconds;

        return ss.str();
    }

    std::string GridInterpolator::get_progress_msg()
    {
        std::stringstream ss;

        if (!interpolationMsgs_.empty())
        {
            std::lock_guard<std::mutex> lk(msgMutex_);

            for (auto &s : interpolationMsgs_)
                ss << s << std::endl;

            // all strings have been transformed to ss, clean internal caches
            interpolationMsgs_.clear();
        }
        return ss.str();
    };

    void GridInterpolator::dump_result(const char *filename, std::shared_ptr<ais::FMatrix> dataMatrixPtr)
    {
        if (!dataMatrixPtr.get())
            dataMatrixPtr = targetMatrixPtr_;

        std::lock_guard<std::mutex> lk(dataMutex_);

        std::ofstream outfile;
        outfile.open(filename, std::ios::out);

        // header
        outfile << "DSAA" << std::endl;
        outfile << header_info(dataMatrixPtr);
        // data
        outfile << data_info(dataMatrixPtr);

        outfile.close();
    }

    void GridInterpolator::dump_result_async(const std::string &filename, std::shared_ptr<ais::FMatrix> dataMatrixPtr, int8_t detailLevel)
    {
        /*
        if (detailLevel <= this->detailLevel_ )
            auto ret = std::async(std::launch::async,
                                  [this, &filename, &dataMatrixPtr]()
                                  {
                                      dump_result(filename.c_str(), dataMatrixPtr);
                                      return;
                                  });
                                  */
    }

    void GridInterpolator::generate_grid_params(std::shared_ptr<ais::FMatrix> targetMatrixPtr)
    {
        if (targetMatrixPtr->size() != 0)
        {
            // config default parameters by input samplePoints, areaPoints, and result matrix
            auto shape = targetMatrixPtr->shape();
            auto totalGridPoints = targetMatrixPtr->size();
            // max iteration suggested 1~2 times the number of grid nodes by suffer
            maxIteration_ = uint64_t(totalGridPoints * 2);

            if (!areaPoints_.empty())
            {
                auto xs = getAxisValues(areaPoints_, Axis3DType::X);
                auto ys = getAxisValues(areaPoints_, Axis3DType::Y);
                xMinMax_ = getMinMax(xs);
                yMinMax_ = getMinMax(ys);
            }
            else
            {
                xMinMax_ = getMinMax(getAxisValues(randomPoints_, Axis3DType::X));
                yMinMax_ = getMinMax(getAxisValues(randomPoints_, Axis3DType::Y));
            }

            auto zs = getAxisValues(randomPoints_, Axis3DType::Z);
            zMinMax_ = getMinMax(zs);
            zMean_ = std::accumulate(zs.begin(), zs.end(), 0.0) / zs.size();

            // residual, default residual_ * (zMax - zMin)
            epsMax_ = residual_ * std::abs(zMinMax_.second - zMinMax_.first);

            // get spacing of grid matrix
            dxSize_ = (xMinMax_.second - xMinMax_.first) / (shape.cols - 1);
            dySize_ = (yMinMax_.second - yMinMax_.first) / (shape.rows - 1);

            alpha_ = dxSize_ / dySize_;
            beta_ = dySize_ / dxSize_;

            x_ = generateAxisPoints(xMinMax_.first, xMinMax_.second, shape.cols);
            y_ = generateAxisPoints(yMinMax_.first, yMinMax_.second, shape.rows);

            // update random point position by current grid params, such as dxSize, dySize
            randomPointPositions_ = coordinates_points(randomPoints_);
        }

        // grid require params incomplete, do nothing.
    }

    void GridInterpolator::generate_data_statistics()
    {
        auto shape = targetMatrixPtr_->shape();

        // distance for random point matrix
        auto dx = xMinMax_.second - xMinMax_.first;
        auto dy = yMinMax_.second - yMinMax_.first;
        double distanceMax = 0.0, distanceMin = std::sqrt(dx * dx + dy * dy) + 1;
        for (auto pia = randomPoints_.begin(); pia != randomPoints_.end() - 1; pia++)
            for (auto pib = pia + 1; pib != randomPoints_.end(); pib++)
            {
                auto distance = pia->distance2D(*pib);
                if (distance > distanceMax)
                    distanceMax = distance;
                if (distance < distanceMin)
                    distanceMin = distance;
            }

        dataStatistics_["distance_min"] = distanceMin;
        dataStatistics_["distance_max"] = distanceMax;

        // distance for position matrix
        distanceMax = 0.0, distanceMin = std::sqrt(shape.cols * shape.cols + shape.rows * shape.rows) + 1;
        for (auto pia = randomPointPositions_.begin(); pia != randomPointPositions_.end() - 1; pia++)
            for (auto pib = pia + 1; pib != randomPointPositions_.end(); pib++)
            {
                auto distance = pia->distance2D(*pib);
                if (distance > distanceMax)
                    distanceMax = distance;
                if (distance < distanceMin)
                    distanceMin = distance;
            }

        dataStatistics_["position_distance_min"] = distanceMin;
        dataStatistics_["position_distance_max"] = distanceMax;
    }

	std::shared_ptr<ais::Matrix<double>> GridInterpolator::matrix()
	{
		return targetMatrixPtr_;
	}

	std::map<std::string, std::any> GridInterpolator::params()
    {
        auto shape = targetMatrixPtr_->shape();

        std::map<std::string, std::any> ret;

        ret["x_min"] = xMinMax_.first;
        ret["x_max"] = xMinMax_.second;
        ret["y_min"] = yMinMax_.first;
        ret["y_max"] = yMinMax_.second;
        ret["z_min"] = zMinMax_.first;
        ret["z_max"] = zMinMax_.second;

        ret["x_nodes"] = shape.cols;
        ret["y_nodes"] = shape.rows;
        ret["x_grid_size"] = dxSize_;
        ret["y_grid_size"] = dySize_;

        ret["max_iterations"] = maxIteration_;
        ret["residual"] = residual_;
        ret["eps_max"] = epsMax_;
        ret["fill_value"] = fillValue_;
        ret["use_multi_threads"] = flagMultiThread_;
        ret["detail_level"] = detailLevel_;

        return ret;
    }

    std::string GridInterpolator::params_info()
    {
        auto shape = targetMatrixPtr_->shape();

        std::stringstream ss;
        ss << std::fixed << std::setprecision(4);
        ss << "Simple points info:" << std::endl;
        ss << "---------------------------------------" << std::endl;
        ss << "points count: " << randomPoints_.size() << std::endl;
        ss << "x_min: " << xMinMax_.first << ", x_max: " << xMinMax_.second << std::endl;
        ss << "y_min: " << yMinMax_.first << ", y_max: " << yMinMax_.second << std::endl;
        ss << "z_min: " << zMinMax_.first << ", z_max: " << zMinMax_.second << std::endl;
        ss << std::endl;

        ss << "Target grid info:" << std::endl;
        ss << "---------------------------------------" << std::endl;
        ss << "Target area:" << std::endl;
        for (auto &point : areaPoints_)
        {
            ss << "\t(" << point.x << ", " << point.y << ")" << std::endl;
        }
        ss << std::endl;
        ss << "x_nodes: " << shape.cols << ", y_nodes: " << shape.rows << std::endl;
        ss << "x_grid_size: " << dxSize_ << ", y_grid_size: " << dySize_ << std::endl;
        ss << "output detail level: " << int(detailLevel_) << std::endl;
        ss << std::endl;

        if (!breaklines_.empty())
        {
            ss << "Breaklines info:" << std::endl;
            ss << "---------------------------------------" << std::endl;
            for (auto &line : breaklines_)
            {
                ss << line << std::endl;
            }
            ss << std::endl;
        }

        if (!faults_.empty())
        {
            ss << "Faults info:" << std::endl;
            ss << "---------------------------------------" << std::endl;
            for (auto &fault : faults_)
            {
                ss << fault << std::endl;
            }
            ss << std::endl;
        }

        ss << "Interpolation info:" << std::endl;
        ss << "---------------------------------------" << std::endl;
        ss << "max_iteration: " << maxIteration_ << std::endl;
        ss << "residual: " << residual_ << std::endl;
        ss << "eps_max: " << epsMax_ << std::endl;
        ss << "fill_value: " << fillValue_ << std::endl;
        ss << "use_multi_threads: " << (flagMultiThread_ ? "True" : "False") << std::endl;
        ss << "corner_weight: " << cornerWeight_ << std::endl;
        ss << std::endl;
        return ss.str();
    }

    std::string GridInterpolator::dump_data_statistics()
    {
        // output the data statistics
        std::stringstream ss;

        ss << "Origin point data statistics:" << std::endl;
        ss << "---------------------------------------" << std::endl;
        for (auto &kv : dataStatistics_)
            ss << kv.first << ": " << kv.second << std::endl;

        ss << std::endl;

        return ss.str();
    }

    void GridInterpolator::update_grid_params(const std::map<std::string, std::any> &params)
    {
        for (auto &kv : params)
        {
            auto key = kv.first;
            auto value = kv.second;
            if (key == "max_iteration")
                maxIteration_ = std::any_cast<size_t>(value);
            else if (key == "residual")
            {
                residual_ = std::any_cast<double>(value);
                epsMax_ = residual_ * (zMinMax_.second - zMinMax_.first);
            }
            else if (key == "fill_value")
                fillValue_ = std::any_cast<double>(value);
            else if (key == "use_multi_threads")
                flagMultiThread_ = std::any_cast<bool>(value);
            else if (key == "fault_edge_level")
                faultEdgeLevel_ = std::any_cast<int8_t>(value);
            else if (key == "estimate_factor")
                estimateFactor_ = std::any_cast<int32_t>(value);
            else if (key == "corner_weight")
                cornerWeight_ = std::any_cast<int16_t>(value);
            else if (key == "detail_level")
                detailLevel_ = std::any_cast<int8_t>(value);
        }
    }

    // setup constraints matrix
    void GridInterpolator::setup_constraint_point(std::shared_ptr<ais::PMatrix> &cmp, size_t xi, size_t yi, double xGap, double yGap, const ais::PointXYZ &point)
    {
        auto shape = cmp->shape();
        if ((0 <= xi && xi < shape.cols) && (0 <= yi && yi < shape.rows))
        {
            auto &p = cmp->at(yi, xi);

            // test the constraint point in fault or not by its real position
            auto cpx = xMinMax_.first + xi * dxSize_;
            auto cpy = yMinMax_.first + yi * dySize_;
            ais::Point realPosition{cpx, cpy};

            bool inFault = false;
            for (auto &fault : faults_)
            {
                // if (realPosition.in_box(fault.box) && (realPosition.in_polylines(fault.points) || realPosition.in_polygon(fault.polygon, 4, dxSize_, dySize_)))
                if (realPosition.in_box(fault.box) && (realPosition.in_polylines(fault.points) || realPosition.in_polygon(fault.polygon)))
                {
                    inFault = true;
                    break;
                }

                // test line rp--cp cross fault line or not
                auto m = fault.points.size();
                ais::Point p{point.x, point.y};
                for (auto mi = 0; mi < m - 1; mi++)
                {
                    if (line_cross(p, realPosition, fault.points[mi], fault.points[mi + 1]))
                    {
                        inFault = true;
                        break;
                    }
                }

                if (inFault)
                    break;
            }

            if ((!inFault) && (!p.has_value() || std::abs(xGap * xGap + yGap * yGap) < std::abs(p.x * p.x + p.y * p.y)))
            {
                // setup constraints point while there is no constraint or current point is more close, and point not in Fault
                cmp->put(yi, xi, ais::PointXYZ{xGap, yGap, point.z});
            }
        }
    };

    std::vector<PointXYZ> GridInterpolator::generate_constraint_points(const ais::BreakLine &breakline)
    {
        std::vector<PointXYZ> result;

        // build up the breaklines
        std::vector<std::pair<ais::PointXYZ, ais::PointXYZ>> lines;
        for (auto iter = breakline.points.begin(); iter != breakline.points.end(); iter++)
        {
            if (iter + 1 != breakline.points.end())
                lines.emplace_back(std::make_pair(*iter, *(iter + 1)));
        }

        for (auto &line : lines)
        {
            auto pStart = line.first;
            auto pEnd = line.second;

            // added points at start and end
            result.push_back(pStart);
            result.push_back(pEnd);

            if (pStart.x > pEnd.x)
                std::swap(pStart, pEnd);

            // added points on line, interpolation point every grid node
            double dx = pEnd.x - pStart.x;
            double dy = pEnd.y - pStart.y;
            double dz = pEnd.z - pStart.z;

            if (dx != 0.0 && std::abs(dy / dx) <= 1.0)
            {
                // use x as interpolation factor
                size_t startIndex = (size_t)std::ceil((pStart.x - xMinMax_.first) / dxSize_);
                size_t endIndex = (size_t)std::ceil((pEnd.x - xMinMax_.first) / dxSize_);

                for (size_t i = startIndex; i < endIndex; i++)
                {
                    auto x = xMinMax_.first + i * dxSize_;

                    auto y = (x - pStart.x) * dy / dx + pStart.y;
                    auto z = (x - pStart.x) * dz / dx + pStart.z;

                    result.push_back({x, y, z});
                }
            }
            else
            {
                // use y as interpolation factor
                size_t startIndex = (size_t)std::ceil((pStart.y - yMinMax_.first) / dySize_);
                size_t endIndex = (size_t)std::ceil((pEnd.y - yMinMax_.first) / dySize_);

                for (size_t i = startIndex; i < endIndex; i++)
                {
                    auto y = yMinMax_.first + i * dySize_;

                    auto x = (y - pStart.y) * dx / dy + pStart.x;
                    auto z = (y - pStart.y) * dz / dy + pStart.z;

                    result.push_back({x, y, z});
                }
            }
        }

        return result;
    }

    std::vector<PointXYZ> GridInterpolator::get_nearest_samples(size_t x_index, size_t y_index, size_t min_count, size_t max_count)
    {
        std::vector<PointXYZ> result;

        auto centerPoint = PointXYZ(x_index, y_index);

        size_t space_count = 0;
        while (result.size() < min_count)
        {
            // build the test area
            space_count++;

            double volatile xMin = centerPoint.x - space_count;
            double volatile xMax = centerPoint.x + space_count;
            double volatile yMin = centerPoint.y - space_count;
            double volatile yMax = centerPoint.y + space_count;

            auto xMaxPosition = (xMinMax_.second - xMinMax_.first) / dxSize_;
            auto yMaxPosition = (yMinMax_.second - yMinMax_.first) / dySize_;

            xMin = xMin < 0 ? 0 : xMin;
            xMax = xMax > xMaxPosition ? xMaxPosition : xMax;
            yMin = yMin < 0 ? 0 : yMin;
            yMax = yMax > yMaxPosition ? yMaxPosition : yMax;

            filter_copy_points({xMin, yMin}, {xMax, yMax}, randomPointPositions_, result);
        }

        // remove some to far points while the result is more than max_count
        if (result.size() > max_count)
        {
            std::vector<std::pair<double, PointXYZ>> pointDistances;
            for (auto iter = result.begin(); iter != result.end(); iter++)
            {
                // get all distances
                auto &p = *iter;
                auto dis = centerPoint.distance(p);
                pointDistances.push_back(std::make_pair(dis, p));
            }

            // sort their distances
            std::sort(pointDistances.begin(), pointDistances.end(), [](auto &a, auto &b)
                      { return a.first < b.first; });

            while (pointDistances.size() > max_count)
            {
                // filter too far points
                pointDistances.pop_back();
            }

            // set result
            result.clear();
            for (auto &i : pointDistances)
                result.push_back(i.second);
        }

        return result;
    }

    std::vector<PointXYZ> GridInterpolator::get_range_samples(size_t xIndex, size_t yIndex, double xGridSize, double yGridSize, const std::vector<PointXYZ> &randoms, double radius)
    {
        auto xMinMaxPosition = getMinMax(getAxisValues(randoms, Axis3DType::X));
        auto yMinMaxPosition = getMinMax(getAxisValues(randoms, Axis3DType::Y));

        auto dX = xMinMaxPosition.second - xMinMaxPosition.first;
        auto dY = yMinMaxPosition.second - yMinMaxPosition.first;

        // default is the max value of grid space / 8
        double r = radius <= 0.0 ? std::max(dX, dY) / 8 : radius;

        std::vector<PointXYZ> result;
        PointXYZ center{xMinMaxPosition.first + xIndex * xGridSize,
                        yMinMaxPosition.first + yIndex * yGridSize};
        for (auto &p : randoms)
        {
            if (center.distance(p) <= r)
                result.push_back(p);
        }

        return result;
    }

    std::vector<PointXYZ> GridInterpolator::coordinates_points(const std::vector<PointXYZ> &points)
    {
        std::vector<PointXYZ> result;

        for (auto &p : points)
        {
            auto xPosition = (p.x - xMinMax_.first) / dxSize_;
            auto yPosition = (p.y - yMinMax_.first) / dySize_;

            result.push_back({xPosition, yPosition, p.z});
        }

        return result;
    }

    std::map<size_t, PointXYZ> GridInterpolator::points_hash_map(const std::vector<PointXYZ> &points)
    {
        std::map<size_t, PointXYZ> result;

        for (auto &p : points)
        {
            result[p.hash2D()] = p;
        }

        return result;
    }

    void GridInterpolator::build_constraint_matrix(std::shared_ptr<ais::FMatrix> targetMatrixPtr, std::shared_ptr<ais::PMatrix> &constrainMatrixPtr)
    {
        std::stringstream msg;
        msg << ais::time_str() << " " << __FUNCTION__;
        interpolationMsgs_.push_back(msg.str());

        if (!targetMatrixPtr)
            THROW_INVALID_ARGUMENT("Target matrix not have been initialized");

        if (randomPoints_.empty())
            THROW_INVALID_ARGUMENT("Random points list is empty");

        // build the constraint matrix and filled nan
        if (!constrainMatrixPtr.get())
        {
            constrainMatrixPtr = std::make_shared<ais::PMatrix>(targetMatrixPtr->shape());
            constrainMatrixPtr->emptys();
        }

        auto eps_x = dxSize_ > 1 ? DBL_EPSILON * dxSize_ : DBL_EPSILON;
        auto eps_y = dySize_ > 1 ? DBL_EPSILON * dySize_ : DBL_EPSILON;

        // fill constraint points by input param
        auto fill_constraint_points = [&](const std::vector<PointXYZ> &points)
        {
            auto size = points.size();
            auto threads = size_t(std::thread::hardware_concurrency() * 1.5);
            auto count = size_t(size / threads);
            std::vector<std::future<uint64_t>> execs;

            for (int t = 0; t < threads; t++)
            {
                // get begin and end param for each thread
                auto begin = count * t;
                // ensure the last thread fill all rest
                auto end = (t == (threads - 1)) ? size : begin + count;

                execs.emplace_back(std::async(
                    std::launch::async, [this, &constrainMatrixPtr, &points, &eps_x, &eps_y](size_t begin, size_t end)
                    {

                        for (size_t i = begin; i < end; i++)
                        {
                            auto& point = points[i];

                            // skip any point in the fault
                            bool inFault = false;
                            for (auto& fault : faults_)
                            {
                                //if (point.in_box(fault.box) && (point.in_polylines(fault.points) || point.in_polygon(fault.polygon, 4, dxSize_, dySize_)))
                                if (point.in_box(fault.box) && (point.in_polylines(fault.points) || point.in_polygon(fault.polygon)))
                                {
                                    inFault = true;
                                    break;
                                }
                            }

                            if (inFault)
                            {
                                // if point in a fault range, don't set its constraint, because it have been invalid by fault
                                continue;
                            }

                            // get x axis constraints i1--p----i2
                            auto xk = (point.x - xMinMax_.first) / dxSize_;
                            size_t i1 = size_t(xk);
                            size_t i2 = i1 + 1;
                            auto xGap1 = point.x - (xMinMax_.first + i1 * dxSize_);
                            auto xGap2 = point.x - (xMinMax_.first + i2 * dxSize_);
                            if (std::abs(xGap1) < eps_x)
                            {
                                // point is on the node, reset i2 & xGap2
                                i2 = i1;
                                xGap2 = xGap1;
                            }

                            // get y axis constraints j1-----p-j2
                            auto yk = (point.y - yMinMax_.first) / dySize_;
                            size_t j1 = size_t(yk);
                            size_t j2 = j1 + 1;
                            auto yGap1 = point.y - (yMinMax_.first + j1 * dySize_);
                            auto yGap2 = point.y - (yMinMax_.first + j2 * dySize_);
                            if (std::abs(yGap1) < eps_y)
                            {
                                // point is on the node, reset j2 & yGap2
                                j2 = j1;
                                yGap2 = yGap1;
                            }

                            setup_constraint_point(constrainMatrixPtr, i1, j1, xGap1, yGap1, point);
                            setup_constraint_point(constrainMatrixPtr, i1, j2, xGap1, yGap2, point);
                            setup_constraint_point(constrainMatrixPtr, i2, j1, xGap2, yGap1, point);
                            setup_constraint_point(constrainMatrixPtr, i2, j2, xGap2, yGap2, point);
                        }

                        return end - begin; },
                    begin, end));
            }

            uint64_t filled = 0;
            for (auto &e : execs)
            {
                e.wait();
                filled += e.get();
            }

            if (filled != size)
            {
                THROW_RUNTIME_ERROR("Setup constraint matrix failed!!");
                exit(-1);
            }
        };

        fill_constraint_points(randomPoints_);

        // update constraints by breakline
        for (auto &bl : breaklines_)
        {
            auto blConstraintPoints = generate_constraint_points(bl);
            fill_constraint_points(blConstraintPoints);
        }

        // finally, copy the constraint matrix point value to target
        std::vector<double> zList;
        for (const ais::PointXYZ &p : *constrainMatrixPtr)
        {
            zList.push_back(p.z);
        }

        targetMatrixPtr->putMany(0, zList);
    }

    void GridInterpolator::update_edge_node(std::shared_ptr<ais::FMatrix> &targetMatrixPtr, EdgeNodeValueMethod method)
    {
        auto tmp = targetMatrixPtr;
        // const for shape
        auto row = tmp->rows();
        auto col = tmp->columns();

        auto setEdgeByFillValue = [&](int64_t begin, int64_t end, int64_t step)
        {
            // pos is the flatten index offset of the point
            for (int64_t pos = begin; pos < end; pos += step)
            {
                auto iter = tmp->begin() + pos;
                if (ais::isnan(*iter))
                {
                    tmp->put(pos, this->fillValue_);

                    // add edge point to constrainMatrixPtr_
                    // auto [r, c] = targetMatrixPtr_->get_pos(iter);
                    // constrainMatrixPtr_->put(pos, {double(r), double(c) ,this->fillValue_ });
                }
            }
        };

        auto setEdgeByNearestValue = [&](int64_t begin, int64_t end, int64_t step)
        {
            // pos is the flatten index offset of the point
            for (int64_t pos = begin; pos < end; pos += step)
            {
                auto iter = tmp->begin() + pos;
                size_t validCount = 0;
                double mean = 0.0;

                auto [pos_row, pos_col] = tmp->get_pos(iter);

                // loop to setup null points
                if (std::isnan(*iter))
                {
                    // set radius to found non-zero value
                    int32_t radius = 1;
                    while (validCount == 0)
                    {
                        int32_t col_increase = static_cast<int64_t>(pos_col) + static_cast<int64_t>(radius);
                        int32_t row_increase = static_cast<int64_t>(pos_row) + static_cast<int64_t>(radius);
                        int32_t cMin = (pos_col - radius) < 0 ? 0 : pos_col - radius;
                        int32_t cMax = (col_increase) >= col ? (col - 1) : col_increase;

                        int32_t rMin = (pos_row - radius) < 0 ? 0 : pos_row - radius;
                        int32_t rMax = (row_increase) >= row ? (row - 1) : row_increase;

                        for (auto r = rMin; r <= rMax; r++)
                        {
                            for (auto c = cMin; c <= cMax; c++)
                            {
                                double zValue = *(tmp->begin() + r * col + c);
                                if (!std::isnan(zValue))
                                {
                                    mean += zValue;
                                    validCount++;
                                }
                            }
                        }
                        // zoom out for more neighbors
                        radius++;
                    }

                    mean /= validCount;
                    tmp->put(pos_row, pos_col, mean);

                    // add edge point to constrainMatrixPtr_
                    // auto [r, c] = targetMatrixPtr_->get_pos(iter);
                    // constrainMatrixPtr_->put(pos, { double(r), double(c) ,this->fillValue_ });
                }
            }
        };

        std::function<void(int64_t, int64_t, int64_t)> fun;
        if (method == EdgeNodeValueMethod::FILL)
            fun = setEdgeByFillValue;
        else
            fun = setEdgeByNearestValue;

        // row-0, bottom border
        fun(0, col, 1);
        // row-n, top border
        fun((row - 1) * col, row * col, 1);
        // column-0, left border
        fun(0, row * col, col);
        // column-n, right border
        fun(col - 1, row * col, col);
    }

    void GridInterpolator::update_nan_node(std::shared_ptr<ais::FMatrix> &targetMatrixPtr)
    {
        auto tmp = targetMatrixPtr;
        size_t count = std::count_if(std::execution::par_unseq, tmp->cbegin(), tmp->cend(), [](double v) {return std::isnan(v); });
        
        if (count > 0 && !std::isnan(fillValue_))
        {
            // only replace nan when fillValue_ is not nan and count > 0
            tmp->replace(std::nan("1"), this->fillValue_);

            std::string msg = "Total update nan nodes: " + std::to_string(count);
            interpolationMsgs_.push_back(msg);
        }
    }

    std::vector<double> build_xy_data(const Point3D &p)
    {
        std::vector<double> r{p.x, p.y, 1.0};
        return r;
    };

    bool all_filled(std::shared_ptr<ais::FMatrix> target, size_t begin_index, size_t end_index)
    {

        const auto function = [](double i) -> bool
        { return !(std::isnan(i) || std::isinf(i)); };

        if ((target->cbegin() + begin_index) > target->cend() || (target->cbegin() + end_index) > target->cend())
        {
            THROW_INVALID_ARGUMENT("Out of target range.");
        }

        bool ret = std::all_of(std::execution::par_unseq, target->cbegin() + begin_index, target->cend() + end_index, function);
        return ret;
    }

    size_t GridInterpolator::detect_estimate_factor()
    {
        // After many test, we found when the points count >= 1.5% of total gird node count, the minimum curvature
        // will output smooth result after estimated. So the main point for this function is try to found out the
        // scale factor for input X/Y grid size, then use this scale factor to increase grid size to get the 
        // estimate grid, later use the estimate grid and interpolate
        //      scale * randomPoints.size() / (x grid count * y grid count) > 1.5%
        if (estimateFactor_ == 0)
        {
            auto temp = ((xMinMax_.second - xMinMax_.first) / dxSize_) * ((yMinMax_.second - yMinMax_.first) / dySize_) * .015 / randomPoints_.size();
            size_t scale = (size_t)std::ceil(std::sqrt(temp));

            if (scale > 1)
                estimateFactor_ = scale;
        }

        return estimateFactor_;
    }

    void GridInterpolator::scale_estimate()
    {
        // function start message
        std::stringstream msg;
        msg << ais::time_str() << " " << __FUNCTION__;
        interpolationMsgs_.push_back(msg.str());

        // only do estimation while the estimateFactor_ is more than 1
        if (estimateFactor_ > 1)
        {
            std::string m = "Estimate factor: " + std::to_string(estimateFactor_);
            interpolationMsgs_.push_back(m);

            // 1. build estimate matrix by estimateFactor
            // the estimate matrix area is same as target matrix,
            // so must reset some class internal vars
            dxSize_ *= estimateFactor_;
            dySize_ *= estimateFactor_;

            size_t xCount = (size_t)std::ceil((xMinMax_.second - xMinMax_.first) / dxSize_) + 1;
            size_t yCount = (size_t)std::ceil((yMinMax_.second - yMinMax_.first) / dySize_) + 1;

            ais::Shape estimateShape = {yCount, xCount};
            estimateMatrixPtr_ = std::make_shared<ais::FMatrix>(estimateShape);
            estimateMatrixPtr_->zeros();
            generate_grid_params(estimateMatrixPtr_);

            // 2. setup estimate matrix faults impact
            // matrix which storage the points in fault for estimate
            std::shared_ptr<ais::FMatrix> estimatePointInFaultMatrixPtr;
            // matrix which storage the fault affect block values for estimate
            std::shared_ptr<ais::FMatrix> estimateFaultAffectMatrixPtr;
            build_fault_impact_matrix(estimateMatrixPtr_, estimatePointInFaultMatrixPtr, estimateFaultAffectMatrixPtr);

            // 3. setup the constrict matrix and initial estimate matrix
            std::shared_ptr<ais::PMatrix> estimateConstrainMatrixPtr;
            build_constraint_matrix(estimateMatrixPtr_, estimateConstrainMatrixPtr);

            // 4. update estimate matrix edge
            update_edge_node(estimateMatrixPtr_);

            // 5. estimate surface for estimate matrix
            estimate_gradients_surface(estimateMatrixPtr_);

            // 6. update nan nodes in estimate matrix
            update_nan_node(estimateMatrixPtr_);

            // 7. go interpolation
            start_interpolation(estimateMatrixPtr_, estimateConstrainMatrixPtr, estimatePointInFaultMatrixPtr, estimateFaultAffectMatrixPtr);

            //dump_result_async("output_estimated.grd", estimateMatrixPtr_);

            // 8. interpolate the estimate to target matrix
            interpolate_estimate_2_target();

            // release inter vars
            estimateConstrainMatrixPtr.reset();
            estimatePointInFaultMatrixPtr.reset();
            estimateFaultAffectMatrixPtr.reset();

            // finally reset minimum curvature params for real interpolation
            generate_grid_params(targetMatrixPtr_);
            //dump_result_async("estimated2target.grd", targetMatrixPtr_);
        }
    }

    void GridInterpolator::interpolate_estimate_2_target()
    {
        // function start message
        std::stringstream msg;
        msg << ais::time_str() << " " << __FUNCTION__;
        interpolationMsgs_.push_back(msg.str());

        auto tShape = targetMatrixPtr_->shape();
        auto eShape = estimateMatrixPtr_->shape();

        auto threads = size_t(std::thread::hardware_concurrency() * 1.5);
        auto rowInThread = size_t(tShape.rows / threads);
        std::vector<std::future<uint64_t>> execs;
        for (int t = 0; t < threads; t++)
        {
            // get begin and end param for each thread
            auto begin = rowInThread * t;
            // ensure the last thread fill all rest
            auto end = (t == (threads - 1)) ? tShape.rows : begin + rowInThread;

            // multi-thread execution
            execs.emplace_back(std::async(
                std::launch::async,
                [this, &tShape, &eShape](size_t begin, size_t end)
                {
                    std::atomic<size_t> failedCount = 0;
                    auto rows = tShape.rows;
                    auto cols = tShape.cols;

                    for (int j = begin; j < end; j++)
                    {
                        for (int i = 0; i < tShape.cols; i++)
                        {
                            auto pos = j * tShape.cols + i;
                            auto z = targetMatrixPtr_->at(pos);

                            if (std::isnan(z) || std::abs(z - fillValue_) < 1e-6)
                            {
                                // put the interpolate value to target matrix while the value is nan or close to fill value
                                size_t ei = std::floor(i / estimateFactor_);
                                size_t ej = std::floor(j / estimateFactor_);

                                size_t mi = i % estimateFactor_;
                                size_t mj = j % estimateFactor_;

                                double zVertical = fillValue_;
                                double p0z = estimateMatrixPtr_->at(ej, ei);

                                if (ei < (eShape.cols-1) && ej < (eShape.rows-1))
                                {
                                    // for all node in good range, use bilinear interpolation
                                    double p1z = estimateMatrixPtr_->at(ej, ei + 1);
                                    double p2z = estimateMatrixPtr_->at(ej + 1, ei + 1);
                                    double p3z = estimateMatrixPtr_->at(ej + 1, ei);

                                    // get z value by bilinear interpolation
                                    double zHorizontal2 = (p2z - p3z) * mi / estimateFactor_ + p3z;
                                    double zHorizontal1 = (p1z - p0z) * mi / estimateFactor_ + p0z;
                                    zVertical = (zHorizontal2 - zHorizontal1) * mj / estimateFactor_ + zHorizontal1;
                                }
                                else
                                {
                                    // for other node(botoom and right edge), just copy p0z point value
                                    zVertical = p0z;
                                }

                                targetMatrixPtr_->put(pos, zVertical);
                            }
                        }
                    }

                    return end - begin;
                },
                begin, end));
        }

        uint64_t rowFilled = 0;
        for (auto &e : execs)
        {
            e.wait();
            auto filledInThread = e.get();
            rowFilled += filledInThread;

            // std::string msg = "Interpolate estimate to target row: " + std::to_string(rowFilled);
            // interpolationMsgs_.push_back(msg);
        }
    }

    void GridInterpolator::estimate_gradients_surface(std::shared_ptr<ais::FMatrix> &targetMatrixPtr)
    {
        // function start message
        std::stringstream msg;
        msg << ais::time_str() << " " << __FUNCTION__;
        interpolationMsgs_.push_back(msg.str());

        std::atomic<size_t> count = 0;
        auto tmp = targetMatrixPtr;
        auto shape = targetMatrixPtr->shape();

        // before delaunay triangles, added rectangle corner points to cover all rectangle
        double rows = shape.rows;
        double cols = shape.cols;

        // add corner points with its origin value, will reset it later if the value is 0 by the gaussian filter
        Point3D p0{0, 0, tmp->at(0, 0)};
        Point3D p1{0, rows - 1, tmp->at(rows - 1, 0)};
        Point3D p2{cols - 1, rows - 1, tmp->at(rows - 1, cols - 1)};
        Point3D p3{cols - 1, 0, tmp->at(0, cols - 1)};

        randomPointPositions_.push_back(p0);
        randomPointPositions_.push_back(p1);
        randomPointPositions_.push_back(p2);
        randomPointPositions_.push_back(p3);

        std::set<size_t> cornerHashs = {
            p0.hash2D(), p1.hash2D(), p2.hash2D(), p3.hash2D()};

        // build the delaunay triangles with random point positions, which will use point's grid position to simple calculation
        // eg: point(2342.22, 1242.23, 23.3) ==> position(6.1, 42.23, 23.3)
        std::vector<Triangle<Point3D>> triangles;
        ais::delaunay_triangulation(randomPointPositions_, triangles);

        // remove triangle without z value, means the triangle contact to super-triangles
        for (auto iter = triangles.begin(); iter != triangles.end();)
        {
            auto t = (*iter);
            auto pz = t.points[0].z * t.points[1].z * t.points[2].z;

            if (std::isnan(pz) || std::isinf(pz))
                iter = triangles.erase(iter);
            else
                iter++;
        }

        // get triangles info, such as triangle points' distance min/max
        std::map<size_t, std::set<size_t>> trianglePointLinkMap;
        double distanceMax = 0.0, distanceMin = std::numeric_limits<double>::max();
        get_triangle_info(triangles, trianglePointLinkMap, distanceMin, distanceMax);

        auto updateCornerPointEstimateZ = [this, &trianglePointLinkMap, &rows, &cols, &tmp, &cornerHashs](size_t rowPos, size_t colPos)
        {
            double ret = std::nan("1");
            Point3D point{(double)colPos, (double)rowPos, INFINITY};

            auto linkPoints = trianglePointLinkMap[point.hash2D()];

            std::vector<double> estimates;

            for (auto &rp : randomPointPositions_)
            {
                if (linkPoints.find(rp.hash2D()) != linkPoints.end() && cornerHashs.find(rp.hash2D()) == cornerHashs.end())
                {
                    // estimate z value by Gaussian with no-corner linked point
                    double estimateZ = point.gaussian_interpolation_z_value(point.distance2D(rp), rp.z, rows, cols, cornerWeight_);
                    if (estimateZ > 1e-6)
                        estimates.push_back(estimateZ);
                }
            }

            if (estimates.size() > 0)
            {
                ret = std::accumulate(estimates.begin(), estimates.end(), 0.0) / estimates.size();

                // update point z for targetMatrixPtr & randomPointPositions_
                tmp->put(rowPos, colPos, ret);
                for (auto rp = randomPointPositions_.begin(); rp != randomPointPositions_.end(); rp++)
                {
                    if (rp->hash2D() == point.hash2D())
                    {
                        rp->z = ret;
                        break;
                    }
                }
            }

            return ret;
        };

        // reset the corner points' z-value to make the rect value more smooth
        double updatedZ = updateCornerPointEstimateZ(0, 0);
        updatedZ = updateCornerPointEstimateZ(rows - 1, 0);
        updatedZ = updateCornerPointEstimateZ(rows - 1, cols - 1);
        updatedZ = updateCornerPointEstimateZ(0, cols - 1);

        // reset triangles
        triangles.clear();
        ais::delaunay_triangulation(randomPointPositions_, triangles);

        // remove triangle without z value, means the triangle contact to super-triangles
        for (auto iter = triangles.begin(); iter != triangles.end();)
        {
            auto t = (*iter);
            auto pz = t.points[0].z * t.points[1].z * t.points[2].z;

            if (std::isnan(pz) || std::isinf(pz))
                iter = triangles.erase(iter);
            else
                iter++;
        }

        // get all coe for all random points
        ais::Matrix<double> allXYMatrix(randomPointPositions_.size(), 3);
        ais::Matrix<double> allZMatrix(1, randomPointPositions_.size());
        for (int pi = 0; pi < randomPointPositions_.size(); pi++)
        {
            auto &p = randomPointPositions_[pi];
            allXYMatrix.putMany(pi, 0, build_xy_data(p));
            allZMatrix.put(0, pi, p.z);
        }
        auto allCoe = ais::linalg::lstsq(allXYMatrix, allZMatrix);

        // use multi thread to speed up
        auto threads = size_t(std::thread::hardware_concurrency() * 1.5);
        // threads = 1;
        auto rowInThread = size_t(shape.rows / threads);

        std::vector<std::future<uint64_t>> execs;
        for (int t = 0; t < threads; t++)
        {
            // get begin and end param for each thread
            auto begin = rowInThread * t;
            // ensure the last thread fill all rest
            auto end = (t == (threads - 1)) ? shape.rows : begin + rowInThread;

            // multi-thread execution
            execs.emplace_back(std::async(
                std::launch::async, [this, &shape, &tmp, &count, &triangles, &allCoe](size_t begin, size_t end)
                {
                    std::atomic<size_t> failedCount = 0;
                    auto tsize = triangles.size();
                    auto rows = shape.rows;
                    auto cols = shape.cols;

                    for (size_t nodeJ = begin; nodeJ < end; nodeJ++)
                    {
                        for (size_t nodeI = 0; nodeI < shape.cols; nodeI++)
                        {
                            auto z = tmp->at(nodeJ, nodeI);
                            if (ais::isnan(z) || std::abs(z - fillValue_) < 1e-6)
                            {
                                std::vector<std::pair<double, double>> interpolations;

                                // use index as the point position to simple calculation
                                Point3D nodePoint{ (double)nodeI, (double)nodeJ, INFINITY };

                                for (int ti = 0 ; ti < tsize; ti++)
                                {
                                    auto& t = triangles[ti];
                                    if (nodePoint.in_box(t.min_max[0], t.min_max[1]) && nodePoint.in_polygon(t.polygon))
                                    {
                                        auto pa = t.points[0];
                                        auto pb = t.points[1];
                                        auto pc = t.points[2];

                                        auto da = nodePoint.distance(pa);
                                        auto db = nodePoint.distance(pb);
                                        auto dc = nodePoint.distance(pc);

                                        auto za = nodePoint.point_interpolate(pb, pc, pa);
                                        auto zb = nodePoint.point_interpolate(pc, pa, pb);
                                        auto zc = nodePoint.point_interpolate(pa, pb, pc);

                                        interpolations.push_back({ da, za });
                                        interpolations.push_back({ db, zb });
                                        interpolations.push_back({ dc, zc });

                                        break;
                                    }
                                }

                                if (interpolations.size() > 0)
                                {
                                    double avg_inter = interpolations.size() == 0 ? interpolations[0].second : ais::get_mean(interpolations);
                                    if (!std::isinf(avg_inter))
                                        tmp->put(nodeJ, nodeI, avg_inter);
                                    else
                                        tmp->put(nodeJ, nodeI, fillValue_);

                                    count++;
                                }
                                else
                                {
                                    // debug usage
                                    failedCount++;
                                }
                            }
                        }
                    }

                    return end - begin; },
                begin, end));
        }

        uint64_t rowFilled = 0;
        for (auto &e : execs)
        {
            e.wait();
            auto filledInThread = e.get();
            rowFilled += filledInThread;

            // std::string msg = "Estimated row: " + std::to_string(rowFilled);
            // interpolationMsgs_.push_back(msg);
        }

        if (count > 0)
        {
            std::string msg = "Total estimated nodes: " + std::to_string(count);
            interpolationMsgs_.push_back(msg);
        }

        // dump after estimate value
        {
            // update zMin and zMax
            zMinMax_ = getMinMax(*targetMatrixPtr);

            // std::lock_guard<std::mutex> lk(msgMutex_);
            // std::string msg = "Estimated completed";
            // interpolationMsgs_.push_back(msg);

			/*
            auto ret = std::async(std::launch::async, [&]()
                                  {
                    std::string filename = "2_Estimated_" + std::to_string(shape.rows) + "x" + std::to_string(shape.cols) + ".grd";
                    dump_result(filename.c_str());
                    return; });
					*/
        }
    }

    void GridInterpolator::estimate_gradients_surface_2(InitTargetMatrixMethod method)
    {
        //        // if (!estimateTargetMatrix_)
        //        //     return;
        //
        //        // function start message
        //        std::stringstream msg;
        //        msg << ais::time_str() << " " << __FUNCTION__;
        //        interpolationMsgs_.push_back(msg.str());
        //
        //        std::atomic<size_t> count = 0;
        //        auto tmp = targetMatrixPtr_;
        //        auto shape = targetMatrixPtr_->shape();
        //
        //        ais::FMatrix updateMatrix(shape);
        //        updateMatrix.nans();
        //
        //        // storage the constrict values to map
        //        std::map<size_t, double> constrictValues;
        //        for (size_t i = 0; i < shape.size(); i++)
        //        {
        //            double value = tmp->at(i);
        //            if (!std::isnan(value) && value != 0.0)
        //                constrictValues[i] = value;
        //        }
        //
        //        // build the delaunay triangles with random point positions, which will use point's grid position to simple calculation
        //        // eg: p(2342.22, 1242.23, 23.3) ==> p(12.2, 42.23, 23.3)
        //        std::vector<Triangle<Point3D>> triangles;
        //        ais::delaunay_triangulation(randomPointPositions_, triangles);
        //
        //        // get all coe for all random points
        //        ais::Matrix<double> allXYMatrix(randomPointPositions_.size(), 3);
        //        ais::Matrix<double> allZMatrix(1, randomPointPositions_.size());
        //        for (int pi = 0; pi < randomPointPositions_.size(); pi++)
        //        {
        //            auto & p = randomPointPositions_[pi];
        //            allXYMatrix.putMany(pi, 0, build_xy_data(p));
        //            allZMatrix.put(0, pi, p.z);
        //        }
        //        auto allCoe = ais::linalg::lstsq(allXYMatrix, allZMatrix);
        //
        //        // remove triangle without z value, means the triangle contact to super-triangles
        //        for (auto iter = triangles.begin(); iter != triangles.end();)
        //        {
        //            auto t = (*iter);
        //            auto pz = t.points[0].z * t.points[1].z * t.points[2].z;
        //
        //            if (std::isnan(pz) || std::isinf(pz))
        //                iter = triangles.erase(iter);
        //            else
        //                iter++;
        //        }
        //
        //        std::map<size_t, std::set<size_t>> trianglePointLinkMap;
        //        double distanceMax = 0.0, distanceMin = std::numeric_limits<double>::max();
        //        get_triangle_info(triangles, trianglePointLinkMap, distanceMin, distanceMax);
        //
        //
        //        // use multi thread to speed up
        //        auto threads = size_t(std::thread::hardware_concurrency() * 1.5);
        // #if defined(_DEBUG)
        //        //threads = 1;      // debug usage
        // #endif
        //        auto rowInThread = size_t(shape.rows / threads);
        //
        //        bool allFilled = false;
        //
        //        // check the target matrix have been filled or not
        //        {
        //            std::lock_guard<std::mutex> lk(dataMutex_);
        //            auto zeroCount = std::count(std::execution::par_unseq, tmp->begin(), tmp->end(), 0.0);
        //            allFilled = all_filled(tmp, 0, shape.size());
        //        }
        //
        //        while (!allFilled)
        //        {
        //            // copy target matrix data to update matrix
        //            {
        //                std::lock_guard<std::mutex> lk(dataMutex_);
        //                updateMatrix.putMany(0, *tmp);
        //            }
        //
        //            std::vector<std::future<uint64_t>> execs;
        //            for (int t = 0; t < threads; t++)
        //            {
        //                // get begin and end param for each thread
        //                auto begin = rowInThread * t;
        //                // ensure the last thread fill all rest
        //                auto end = (t == (threads - 1)) ? shape.rows : begin + rowInThread;
        //
        //                //============== Solution 5, MA Gaussian filter for all point =========================
        //                execs.emplace_back(std::async(
        //                    std::launch::async, [this, &shape, &tmp, &count, &constrictValues, &updateMatrix](size_t begin, size_t end)
        //                    {
        //                        for (size_t j = begin; j < end; j++)
        //                        {
        //                            for (size_t i = 0; i < shape.cols; i++)
        //                            {
        //                                // use index as the point position to simple calculation
        //                                Point3D girdPoint{ (double)i, (double)j };
        //                                size_t pos = j * shape.cols + i;
        //
        //                                double z = tmp->at(pos);
        //                                if (ais::isnan(z) || constrictValues.find(pos) == constrictValues.end())
        //                                {
        //                                    // update all nodes which is not in constrict values
        //                                    z = ma_gaussian_filter(i, j, tmp, shape);
        //                                    if(!std::isnan(z))
        //                                        updateMatrix.put(pos, z);
        //                                }
        //                                else
        //                                {
        //                                    // for all constrict values, just copy it
        //                                    updateMatrix.put(pos, z);
        //                                }
        //                            }
        //                        }
        //
        //                        return end - begin;
        //                    },
        //                    begin, end));
        //            }
        //
        //            // wait for multi-thread exec
        //            uint64_t rowFilled = 0;
        //            for (auto& e : execs)
        //            {
        //                e.wait();
        //                auto filledInThread = e.get();
        //                rowFilled += filledInThread;
        //            }
        //
        //            std::string msg = "Completed loop: " + std::to_string(count);
        //            interpolationMsgs_.push_back(msg);
        //
        //            // copy the update matrix to target
        //            {
        //                std::lock_guard<std::mutex> lk(dataMutex_);
        //                tmp->putMany(0, updateMatrix);
        //                auto zeroCount = std::count(std::execution::par_unseq, tmp->begin(), tmp->end(), 0.0);
        //                allFilled = all_filled(tmp, 0, shape.size());
        //            }
        //            count++;
        //        }
        //
        //        if (count > 0)
        //        {
        //            std::string msg = "Total estimated loops: " + std::to_string(count);
        //            interpolationMsgs_.push_back(msg);
        //        }
    }

    void GridInterpolator::build_fault_impact_matrix(std::shared_ptr<ais::FMatrix> targetMatrixPtr,
                                                     std::shared_ptr<ais::FMatrix> &pointInFaultMatrixPtr, std::shared_ptr<ais::FMatrix> &faultAffectMatrixPtr)
    {
        // do nothing while the fault is empty
        if (faults_.empty())
            return;

        std::stringstream msg;
        msg << ais::time_str() << " " << __FUNCTION__;
        interpolationMsgs_.push_back(msg.str());

        // pointInFaultMatrix means for all grid points which are in the fault
        auto shape = targetMatrixPtr->shape();
        if (!pointInFaultMatrixPtr.get())
            pointInFaultMatrixPtr = std::make_shared<ais::FMatrix>(shape);

        // faultAffectMatrix means for all grid points which iteration block have been affect fault
        if (!faultAffectMatrixPtr.get())
        {
            faultAffectMatrixPtr = std::make_shared<ais::FMatrix>(shape.rows + 4, shape.cols + 4);
            faultAffectMatrixPtr->zeros();
        }
        // build up point-in-fault matrix, used for add fault line on output grid
        for (size_t j = 0; j < shape.rows; j++)
        {
            for (size_t i = 0; i < shape.cols; i++)
            {
                // build the node point
                auto x = xMinMax_.first + i * dxSize_;
                auto y = yMinMax_.first + j * dySize_;
                auto ui = i + 2;
                auto uj = j + 2;
                ais::Point p{x, y};

                // loop fault to check the node point in fault or not
                for (auto &fault : faults_)
                {
                    // use loose test(in_polygon(controlPoints=0) for identification the node point in fault as more as possible
                    if (p.in_box(fault.box) && (p.in_polylines(fault.points) || p.in_polygon(fault.polygon)))
                    {
                        // build a fault point box for fault line constrict
                        std::vector<ais::Point> faultPointBox = {
                            {x - 0.5 * dxSize_, y - 0.5 * dySize_},
                            {x - 0.5 * dxSize_, y + 0.5 * dySize_},
                            {x + 0.5 * dxSize_, y + 0.5 * dySize_},
                            {x + 0.5 * dxSize_, y - 0.5 * dySize_}};

                        int hitCount = 0;
                        for (auto &fpb : faultPointBox)
                        {
                            if (fpb.in_polygon(fault.polygon))
                                hitCount++;
                        }

                        // node point in fault, flag it and break loop for next point
                        faultAffectMatrixPtr->put(uj, ui, double(1 << hitCount));
                        // the faults will not overlapped, so don't test anymore when hit one fault, just break loop
                        break;
                    }
                }
            }
        }

        // block point check position
        // iterated points which need be analysis are below
        //                0,2
        //        -1,1    0,1    1,1
        // -2,0   -1,0    0,0    1,0    2,0
        //        -1,-1   0,-1   1,-1
        //                0,-2
        const std::vector<std::pair<int, int>> insidePoint1 = {{0, 1}, {-1, 0}, {1, 0}, {0, -1}};
        const std::vector<std::pair<int, int>> insidePoint2 = {{-1, 1}, {-1, -1}, {1, 1}, {1, -1}};

        const std::vector<std::pair<int, int>> insidePoint3 = {
            {-2, 2}, {-2, 1}, {-2, 0}, {-2, -1}, {-2, -2}, {-1, 2}, {-1, -2}, {0, 2}, {0, -2}, {1, 2}, {1, -2}, {2, 2}, {2, 1}, {2, 0}, {2, -1}, {2, -2}};

        // get block point fault tag value
        auto pointFaultTagValue = [](const std::shared_ptr<ais::FMatrix> pifMatrixPtr, size_t i, size_t j, const std::pair<int, int> &p, bool force = false)
        {
            long ret = 0;
            auto shape = pifMatrixPtr->shape();
            auto pos = (j + 2 + p.second) * shape.cols + i + 2 + p.first;
            auto pInFault = pifMatrixPtr->at(pos);
            if (pInFault > 0.0 || force)
            {
                ret = 1 << ((p.second + 2) * 5 + (p.first + 2));
            }

            return ret;
        };

        // get point fault tag for faults
        auto pointFaultAffectValue = [&insidePoint1, &insidePoint2, &insidePoint3, this, &pointFaultTagValue](const std::shared_ptr<ais::FMatrix> pifMatrixPtr, size_t i, size_t j, const ais::Fault &fault)
        {
            long ret = 0;

            auto &fps = fault.points;
            size_t m = fault.points.size();

            auto x = xMinMax_.first + i * dxSize_;
            auto y = yMinMax_.first + j * dySize_;
            ais::Point p1{x, y};

            auto faultBoxMin = fault.box[0];
            auto faultBoxMax = fault.box[2];

            if (x < (faultBoxMin.x - 2 * dxSize_) || x > (faultBoxMax.x + 2 * dxSize_) ||
                y < (faultBoxMin.y - 2 * dySize_) || y > (faultBoxMax.y + 2 * dySize_))
            {
                // any point out of fault box will skip it.
                return ret;
            }

            // check insidePoint1
            for (auto &p : insidePoint1)
            {
                // loop inside nodes to build the line p(0,0) --- p(inside point)
                auto x2 = xMinMax_.first + (i + p.first) * dxSize_;
                auto y2 = yMinMax_.first + (j + p.second) * dySize_;
                ais::Point p2{x2, y2};

                for (auto mi = 0; mi < m - 1; mi++)
                    // loop fault line points to build another line
                    if (line_cross(p1, p2, fps[mi], fps[((mi + 1) % m)]))
                    {
                        // the fault node line cross fault line
                        ret |= pointFaultTagValue(pifMatrixPtr, i, j, p, true);

                        // for expend point
                        if (p.first != 0)
                        {
                            // for x axis
                            auto newX = p.first > 0 ? 2 : -2;
                            auto pExtPos = (p.second + 2) * 5 + (newX + 2);
                            ret |= 1 << pExtPos;
                        }
                        else
                        {
                            // for y axis
                            auto newY = p.second > 0 ? 2 : -2;
                            auto pExtPos = (newY + 2) * 5 + (p.first + 2);
                            ret |= 1 << pExtPos;
                        }

                        // bingo, the line p1--p2 cross fault, break loop check next
                        break;
                    }
            }

            // check insidePoint2
            for (auto &p : insidePoint2)
            {
                // loop inside nodes to build the line p(0,0) --- p(inside point)
                auto x2 = xMinMax_.first + (i + p.first) * dxSize_;
                auto y2 = yMinMax_.first + (j + p.second) * dySize_;
                ais::Point p2{x2, y2};

                for (auto mi = 0; mi < m - 1; mi++)
                    // loop fault line points to build another line
                    if (line_cross(p1, p2, fps[mi], fps[((mi + 1) % m)]))
                    {
                        // the fault box node line cross fault line
                        ret |= pointFaultTagValue(pifMatrixPtr, i, j, p, true);

                        // for expend point
                        auto newX = p.first > 0 ? 2 : -2;
                        auto newY = p.second > 0 ? 2 : -2;
                        auto pExtPos = (newY + 2) * 5 + (newX + 2);

                        ret |= 1 << pExtPos;

                        // bingo, the line p1--p2 cross fault, break loop check next
                        break;
                    }
            }

            // check insidePoint3
            for (auto &p : insidePoint3)
            {
                if (ret | pointFaultTagValue(pifMatrixPtr, i, j, p, true))
                {
                    // already have been setup, skip and continue
                    continue;
                }

                auto x2 = xMinMax_.first + (i + p.first) * dxSize_;
                auto y2 = yMinMax_.first + (j + p.second) * dySize_;
                ais::Point p2{x2, y2};

                for (auto mi = 0; mi < m - 1; mi++)
                    if (line_cross(p1, p2, fps[mi], fps[((mi + 1) % m)]))
                    {
                        ret |= pointFaultTagValue(pifMatrixPtr, i, j, p, true);
                    }
            }

            return ret;
        };

        // build up pointInFaultMatrixPtr_
        for (size_t j = 0; j < shape.rows; j++)
        {
            for (size_t i = 0; i < shape.cols; i++)
            {
                // 0 means the point not effected by fault
                long tempZ = 0;

                // check center point first
                tempZ |= pointFaultTagValue(faultAffectMatrixPtr, i, j, {0, 0});
                if (tempZ > 0)
                {
                    // central point in the fault, break fault loop to flag it and check next point
                    pointInFaultMatrixPtr->put(j, i, double(tempZ));
                    continue;
                }

                // 1. for node points in fault polygon
                for (auto &p : insidePoint1)
                {
                    // check inner point, if in polygon, flag it and it's expanded point
                    auto pValue = pointFaultTagValue(faultAffectMatrixPtr, i, j, p);
                    if (pValue > 0)
                    {
                        tempZ |= pValue;
                        if (p.first != 0)
                        {
                            auto newX = p.first > 0 ? 2 : -2;
                            auto pExtPos = (p.second + 2) * 5 + (newX + 2);
                            tempZ |= 1 << pExtPos;
                        }
                        else
                        {
                            auto newY = p.second > 0 ? 2 : -2;
                            auto pExtPos = (newY + 2) * 5 + (p.first + 2);
                            tempZ |= 1 << pExtPos;
                        }
                    }
                }
                // if (j == 470 && i == 495)
                //{
                //     std::cerr << "After pass point1 Point(470, 495): " << tempZ << std::endl;
                // }

                for (auto &p : insidePoint2)
                {
                    // check inner point, if in polygon, flag it and it's expanded point
                    auto pValue = pointFaultTagValue(faultAffectMatrixPtr, i, j, p);
                    if (pValue > 0)
                    {
                        tempZ |= pValue;

                        auto newX = p.first > 0 ? 2 : -2;
                        auto newY = p.second > 0 ? 2 : -2;
                        auto pExtPos = (newY + 2) * 5 + (newX + 2);
                        tempZ |= 1 << pExtPos;
                    }
                }
                if (j == 470 && i == 495)
                {
                    std::cerr << "After pass point2 Point(470, 495): " << tempZ << std::endl;
                }

                for (auto &p : insidePoint3)
                {
                    tempZ |= pointFaultTagValue(faultAffectMatrixPtr, i, j, p);
                }
                if (j == 470 && i == 495)
                {
                    std::cerr << "After pass point3 Point(470, 495): " << tempZ << std::endl;
                }

                // 2. for fault line cross block but not on the point
                for (auto &fault : faults_)
                {
                    tempZ |= pointFaultAffectValue(faultAffectMatrixPtr, i, j, fault);
                }
                if (j == 470 && i == 495)
                {
                    std::cerr << "After pass line cross check Point(470, 495): " << tempZ << std::endl;
                }

                // finally put the point fault effect to the matrix
                pointInFaultMatrixPtr->put(j, i, double(tempZ));
            }
        }
    }

    void GridInterpolator::start_interpolation(std::shared_ptr<ais::FMatrix> &targetMatrixPtr, std::shared_ptr<ais::PMatrix> &constrainMatrixPtr,
                                               std::shared_ptr<ais::FMatrix> &pointInFaultMatrixPtr, std::shared_ptr<ais::FMatrix> &faultAffectMatrixPtr,
                                               InterpolationMethod method)
    {
        if (method != InterpolationMethod::MINIMUM_CURVATURE)
            THROW_INVALID_ARGUMENT("Only support minimum curvature interpolations method right now.");

        startTime_ = std::chrono::system_clock::now();

        // setup running flag
        flagRunning_ = true;

        // selected interpolation method to start working thread
        update_min_curv_interpolation(targetMatrixPtr, constrainMatrixPtr, pointInFaultMatrixPtr, faultAffectMatrixPtr);
    }

    bool GridInterpolator::update_min_curv_interpolation(std::shared_ptr<ais::FMatrix> &targetMatrixPtr, std::shared_ptr<ais::PMatrix> &constrainMatrixPtr,
                                                         std::shared_ptr<ais::FMatrix> &pointInFaultMatrixPtr, std::shared_ptr<ais::FMatrix> &faultAffectMatrixPtr,
                                                         const std::vector<ais::Point> &targetArea)
    {
        {
            std::lock_guard<std::mutex> lk(msgMutex_);
            interpolationMsgs_.push_back(ais::time_str() + " Start interpolations:");
            interpolationMsgs_.push_back("=======================================");
        }

        // build a expansion matrix for minimum curvature interpolation
        // due to the principle of minimum curvature interpolation,
        // we need to expand 2 additional points on each edge of the target matrix
        // PS: all matrix object in this function should alloc in heap, due the matrix is very big
        auto shape = targetMatrixPtr->shape();
        auto updateMatrixPtr = std::make_shared<ais::FMatrix>(shape.rows + 4, shape.cols + 4);
        updateMatrixPtr->emptys();

        // build a interpolation params matrix
        auto interpolationParamsPtr = std::make_shared<ais::Matrix<ais::MCI_APoints>>(shape);
        interpolationParamsPtr->emptys();

        auto sPtr = std::make_shared<ais::FMatrix>(shape);
        auto tPtr = std::make_shared<ais::FMatrix>(shape);
        sPtr->zeros();
        tPtr->zeros();

        // origin matrix reference for share_ptr<ais::Matrix>
        auto &umo = *updateMatrixPtr;

        auto &cmp = *constrainMatrixPtr;

        // calc interpolation alpha
        double alpha0 = -1.0 / (6 + 8 * alpha_ * alpha_ + 6 * (std::pow(alpha_, 4)));
        double alpha1 = std::pow(alpha_, 4);
        double alpha2 = 2 * (alpha_ * alpha_);
        double alpha3 = -4 * (1 + alpha_ * alpha_);
        double alpha4 = -4 * (1 + alpha_ * alpha_) * alpha_ * alpha_;
        double alpha5 = -2 * (1 + alpha_ * alpha_);
        double alpha6 = -2 * (1 + alpha_ * alpha_) * alpha_ * alpha_;

        double beta1 = 2 * std::pow((1 + alpha_ * alpha_), 2);
        double beta2 = (1 + std::pow(alpha_, 4));

        // loop constraint matrix to get the interpolated params
        for (int j = 0; j < shape.rows; j++)
        {
            for (int i = 0; i < shape.cols; i++)
            {
                auto cp = cmp(j, i);
                if (!ais::isnan(cp))
                {
                    sPtr->at(j, i) = cp.x > 0 ? 1 : -1;
                    tPtr->at(j, i) = cp.y > 0 ? 1 : -1;

                    ais::PointXYZ up{std::abs(cp.x / dxSize_), std::abs(cp.y / dySize_), cp.z};
                    constrainMatrixPtr->put(j, i, up);

                    double denominator = beta2 * (up.x + up.y) * (1 + up.x + up.y) +
                                         beta1 * (up.x * up.y + up.x + up.y + 1);
                    denominator *= 2.0;

                    double a0 = -(up.x + up.y) * (1 + up.x + up.y) / denominator;
                    double a1 = beta1 * up.x * (1 + up.x + 2 * up.y) / denominator;
                    double a2 = -2 * beta1 * up.x * (up.x + up.y) / denominator;
                    double a3 = -2 * beta1 * up.y * (up.x + up.y) / denominator;
                    double a4 = -beta1 * up.x * (1 + up.x) / denominator;
                    double a5 = 2 * beta1 / denominator;

                    interpolationParamsPtr->put(j, i, {a0, a1, a2, a3, a4, a5});
                }
            }
        }

        // copy original data to expected matrix for interpolation
        for (int j = 0; j < shape.rows; j++)
        {
            for (int i = 0; i < shape.cols; i++)
            {
                auto zValue = targetMatrixPtr->at(j, i);
                umo(j + 2, i + 2) = zValue;
            }
        }

        // setup the starting values for interpolation
        double epsError = std::abs(epsMax_) * 2;
        iteration_ = 0;

        // setup the thread start row index vector
        std::list<size_t> threadRowIndex;
        auto threadRowStep = 6;
        for (size_t tri = 0; tri < shape.rows; tri += threadRowStep)
        {
            threadRowIndex.push_back(tri);
        }

        auto copy_to_target_matrix = [&](const ais::FMatrix &source)
        {
            std::lock_guard<std::mutex> lk(dataMutex_);

            // copy interpolation value to target matrix
            for (int j = 0; j < shape.rows; j++)
            {
                for (int i = 0; i < shape.cols; i++)
                {
                    targetMatrixPtr->at(j, i) = source(j + 2, i + 2);
                }
            }
        };

        // thread function
        std::function<double(size_t, size_t, const ais::Shape &)> iterationWorker = [&umo, &cmp, &interpolationParamsPtr,
                                                                                     &sPtr, &tPtr, this,
                                                                                     &alpha0, &alpha1, &alpha2, &alpha3,
                                                                                     &alpha4, &alpha5, &alpha6](size_t rowStart, size_t step, const ais::Shape &shape)
        {
            size_t rowEnd = rowStart + step > shape.rows ? shape.rows : rowStart + step;

            double epsErrorInThread = 0.0;

            for (size_t j = rowStart; j < rowEnd; j++)
            {
                for (size_t i = 0; i < shape.cols; i++)
                {
                    auto ui = i + 2;
                    auto uj = j + 2;
                    auto cp = cmp(j, i);
                    double temp = 0;

                    if (_LIKELY(std::isnan(cp.z)))
                    {
                        // WITHOUT constrain point
                        temp = (umo(uj, ui + 2) + umo(uj, ui - 2)) +
                               alpha1 * (umo(uj + 2, ui) + umo(uj - 2, ui)) +
                               alpha2 * (umo(uj + 1, ui + 1) + umo(uj + 1, ui - 1) + umo(uj - 1, ui + 1) + umo(uj - 1, ui - 1)) +
                               alpha3 * (umo(uj, ui + 1) + umo(uj, ui - 1)) +
                               alpha4 * (umo(uj + 1, ui) + umo(uj - 1, ui));
                        temp *= alpha0;
                    }
                    else
                    {
                        // with constrain point
                        auto ias = interpolationParamsPtr->at(j, i);
                        auto s = sPtr->at(j, i);
                        auto t = tPtr->at(j, i);

                        temp = (umo(uj, ui + 2) + umo(uj, ui - 2)) +
                                alpha1 * (umo(uj + 2, ui) + umo(uj - 2, ui)) + 
                                alpha2 * (umo(uj + 1, ui + 1) + umo(uj + 1, ui - 1) + umo(uj - 1, ui + 1) + umo(uj - 1, ui - 1)) +
                                alpha5 * (umo(uj, ui + 1) + umo(uj, ui - 1)) + alpha6 * (umo(uj + 1, ui) + umo(uj - 1, ui));
                        temp *= ias[0];
                        temp += ias[0] * alpha5 * (-umo(uj + t, ui - s) + 2 * umo(uj, ui - s) + alpha2 * umo(uj - t, ui) + umo(uj - t, ui + s)) +
                                ias[1] * umo(uj + t, ui - s) + ias[2] * umo(uj, ui - s) + 
                                ias[3] * umo(uj - t, ui) + ias[4] * umo(uj - t, ui + s) + 
                                ias[5] * cp.z;
                    }

                    // over relaxation adjustments
                    temp = temp * 1.4 - 0.4 * umo(uj, ui);

                    epsErrorInThread = std::max(std::abs(temp - umo(uj, ui)), epsErrorInThread);
                    {
                        umo(uj, ui) = temp;
                    }
                }
            }

            return epsErrorInThread;
        };

        // thread function with fault
        std::function<double(size_t, size_t, const ais::Shape &)> iterationFaultWorker = [&umo, &cmp, &interpolationParamsPtr,
                                                                                          &sPtr, &tPtr, this,
                                                                                          &alpha0, &alpha1, &alpha2, &alpha3,
                                                                                          &alpha4, &alpha5, &alpha6](size_t rowStart, size_t step, const ais::Shape &shape)
        {
            size_t rowEnd = rowStart + step > shape.rows ? shape.rows : rowStart + step;

            double epsErrorInThread = 0.0;

            for (size_t j = rowStart; j < rowEnd; j++)
            {
                for (size_t i = 0; i < shape.cols; i++)
                {
                    auto ui = i + 2;
                    auto uj = j + 2;
                    auto cp = cmp(j, i);

                    // check fault condition
                    long fpz = long(pointInFaultMatrixPtr_->at(j, i));

                    double temp = 0.0;

                    if (fpz > 0)
                    {
                        // fault inspected node, build the block data for iteration

                        // copy umo data to a block
                        ais::GridInterpolatorBlock blockData(umo, uj, ui);
                        auto pos = umo.index(uj, ui);

                        // use pre-iter block if existed
                        ais::GridInterpolatorBlock<> blockIter(fillValue_);
                        ais::GridInterpolatorBlock<> &block = blockIter;

                        if (faultEdgePointExpandBoxes_.find(pos) != faultEdgePointExpandBoxes_.cend())
                        {
                            std::lock_guard<std::mutex> lk(mapMutex_);
                            block = faultEdgePointExpandBoxes_[pos];
                        }
                        else
                        {
                            // make init values for block
                            for (int index = 0; index < 25; index++)
                            {
                                if (fpz & (1 << index))
                                {
                                    // build init value for block nodes
                                    block[index] = blockData.mock_value9(fpz, index, alpha_);
                                }
                            }

                            // if(j == 470 && i == 495)
                            //{
                            //     std::cerr << "-----------------------------------------" << std::endl;
                            //     std::cerr << "Point: " << pos << " Mask: " << fpz << std::endl;
                            //     block.print();
                            //     std::cerr << "-----------------------------------------" << std::endl;
                            // }
                        }

                        // Due to the fault mock value based on the pre-data value, so split this operations into 2 loop, don't merge them
                        // 1. copy pre-data to block
                        for (int index = 0; index < 25; index++)
                        {
                            if ((fpz & (1 << index)) == 0)
                            {
                                // copy non-fault value from origin data
                                block[index] = blockData[index];
                            }
                        }

                        if (j == 470 && i == 495 && iteration_ < 100)
                        {
                            std::cerr << "--------------Before Mock-------------" << std::endl;
                            std::cerr << "Point: " << pos << " Mask: " << fpz << " Iteration: " << iteration_ << std::endl;
                            block.print();
                        }

                        // 2. mock fault inspect node in block
                        if (faultEdgePointExpandBoxes_.find(pos) != faultEdgePointExpandBoxes_.cend())
                        {
                            for (int index = 0; index < 25; index++)
                            {
                                if (fpz & (1 << index))
                                {
                                    // block[index] = block.mock_value(fpz);
                                    block[index] = block.mock_value4(fpz, index);
                                    // block[index] = block.mock_value10(fpz, index, alpha_);
                                }
                            }

                            if (j == 470 && i == 495 && iteration_ < 100)
                            {
                                std::cerr << "--------------After Mock-------------" << std::endl;
                                std::cerr << "Point: " << pos << " Mask: " << fpz << " Iteration: " << iteration_ << std::endl;
                                block.print();
                            }
                        }

                        // storage the block to map, for next iteration
                        {
                            std::lock_guard<std::mutex> lk(mapMutex_);
                            faultEdgePointExpandBoxes_[pos] = block;
                        }
                        // use data block to calculate node value
                        if (std::isnan(cp.z))
                        {
                            // WITHOUT constrain point
                            temp = (block(0, 2) + block(0, -2)) +
                                   alpha1 * (block(2, 0) + block(-2, 0)) +
                                   alpha2 * (block(1, 1) + block(1, -1) + block(-1, 1) + block(-1, -1)) +
                                   alpha3 * (block(0, 1) + block(0, -1)) +
                                   alpha4 * (block(1, 0) + block(-1, 0));
                            temp *= alpha0;
                        }
                        else
                        {
                            // with constrain point
                            auto ias = interpolationParamsPtr->at(j, i);
                            auto s = sPtr->at(j, i);
                            auto t = tPtr->at(j, i);

                            temp = (block(0, 2) + block(0, -2));
                            temp += alpha1 * (block(2, 0) + block(-2, 0));
                            temp += alpha2 * (block(1, 1) + block(1, -1) + block(-1, 1) + block(-1, -1));
                            temp += alpha5 * (block(0, 1) + block(0, -1));
                            temp += alpha6 * (block(1, 0) + block(-1, 0));
                            temp *= ias[0];
                            temp += ias[0] * alpha5 * (-block(t, -s) + 2 * block(0, -s) + alpha2 * block(-t, 0) + block(-t, s));
                            temp += ias[1] * block(t, -s) + ias[2] * block(0, -s) + ias[3] * block(-t, 0) + ias[4] * block(-t, s) + ias[5] * cp.z;
                        }

                        // flash in-fault (0,0) point in map for next iteration
                        if (fpz & (1 << 12))
                        {
                            std::lock_guard<std::mutex> lk(mapMutex_);
                            faultEdgePointExpandBoxes_[pos][12] = temp * 1.4 - 0.4 * umo(uj, ui);
                        }
                    }
                    else
                    {
                        // normal node
                        if (std::isnan(cp.z))
                        {
                            // WITHOUT constrain point
                            temp = (umo(uj, ui + 2) + umo(uj, ui - 2)) +
                                   alpha1 * (umo(uj + 2, ui) + umo(uj - 2, ui)) +
                                   alpha2 * (umo(uj + 1, ui + 1) + umo(uj + 1, ui - 1) + umo(uj - 1, ui + 1) + umo(uj - 1, ui - 1)) +
                                   alpha3 * (umo(uj, ui + 1) + umo(uj, ui - 1)) +
                                   alpha4 * (umo(uj + 1, ui) + umo(uj - 1, ui));
                            temp *= alpha0;
                        }
                        else
                        {
                            // with constrain point
                            auto ias = interpolationParamsPtr->at(j, i);
                            auto s = sPtr->at(j, i);
                            auto t = tPtr->at(j, i);

                            temp = (umo(uj, ui + 2) + umo(uj, ui - 2)) +
                                alpha1 * (umo(uj + 2, ui) + umo(uj - 2, ui)) +
                                alpha2 * (umo(uj + 1, ui + 1) + umo(uj + 1, ui - 1) + umo(uj - 1, ui + 1) + umo(uj - 1, ui - 1)) +
                                alpha5 * (umo(uj, ui + 1) + umo(uj, ui - 1)) + alpha6 * (umo(uj + 1, ui) + umo(uj - 1, ui));
                            temp *= ias[0];
                            temp += ias[0] * alpha5 * (-umo(uj + t, ui - s) + 2 * umo(uj, ui - s) + alpha2 * umo(uj - t, ui) + umo(uj - t, ui + s)) +
                                ias[1] * umo(uj + t, ui - s) + ias[2] * umo(uj, ui - s) +
                                ias[3] * umo(uj - t, ui) + ias[4] * umo(uj - t, ui + s) +
                                ias[5] * cp.z;
                        }
                    }

                    // over relaxation adjustments
                    temp = temp * 1.4 - 0.4 * umo(uj, ui);

                    epsErrorInThread = std::max(std::abs(temp - umo(uj, ui)), epsErrorInThread);
                    {
                        umo(uj, ui) = temp;
                    }
                }
            }

            return epsErrorInThread;
        };

        // dump init value
        {

            // copy middle iteration result to target
            copy_to_target_matrix(umo);

            // update zMin and zMax
            zMinMax_ = getMinMax(*targetMatrixPtr);

            std::lock_guard<std::mutex> lk(msgMutex_);
            std::string msg = "Iterating: " + std::to_string(iteration_);
            interpolationMsgs_.push_back(msg);

            //auto ret = std::async(std::launch::async, [&]()
            //                      { std::string filename = "init_0.grd";
            //dump_result(filename.c_str());
            //return; });
        }

        // main interpolation loop
        while (epsError > epsMax_ && iteration_ < maxIteration_ && flagRunning_)
        {
            epsError = 0.0;
            update_min_curv_boundary(updateMatrixPtr, shape.cols, shape.rows, alpha_);

            // dump updated boundary value
            if (iteration_ == 0)
            {

                // copy middle iteration result to target
                copy_to_target_matrix(umo);

                // update zMin and zMax
                zMinMax_ = getMinMax(*targetMatrixPtr);

                std::lock_guard<std::mutex> lk(msgMutex_);
                std::string msg = "Iterating: " + std::to_string(iteration_);
                interpolationMsgs_.push_back(msg);

                //auto ret = std::async(std::launch::async, [&]()
                //                      { std::string filename = "updated_edge_0.grd";
                //dump_result(filename.c_str());
                //return; });
            }

            if (flagMultiThread_)
            {
                // ********* multi-thread *********
                std::list<size_t> rowIndex = threadRowIndex;
                std::vector<double> threadEpsErrors;
                std::vector<std::future<double>> epsFutures;

                // start threads
                while (!rowIndex.empty())
                {
                    size_t rowStart = rowIndex.front();
                    rowIndex.pop_front();

                    if (faults_.empty())
                    {
                        epsFutures.emplace_back(std::async(std::launch::async, iterationWorker, rowStart, threadRowStep, shape));
                    }
                    else
                    {
                        epsFutures.emplace_back(std::async(std::launch::async, iterationFaultWorker, rowStart, threadRowStep, shape));
                    }
                }

                // waiting for results
                for (auto &ef : epsFutures)
                {
                    ef.wait();
                    threadEpsErrors.push_back(ef.get());
                }

                epsError = *std::max_element(std::execution::par_unseq, threadEpsErrors.begin(), threadEpsErrors.end());
            }
            else
            {
                // ********* 1 thread async iterative *********
                // auto worker = std::async(std::launch::async, iterationWorker, 0, shape.rows, shape);
                // worker.wait();
                // epsError = worker.get();

                // ********* main thread sync iterative *********

                if (faults_.empty())
                {
                    epsError = iterationWorker(0, shape.rows, shape);
                }
                else
                {
                    epsError = iterationFaultWorker(0, shape.rows, shape);
                }
            }

            // dump progress while every 100 iteration
            if (iteration_ % 100 == 0)
            {

                // copy middle iteration result to target
                copy_to_target_matrix(umo);

                // update zMin and zMax
                zMinMax_ = getMinMax(*targetMatrixPtr);

                std::lock_guard<std::mutex> lk(msgMutex_);
                std::string msg = "Iterating: " + std::to_string(iteration_);
                interpolationMsgs_.push_back(msg);

                //auto ret = std::async(std::launch::async, [&]()
                std::string filename = "progress_" + std::to_string(iteration_) + ".grd";
                int8_t level = iteration_ % 1000 == 0 ? 1 : 2;
                //dump_result_async(filename, targetMatrixPtr_, level);
            }

            iteration_++;
        }

        {
            std::lock_guard<std::mutex> lk(msgMutex_);
            interpolationMsgs_.push_back("=======================================");
            interpolationMsgs_.push_back(ais::time_str() + " Completed interpolations:\n");
        }

        // copy to target and update zMin and zMax
        copy_to_target_matrix(umo);
        zMinMax_ = getMinMax(*targetMatrixPtr);

        // setup fault point, due to the surfer will not set points to infinity, not sure this behavior make reasonable
        if (!faults_.empty() && faultEdgeLevel_ >= 0)
        {
            int faultEdgeLimitation = 1 << faultEdgeLevel_;

            for (size_t j = 0; j < shape.rows; j++)
            {
                for (size_t i = 0; i < shape.cols; i++)
                {
                    auto pointInFault = faultAffectMatrixPtr_->at(j + 2, i + 2);
                    if (pointInFault >= faultEdgeLimitation)
                    {
                        targetMatrixPtr->put(j, i, INFINITY);
                        faultNodesCount_++;
                    }
                }
            }
        }

        endTime_ = std::chrono::system_clock::now();

        // std::cout << "------------------End of min curv interpolation-----------------" << std::endl;
        // std::cout << data_info();

        // release unused memory
        updateMatrixPtr.reset();
        interpolationParamsPtr.reset();
        pointInFaultMatrixPtr_.reset();
        faultAffectMatrixPtr_.reset();
        sPtr.reset();
        tPtr.reset();

        flagSucceed_ = (iteration_ < maxIteration_);
        return flagSucceed_;
    }

    void GridInterpolator::update_min_curv_boundary(std::shared_ptr<ais::FMatrix> &updateMatrixPtr, size_t column, size_t row, double alpha)
    {
        // updateMatrixPtr are extened matrix, [0:x+3, 0:y+3],
        // extened 2 point at each side of the matrix
        //
        //  e, e, e, e, ~, e, e,  e,  e | y+3
        //  e, i, i, i, ~ ,i ,i,  i,  e | y+2
        //  e ,i ,0 ,0, ~, 0 ,0,  i,  e | y+1
        //  e ,i ,0 ,0, ~, 0 ,0,  i,  e | y
        //  ...              ...        |
        //  e, i, 0, 0, ~, 0, 0,  i,  e | 2
        //  e, i, i, i, ~, i, i,  i,  e | 1
        //  e, e, e, e, ~, e, e,  e,  e | 0
        //  ----------------------------+
        //  0  1  2  3  ~  x x+1 x+2 x+3

        // std::cout << "--------------------Update min curv boundary--------------------" << std::endl;
        // updateMatrixPtr->print();
        // std::cout << "----------------------------------------------------------------" << std::endl;

        auto &ump = *updateMatrixPtr;

        auto up_shape = ump.shape();
        if (up_shape.rows != (row + 4) || up_shape.cols != (column + 4))
        {
            THROW_INVALID_ARGUMENT("Extended matrix values not matching");
        }

        auto ej = row + 2;    // extened matrix j for y axis
        auto ei = column + 2; // extened matrix i for x axis

        // points at left&right side of extended boundary
        for (size_t j = 0; j < row; j++)
        {
            auto uj = j + 2;
            auto ui = ei;
            ump(uj, 1) = 2 * ump(uj, 2) - ump(uj, 3);            // left
            ump(uj, ui) = 2 * ump(uj, ui - 1) - ump(uj, ui - 2); // right
        }

        // points at down&top side of extended boundary
        for (size_t i = 0; i <= column; i++)
        {
            auto uj = ej;
            auto ui = i + 2;
            ump(1, ui) = 2 * ump(2, ui) - ump(3, ui);            // bottom
            ump(uj, ui) = 2 * ump(uj - 1, ui) - ump(uj - 2, ui); // top
        }

        // points at corners
        ump(1, 1) = ump(1, 3) + ump(3, 1) - ump(3, 3);                         // left-down
        ump(1, ei) = ump(3, ei) + ump(1, ei - 2) - ump(3, ei - 2);             // right-down
        ump(ej, 1) = ump(ej, 3) + ump(ej - 2, 1) - ump(ej - 2, 3);             // left-top
        ump(ej, ei) = ump(ej - 2, ei) + ump(ej, ei - 2) - ump(ej - 2, ei - 2); // right-top

        // exterior extension points
        double alpha1 = alpha * alpha;
        double alpha2 = -2 * (1 + alpha1);

        double beta = 1 / alpha;
        double beta1 = beta * beta;
        double beta2 = -2 * (1 + beta1);

        for (size_t j = 0; j < row; j++)
        {
            auto uj = j + 2;
            auto p_ij = [&](size_t i)
            {
                auto a1 = alpha1 * (ump(uj + 1, i + 1) - ump(uj + 1, i - 1) + ump(uj - 1, i + 1) - ump(uj - 1, i - 1));
                auto a2 = alpha2 * (ump(uj, i + 1) - ump(uj, i - 1));
                return a1 + a2;
            };

            // left
            ump(uj, 0) = ump(uj, 4) + p_ij(2);
            // right
            ump(uj, column + 3) = ump(uj, column - 1) - p_ij(column + 1);
        }

        for (size_t i = 0; i < column; i++)
        {
            auto ui = i + 2;
            auto q_ij = [&](size_t j)
            {
                auto b1 = beta1 * (ump(j + 1, ui + 1) - ump(j - 1, ui + 1) + ump(j + 1, ui - 1) - ump(j - 1, ui - 1));
                auto b2 = beta2 * (ump(j + 1, ui) - ump(j - 1, ui));
                return b1 + b2;
            };

            // bottom
            ump(0, ui) = ump(4, ui) + q_ij(2);
            // top
            ump(row + 3, ui) = ump(row - 1, ui) - q_ij(row + 1);
        }

        // std::cout << "--------------------End of min curv boundary--------------------" << std::endl;
        // updateMatrixPtr->print();
        // std::cout << "----------------------------------------------------------------" << std::endl;

        // std::cout << ump(79, 34) << std::endl;
        // ais::GridInterpolatorBlock block(ump, 79, 34);
        // block.print();
    }
} // namespace ais