/*!  @author Michael Brand
*    @Excercise 8
*    @date 17.07.2017
*
*   Algorithm to find Steiner tree on a graph where primes count as termainals.
*
*   I decided to leave all code in one file since its mainly consistent of two bigger algorithms
*   1. Dijkstra
*   2. Analyzing Dijkstra output in main-function
*   I don't think it gets to complicated reading it from top to bottom.
*/

#include <string>
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <stdio.h>
#include <cstdio>
#include <ctime>
#include <chrono>
#include <vector>
#include <climits>
#include <utility>                          // for std::pair

#include <boost/config.hpp>
#include <boost/utility.hpp>                // for boost::tie
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>

using namespace std;

typedef int vertex_;  /*!< defines a vertex as an int */
typedef int weight_;  /*!< defines a weight as an int */

/**Edge
 * pair of ints containing the weight and
 * the vertex pointed to. This is useful
 * for the adjacency list
 */
typedef pair<vertex_, weight_> Edge;

/**Graph
 * An adjacency list representing the graph.
 * graph[i] returns a list with Edge elements.
 *
 */
typedef vector< vector<Edge> > Graph;

/**struct pq_compare
 * compare structure for edges
 * allows me to compare two edges in a graph
 * this way I can sort edges for any vertex under consideration of their weight
 */
struct pq_compare {
    bool operator() (const Edge i, const Edge j) const{
    return (i.second <= j.second); }
};

/*! \fn bool isPrime(int number)
    \brief checks if number is a prime.
    \param number number to be checked.
*/
bool isPrime(int number){

    if(number < 2) return false;
    if(number == 2) return true;
    if(number % 2 == 0) return false;
    for(int i=3; (i*i)<=number; i+=2){
        if(number % i == 0 ) return false;
    }
    return true;

}

/*! \fn std::vector<int> getRequiredPrimes(int numV)
    \brief lists all primes <= numV in a vector.
    \param numV number to be checked.
    \return vector of primes
*/
std::vector<int> getRequiredPrimes(int numV){
  std::vector<int> primes;
  for(int i=3; i<=numV; i++){
    if(isPrime(i)) primes.push_back(i);
  }
  return primes;
}

/*! \fn std::vector<vertex_> dijkstra(const Graph &graph, vertex_ root, vector<int> &remPrimes)
*   \brief modified dijkstra algorithm used for steiner tree problem
*   \param &graph the graph we are working on.
*   \param root source vertex from which we want to calculate the distance
*   \&remPrimes vector of remaining prime no.
*   \return vector path that consists of
*     path[0] distance to terminal
*     path[1] terminal index
*     path[2] pre of path[1]
*     ...
*     path[n] pre of path[n-1]
*     path[n] is vertex index which has source index as pre
*/
std::vector<vertex_> dijkstra(const Graph &graph, vertex_ root, vector<int> &remPrimes, bool stNodeStructLoc[]) {

  std::vector<vertex_> path;
  std::vector<weight_> dist(graph.size(), INT_MAX);
  /* A set helps insertion and insert/erase/find operations in logarithmic time.
   * This set maintains Edge(distance,vertex number) sorted on basis of distance
   */
  set< Edge , pq_compare> pq;
  set< Edge , pq_compare > ::iterator it;

  vector<vertex_> pre(graph.size(), (-1)); /*!< vevtor of predecessors */
  int u,v,wt;
  int nPrime = 0; /*!< next prime - closest prime that is remaining in &remPrimes */

  dist[root] = 0;
  pq.insert(Edge(root,0));

  while(pq.size() != 0){
    bool found = false;
    it = pq.begin();
    u = it->first;
    pq.erase(it);
    if(isPrime(u+1)){
        for(int chk=0; chk<remPrimes.size(); chk++){
          if((remPrimes[chk])==(u+1)){
            nPrime=u;
            found = true;
            break;
          }
        }
    }
    if(found) break;

    for(vector<Edge>::const_iterator ni = graph[u].begin(); ni != graph[u].end(); ni++){
      v  = ni->first;
      wt = ni->second;
      if(stNodeStructLoc[v]){
        continue;
      }
      if(dist[v] > dist[u] + wt){
        pre[v] = u;
        if(dist[v] != INT_MAX){
          pq.erase(Edge(v,dist[v]));
        }
        dist[v] = dist[u] + wt;
        pq.insert(Edge(v,dist[v]));
      }

    }
  }
  /**
  * If source was internal node of steiner subgraph no new terminal is found
  * so we return vector of INT_MAX element
  */
  if(nPrime==0){
    path.push_back(INT_MAX);
    return path;
  }
  int distTerm = dist[nPrime];  /*!< distance to chosen terminal */
  /*! Create path
  *   save all nodes on path to a vector
  *   add distance of edges at the end at the top
  *   do not at the chosen root. does not need to be added to the subgraph anymore
  */
  path.push_back(nPrime);
  int lN = nPrime;
  while(lN != root){
    lN = pre[lN];
    path.push_back(lN);
  }
  path.insert(path.begin(),distTerm);
  return path;
}

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

  /**
  * start timers for cpu and wall time
  */
  clock_t cpu0 = clock();
  auto   wall0 = chrono::system_clock::now();
  if( argc != 2){
      fprintf(stderr, "Call the program as: %s 'filename.gph'", argv[0]);
      exit(EXIT_FAILURE);
  }
  ifstream    file(argv[1]);
  string      line;
  if(!file){
    fprintf(stderr, "Could not open file.");
    return -1;
  }
  /**
  * read number of vertices and edges
  */
  getline(file, line, ' ');
  const int numV = stoi(line);
  getline(file, line, '\n');
  const int numE = stoi(line);
  /**
  * create graph and steiner subgraph structures
  */
  Graph graph(numV);
  Graph steinerGraph(numV);
  bool stNodeStruct[numV] = { 0 };;
  int noStEdges = 0;  /*!< #edges in steiner tree */
  int stEdWght = 0;   /*!< obj value of steiner tree */
  int stNodes = 1;
  /**
  * read ín given gph file
  */
  while( getline(file, line) ){
    stringstream linestream(line);
    string       vertex1, vertex2, weight;
    try{
      getline(linestream, vertex1, ' ');
      getline(linestream, vertex2, ' ');
      getline(linestream, weight, '\n');
      /**
      * add both directions of edge to the graph since undirected
      * index switch applies: 1 --> 0, 1 --> 2, etc.
      */
      graph[stoi(vertex1)-1].push_back(Edge(stoi(vertex2)-1, stoi(weight)));
      //std::sort (graph[stoi(vertex1)-1].begin(), graph[stoi(vertex1)-1].end(), sortEdges);
      graph[stoi(vertex2)-1].push_back(Edge(stoi(vertex1)-1, stoi(weight)));
      // std::sort (graph[stoi(vertex2)-1].begin(), graph[stoi(vertex2)-1].end(), sortEdges);
    }catch (invalid_argument& ia){
      //when data is not a digit,
      //std::stoi throws an invalid argument exception
    } catch ( ... ){}
  }//while
  file.close();
  /**
  * get vector of primes that need to be connected in steiner tree
  */
  std::vector<int> remainingPrimes = getRequiredPrimes(numV);
  bool initState=true;
  /**
  * we initialize the steiner subgraph with terminal 2 (initState=true)
  * in the following (initState=false) we start dijkstra from every single node in the
  * subgraph and collect in every iteration the closest new terminal to the existing subgraph
  * -> saved in lovalSelection
  * we copy the result into an existing vector -> globalSelection
  * after all localSelections have been calculated we iterate through globalSelection
  * and coose the terminal that has the minimal distance (saved in globalSelection[i][0])
  */
  do{
    std::vector< std::vector<vertex_>> globalSelection;
    int distance = INT_MAX;
    int choice = -1;
    if(initState){
      std::vector<vertex_> localSelection = dijkstra(graph, 1, remainingPrimes, stNodeStruct);
      globalSelection.push_back(localSelection);
      for(int j=0; j<globalSelection.size(); j++){
        if(globalSelection[j][0]<distance){
         distance = globalSelection[j][0];
         choice = j;
        }
      }
      initState=false;
    }
    else{
      for(int i=0; i<steinerGraph.size(); i++){
        if(!(steinerGraph[i].empty())){
          std::vector<vertex_> localSelection = dijkstra(graph, i, remainingPrimes, stNodeStruct);
          globalSelection.push_back(localSelection);
        }
      }
      for(int j=0; j<globalSelection.size(); j++){
        if(globalSelection[j][0]<distance){
          distance = globalSelection[j][0];
          choice = j;
        }
      }
    }
    /**
    * add edges by pre-information:
    * globalSelection consists of path-vectors
    * which have certain structure
    * --> see dijkstra algo info
    */
    for(int pathNode = 1; pathNode < globalSelection[choice].size()-1; pathNode++){
      int from = globalSelection[choice][pathNode+1];
      int to = globalSelection[choice][pathNode];
      stNodes++;
      /*!
      * iterate over from-node-edges
      * search for relevant edge and add it to steinerGraph including its weight
      */
      for(int eIndx = 0; eIndx < graph[from].size(); eIndx++){
        if(graph[from][eIndx].first==to){
          steinerGraph[from].push_back(Edge(to, graph[from][eIndx].second));
          steinerGraph[to].push_back(Edge(from, graph[from][eIndx].second));
          noStEdges++;
          stEdWght += graph[from][eIndx].second;
          stNodeStruct[from] = true;
          stNodeStruct[to] = true;
        }
      }
    }
    /**
    * delete prime from remainingPrimes
    */
    for(int chk=0; chk<remainingPrimes.size(); chk++){
      if((remainingPrimes[chk]==(globalSelection[choice][1]+1))){
        remainingPrimes.erase(remainingPrimes.begin()+chk);
        break;
      }

    }
    globalSelection.clear();
  }while(remainingPrimes.size()>0);
  /**
  * collection stats
  */
  //Stats
  fprintf(stdout, "\n");
  fprintf(stdout, "Original graph:\n#Vert: \t %i\n#Edges \t %i\n", numV, numE);
  fprintf(stdout, "\n");
  fprintf(stdout, "Steiner graph:\n#Vert: \t %i\n#Edges \t %i\nObjVal \t %i\n", stNodes, noStEdges, stEdWght);

  fprintf(stdout, "\n");
  double cpuTime = (clock() - cpu0) / (double) CLOCKS_PER_SEC;
  chrono::duration<double> wallDur = (chrono::system_clock::now() - wall0);
  double wallTime = wallDur.count();
  fprintf(stdout, "Finished in %f seconds [CPU Clock] and %f seconds [Wall Clock] \n", cpuTime, wallTime);
  fprintf(stdout, "\n");
  return 0;

}


#include <iostream>
#include <vector>
#include <string>
#include "jefflib.h"
#include <boost/tokenizer.hpp>
#include <cmath>
#include <map>

using namespace std;
using namespace boost;

struct coord_t {
    int x;
    int y;
};

struct loc_t {
    int x;
    int y;
    int distSum = 0;
};

int main()
{
    vector<string> vect;
    if(GetStringInput(vect)){
        cout << "Got data!" << endl;
        cout << endl;
    }
    else {
        cout << "Failed to read input :( " << cout;
        return -1;
    }

    // Create the list of coords, making note of bounds
    vector<coord_t> coords;
    for (const string line : vect)
    {
        char_separator<char> sep(",");
        tokenizer< char_separator<char> > tokens(line, sep);
        // x y
        vector<string> s_tmp(tokens.begin(), tokens.end());
        coord_t tempCoord;
        tempCoord.x = stoi(s_tmp[0]);
        tempCoord.y = stoi(s_tmp[1]);
        coords.push_back(tempCoord);
    }

    int minX = -100;
    int minY = -100;
    int maxX = 1000;
    int maxY = 1000;

    // Let's build a (bounded) list of locations of interest
    vector<loc_t> locs;
    for(int x = minX; x <= maxX; x++){
        for(int y = minY; y <= maxY; y++){
            loc_t tempLoc;
            tempLoc.x = x;
            tempLoc.y = y;
            locs.push_back(tempLoc);
        }
    }
    cout << "Number of locations: " << locs.size() << endl;
    int regionSize = 0;
    // For each location, let's calculate the sum of distance to each coordinate
    for(auto  &location : locs){
        int distanceSum = 0;
        for(int i = 0; i < coords.size(); i++){
            distanceSum += abs(location.x - coords[i].x) + abs(location.y - coords[i].y);
            if(distanceSum > 10000) break;
        }
        if(distanceSum < 10000) regionSize++;
    }

    cout << "Region size: " << regionSize << endl;
}


//
//  geometry.cpp
//  Nesting Problem
//
//  Created by 爱学习的兔子 on 2020/4/14.
//  Copyright © 2020 Tongji SEM. All rights reserved.
//

#include "data_assistant.cpp"
#include <deque>
#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/foreach.hpp>
#include <boost/geometry/algorithms/for_each.hpp>

using namespace boost::geometry;
using namespace std;

#define BIAS 0.000001

// 基础定义
typedef model::d2::point_xy<double> Point;
typedef model::polygon<Point> Polygon;
typedef model::linestring<Point> LineString;

// 封装获得全部点的函数
template <typename Point>
class AllPoint{
private :
    VectorPoints *temp_all_points;
public :
    AllPoint(VectorPoints *all_points){
        temp_all_points=all_points;
    };
    inline void operator()(Point& pt)
    {
        vector<double> new_pt={get<0>(pt),get<1>(pt)};
        (*temp_all_points).push_back(new_pt);
    }
};

//主要包含注册多边形、转化多边形
class GeometryProcess{
public:
    /*
     数组转化为多边形
     */
    static void convertPoly(vector<vector<double>> poly, Polygon &Poly){
        // 空集的情况
        if(poly.size()==0){
            read_wkt("POLYGON(())", Poly);
            return;
        }
        // 首先全部转化为wkt格式
        string wkt_poly="POLYGON((";
        for (int i = 0; i < poly.size();i++){
            wkt_poly+=to_string(poly[i][0]) + " " + to_string(poly[i][1]) + ",";
            if(i==poly.size()-1){
                wkt_poly+=to_string(poly[0][0]) + " " + to_string(poly[0][1]) + "))";
            }
        };
        // 然后读取到Poly中
        read_wkt(wkt_poly, Poly);
    };
    /*
     通过for each point遍历
     */
    static void getAllPoints(list<Polygon> all_polys,VectorPoints &all_points){
        for(auto poly_item:all_polys){
            VectorPoints temp_points;
            getGemotryPoints(poly_item,temp_points);
            all_points.insert(all_points.end(),temp_points.begin(),temp_points.end());
        }
    };
    // 获得vector<list<VectorPoints>>的多边形（并非全部点）
    static void getListPolys(vector<list<Polygon>> list_polys,vector<VectorPoints> &all_polys){
        for(auto _list:list_polys){
            for(Polygon poly_item:_list){
                VectorPoints poly_points;
                getGemotryPoints(poly_item,poly_points);
                all_polys.push_back(poly_points);
            }
        }
    };
    // 获得某个集合对象的全部点
    static void getGemotryPoints(Polygon poly,VectorPoints &temp_points){
        for_each_point(poly, AllPoint<Point>(&temp_points));
    };
};

// 包含处理函数
class PackingAssistant{
public:
    /*
     获得Inner Fit Rectangle
     */
    static void getIFR(VectorPoints polygon,double container_width,double container_length,VectorPoints &IFR){
        // 初始参数，获得多边形特征
        VectorPoints border_points;
        getBorder(polygon,border_points);

        double poly_width_left=border_points[3][0]-border_points[0][0];
        double poly_width_right=border_points[2][0]-border_points[3][0];
        double poly_height=border_points[3][1]-border_points[1][1];

        // IFR具体计算（从左上角顺时针计算）
        IFR.push_back({poly_width_left,container_width});
        IFR.push_back({container_length-poly_width_right,container_width});
        IFR.push_back({container_length-poly_width_right,poly_height});
        IFR.push_back({poly_width_left,poly_height});
    };
    /*
     移动某个多边形
     */
    static void slidePoly(VectorPoints &polygon,double delta_x,double delta_y){
        for(int i=0;i<polygon.size();i++){
            polygon[i][0]=polygon[i][0]+delta_x;
            polygon[i][1]=polygon[i][1]+delta_y;
        }
    };
    /*
     移动多边形到某个位置（参考点）
     */
    static void slideToPosition(VectorPoints &polygon,vector<double> target_pt){
        vector<double> refer_pt;
        getReferPt(polygon,refer_pt);
        cout<<"多边形";
        PrintAssistant::print2DVector(polygon,true);
        cout<<"参考点:"<<refer_pt[0]<<","<<refer_pt[1]<<endl;
        cout<<"目标点:"<<target_pt[0]<<","<<target_pt[1]<<endl;
        double delta_x=target_pt[0]-refer_pt[0];
        double delta_y=target_pt[1]-refer_pt[1];
        for(int i=0;i<polygon.size();i++){
            polygon[i][0]=polygon[i][0]+delta_x;
            polygon[i][1]=polygon[i][1]+delta_y;
        }
    };
    /*
     获得多边形的所有的边界情况min_x min_y max_x max_y
     */
    static void getBound(VectorPoints polygon,vector<double> &bound){
        VectorPoints border_points;
        getBorder(polygon,border_points);
        bound={border_points[0][0],border_points[1][1],border_points[2][0],border_points[3][1]};
    };
    /*
     遍历获得一个多边形的最左侧点
     */
    static void getBottomLeft(VectorPoints polygon,vector<double> &bl_point){
        bl_point={999999999,999999999};
        for(auto point:polygon){
            if(point[0]<bl_point[0] || (point[0]==bl_point[0]&&point[1]<bl_point[1]) ){
                bl_point[0]=point[0];
                bl_point[1]=point[1];
            }
        };
    };
    /*
     仅仅获得最右侧点，同样为逆时针处理（用于判断是逗超出界限）
     */
    static void getRightPt(VectorPoints polygon,vector<double> &right_pt){
        right_pt={-9999999999,0};
        int poly_size=(int)polygon.size();
        for(int i=poly_size-1;i>=0;i--){
            if(polygon[i][0]>right_pt[0]){
                right_pt[0]=polygon[i][0];
                right_pt[1]=polygon[i][1];
            }
        }
    };
    /*
     仅仅获得参考点，是第一个Top位置，需要逆时针处理（NFP为逆时针）
     */
    static void getReferPt(VectorPoints polygon,vector<double> &refer_pt){
        refer_pt={0,-9999999999};
        int poly_size=(int)polygon.size();
        for(int i=poly_size-1;i>=0;i--){
            if(polygon[i][1]>refer_pt[1]){
                refer_pt[0]=polygon[i][0];
                refer_pt[1]=polygon[i][1];
            }
        }
    };
    /*
     仅仅获得底部位置（用于NFP计算），是第一个Bottom位置，需要逆时针处理
     */
    static void getBottomPt(VectorPoints polygon,vector<double> &bottom_pt){
        bottom_pt={0,9999999999};
        int poly_size=(int)polygon.size();
        for(int i=poly_size-1;i>=0;i--){
            if(polygon[i][1]<bottom_pt[1]){
                bottom_pt[0]=polygon[i][0];
                bottom_pt[1]=polygon[i][1];
            }
        }
    };

    /*
     获得多边形的边界四个点，border_points有left bottom right top四个点
     暂时不考虑参考点，参考点统一逆时针旋转第一个最上方的点
     */
    static void getBorder(VectorPoints polygon,VectorPoints &border_points){
        // 增加边界的几个点
        border_points.push_back(vector<double>{9999999999,0});
        border_points.push_back(vector<double>{0,999999999});
        border_points.push_back(vector<double>{-999999999,0});
        border_points.push_back(vector<double>{0,-999999999});
        // 遍历所有的点，分别判断是否超出界限
        int poly_size=(int)polygon.size();
        for(int i=poly_size-1;i>=0;i--){
            // 左侧点判断
            if(polygon[i][0]<border_points[0][0]){
                border_points[0][0]=polygon[i][0];
                border_points[0][1]=polygon[i][1];
            }
            // 下侧点判断
            if(polygon[i][1]<border_points[1][1]){
                border_points[1][0]=polygon[i][0];
                border_points[1][1]=polygon[i][1];
            }
            // 右侧点判断
            if(polygon[i][0]>border_points[2][0]){
                border_points[2][0]=polygon[i][0];
                border_points[2][1]=polygon[i][1];
            }
            // 上侧点判断
            if(polygon[i][1]>border_points[3][1]){
                border_points[3][0]=polygon[i][0];
                border_points[3][1]=polygon[i][1];
            }
        };
    };

    // 判断两个多边形是否重叠
    static bool judgeOverlap(VectorPoints poly1,VectorPoints poly2){
        Polygon Poly1,Poly2;
        GeometryProcess::convertPoly(poly1,Poly1);
        GeometryProcess::convertPoly(poly2,Poly2);
        return intersects(Poly1, Poly2);
    };

    // 获得两个多边形的重叠情况
    static double overlapArea(VectorPoints poly1,VectorPoints poly2){
        double overlap_area=0;

        Polygon Poly1,Poly2;
        GeometryProcess::convertPoly(poly1,Poly1);
        GeometryProcess::convertPoly(poly2,Poly2);

        // 获得重叠情况
        deque<Polygon> output;
        intersection(Poly1, Poly2, output);

        // 遍历计算重叠面积
        BOOST_FOREACH(Polygon const& p, output)
        {
            overlap_area+=area(p);
        }
        if(overlap_area>BIAS){
            return overlap_area;
        }else{
            return 0;
        }
    };
    // 获得List对象的全部重叠
    static double totalArea(list<Polygon> poly_list){
        double total_area=0;
        BOOST_FOREACH(Polygon const& p, poly_list)
        {
            total_area+=area(p);
        }
        return total_area;
    };
    // 获得当前排样的宽度
    static double arrangetLenth(vector<VectorPoints> all_polys){
        double length=0;
        for(VectorPoints poly:all_polys){
            vector<double> pt;
            getRightPt(poly,pt);
            if(pt[0]>length){
                length=pt[0];
            }
        }
        return length;
    }
};

// 获得NFP
class NFPAssistant{
protected:
    csv::Reader nfp_result;
    int poly_num;
    int orientation_num;
    vector<VectorPoints> NPFs; // 存储全部的NFP，按行存储
public:
    /*
     预加载全部的NFP，直接转化到NFP中
     */
    NFPAssistant(string _path,int poly_num,int orientation_num){
        nfp_result.read(_path);
        this->poly_num=poly_num;
        this->orientation_num=orientation_num;
        cout<<"加载全部NFP"<<endl;
        while(nfp_result.busy()) {
            if (nfp_result.ready()) {
                auto row = nfp_result.next_row();
                VectorPoints nfp;
                if(row["nfp"]!=""){
                    DataAssistant::load2DVector(row["nfp"],nfp);
                    NPFs.push_back(nfp);
                }
            }
        }
    };
    /*
     读取NFP的确定行数，i为固定形状，j为非固定形状,oi/oj为形状
     */
    void getNFP(int i,int j, int oi, int oj, VectorPoints poly_j ,VectorPoints &nfp){
        // 获得原始的NFP
        int row_num= i*192+j*16+oi*4+oj;
        nfp=NPFs[row_num];
        // 将NFP移到目标位置
        vector<double> bottom_pt;
        PackingAssistant::getBottomPt(poly_j,bottom_pt);
        PackingAssistant::slidePoly(nfp,bottom_pt[0],bottom_pt[1]);
    }
};

// 处理多个多边形的关系
class PolygonsOperator{
public:
    // 计算多边形的差集合
    static void polysDifference(list<Polygon> &feasible_region, Polygon sub_region){
        // 逐一遍历求解重叠
        list<Polygon> new_feasible_region;
        for(auto region_item:feasible_region){
            list<Polygon> output;
            difference(region_item, sub_region, output);
            DataAssistant::appendList(new_feasible_region,output);
        };
        // 将新的Output全部输入进去
        feasible_region.clear();
        copy(new_feasible_region.begin(), new_feasible_region.end(), back_inserter(feasible_region));
    }
    // 逐一遍历求差集
    static void polyListDifference(list<Polygon> &feasible_region, list<Polygon> sub_region){
        for(auto region_item:sub_region){
            polysDifference(feasible_region,region_item);
        }
    }
    // List和一个Poly的差集
    static void listToPolyIntersection(list<Polygon> region_list, Polygon region, list<Polygon> &inter_region){
        for(auto region_item:region_list){
            list<Polygon> output;
            intersection(region_item, region, output);
            DataAssistant::appendList(inter_region,output);
        }
    }
    // List和List之间的交集
    static void listToListIntersection(list<Polygon> region1, list<Polygon> region2, list<Polygon> &inter_region){
        for(auto region_item1:region1){
            for(auto region_item2:region2){
                list<Polygon> output;
                intersection(region_item1, region_item2, output);
                DataAssistant::appendList(inter_region,output);
            }
        }
    }
    // 判断某个List是否为空
    static bool judgeListEmpty(list<Polygon> poly_list){
        for(auto item:poly_list){
            if(area(item)>BIAS){
                return false;
            }
        }
        return true;
    }
    // 计算多边形的交集
    void polysUnion(){
        // 测试基础
        Polygon green, blue;

        vector<Polygon> output;
        union_(green, blue, output);

        int i = 0;
        cout << "green || blue:" << endl;
        BOOST_FOREACH(Polygon const& p, output)
        {
            cout << i++ << ": " << area(p) << endl;
        }
    }
    /*
     List数组的增长
     */
    static void appendPolyList(list<Polygon> &old_list,list<Polygon> &new_list){
        for(auto item:new_list){
            if(area(item)>BIAS){
                Polygon new_item;
                PolygonsOperator::convertToFeasible(new_item,item);
                old_list.push_back(new_item);
            }
        }
    }
    /*
     将不可行转化为可行
     */
    static void convertToFeasible(Polygon new_item,Polygon item){
        // 确认所有的点
        VectorPoints all_points;
        VectorPoints new_all_points;
        GeometryProcess::getGemotryPoints(item,all_points);
        // 判断点是否重叠了
        for(int i = 0; i < all_points.size(); i++){
            VectorPoints line1, line2;
            line1 = {all_points[i], all_points[i+1]};
            if(i == all_points.size() - 1){
                line2 = {all_points[0], all_points[1]};
            }else if (i == all_points.size() - 2){
                line2 = {all_points[i+1], all_points[0]};
            }else{
                line2 = {all_points[i+1], all_points[i+2]};
            }
            // 首先判断垂直情况
            double delta_x1, delta_y1, delta_x2, delta_y2;
            delta_x1 = line1[1][0] - line1[0][0];
            delta_y1 = line1[1][1] - line1[0][1];
            delta_x2 = line2[1][0] - line2[0][0];
            delta_y2 = line2[1][1] - line2[0][1];
            if(delta_x1 < BIAS && delta_x2 < BIAS){
                continue;
            }else if(delta_x1 < BIAS || delta_x2 < BIAS){
                new_all_points.push_back(all_points[i+1]);
            }else{
                // 判断非垂直情况
                double k1 = delta_y1/delta_x1;
                double k2 = delta_y2/delta_x2;
                if(abs(abs(k1) - abs(k2)) < BIAS){
                    continue;
                }
            }
        }
    }
};

C++ Nesting Problem/Nesting Problem/geometry.cpp


// -*- coding: utf-8 -*-
#pragma once

#include <cassert>
#include <cmath>
#include <tuple>

#include "cut_config.hpp"
#include "half_nonnegative.hpp"

/**
 * @brief Find a point in a convex set (defined through a cutting-plane oracle).
 *
 *     A function f(x) is *convex* if there always exist a g(x)
 *     such that f(z) >= f(x) + g(x)' * (z - x), forall z, x in dom f.
 *     Note that dom f does not need to be a convex set in our definition.
 *     The affine function g' (x - xc) + beta is called a cutting-plane,
 *     or a ``cut'' for short.
 *     This algorithm solves the following feasibility problem:
 *
 *             find x
 *             s.t. f(x) <= 0,
 *
 *     A *separation oracle* asserts that an evalution point x0 is feasible,
 *     or provide a cut that separates the feasible region and x0.
 *
 * @tparam Oracle
 * @tparam Space
 * @param[in,out] Omega perform assessment on x0
 * @param[in,out] S     search Space containing x*
 * @param[in] options   maximum iteration and error tolerance etc.
 * @return Information of Cutting-plane method
 */
template <typename Oracle, typename Space>
auto cutting_plane_feas(Oracle&& Omega, Space&& S, const Options& options = Options()) -> CInfo {
    auto feasible = false;
    auto status = CUTStatus::success;

    auto niter = 0U;
    while (++niter != options.max_it) {
        const auto cut = Omega(S.xc());  // query the oracle at S.xc()
        if (!cut) {                      // feasible sol'n obtained
            feasible = true;
            break;
        }
        const auto result = S.update(*cut);  // update S

        const auto& cutstatus = std::get<0>(result);
        const auto& tsq = std::get<1>(result);
        if (cutstatus != CUTStatus::success) {
            status = cutstatus;
            break;
        }
        if (tsq < options.tol) {  // no more
            status = CUTStatus::smallenough;
            break;
        }
    }
    return {feasible, niter, status};
}

/**
 * @brief Cutting-plane method for solving convex problem
 *
 * @tparam Oracle
 * @tparam Space
 * @tparam opt_type
 * @param[in,out] Omega perform assessment on x0
 * @param[in,out] S     search Space containing x*
 * @param[in,out] t     best-so-far optimal sol'n
 * @param[in] options   maximum iteration and error tolerance etc.
 * @return Information of Cutting-plane method
 */
template <typename Oracle, typename Space, typename opt_type>
auto cutting_plane_dc(Oracle&& Omega, Space&& S, opt_type&& t, const Options& options = Options()) {
    const auto t_orig = t;
    decltype(S.xc()) x_best;
    auto status = CUTStatus::success;

    auto niter = 0U;
    while (++niter != options.max_it) {
        const auto result1 = Omega(S.xc(), t);
        const auto& cut = std::get<0>(result1);
        const auto& shrunk = std::get<1>(result1);
        if (shrunk) {  // best t obtained
            x_best = S.xc();
        }
        const auto result2 = S.update(cut);

        const auto& cutstatus = std::get<0>(result2);
        const auto& tsq = std::get<1>(result2);
        if (cutstatus != CUTStatus::success)  // ???
        {
            status = cutstatus;
            break;
        }
        if (tsq < options.tol) {  // no more
            status = CUTStatus::smallenough;
            break;
        }
    }
    return std::make_tuple(std::move(x_best), CInfo{t != t_orig, niter, status});
}  // END

/**
    Cutting-plane method for solving convex discrete optimization problem
    input
             oracle        perform assessment on x0
             S(xc)         Search space containing x*
             t             best-so-far optimal sol'n
             max_it        maximum number of iterations
             tol           error tolerance
    output
             x             solution vector
             niter          number of iterations performed
**/
// #include <boost/numeric/ublas/symmetric.hpp>
// namespace bnu = boost::numeric::ublas;
// #include <xtensor-blas/xlinalg.hpp>
// #include <xtensor/xarray.hpp>

/**
 * @brief Cutting-plane method for solving convex discrete optimization problem
 *
 * @tparam Oracle
 * @tparam Space
 * @param[in,out] Omega perform assessment on x0
 * @param[in,out] S     search Space containing x*
 * @param[in,out] t     best-so-far optimal sol'n
 * @param[in] options   maximum iteration and error tolerance etc.
 * @return Information of Cutting-plane method
 */
template <typename Oracle, typename Space, typename opt_type>
auto cutting_plane_q(Oracle&& Omega, Space&& S, opt_type&& t, const Options& options = Options()) {
    const auto t_orig = t;
    decltype(S.xc()) x_best;
    auto status = CUTStatus::nosoln;  // note!!!
    auto retry = (status == CUTStatus::noeffect);

    auto niter = 0U;
    while (++niter != options.max_it) {
        // auto retry = (status == CUTStatus::noeffect);
        const auto result1 = Omega(S.xc(), t, retry);
        const auto& cut = std::get<0>(result1);
        const auto& shrunk = std::get<1>(result1);
        const auto& x0 = std::get<2>(result1);
        const auto& more_alt = std::get<3>(result1);
        if (shrunk) {  // best t obtained
            // t = t1;
            x_best = x0;  // x0
        }
        const auto result2 = S.update(cut);
        const auto& cutstatus = std::get<0>(result2);
        const auto& tsq = std::get<1>(result2);

        if (cutstatus == CUTStatus::noeffect) {
            if (!more_alt) {  // more alt?
                break;        // no more alternative cut
            }
            status = cutstatus;
            retry = true;
        }
        if (cutstatus == CUTStatus::nosoln) {
            status = cutstatus;
            break;
        }
        if (tsq < options.tol) {
            status = CUTStatus::smallenough;
            break;
        }
    }
    return std::make_tuple(std::move(x_best), CInfo{t != t_orig, niter, status});
}  // END

/**
 * @brief
 *
 * @tparam Oracle
 * @tparam Space
 * @param[in,out] Omega    perform assessment on x0
 * @param[in,out] I        interval containing x*
 * @param[in]     options  maximum iteration and error tolerance etc.
 * @return CInfo
 */
template <typename Oracle, typename Space>
auto bsearch(Oracle&& Omega, Space&& I, const Options& options = Options()) -> CInfo {
    // assume monotone
    // auto& [lower, upper] = I;
    auto& lower = I.first;
    auto& upper = I.second;
    assert(lower <= upper);
    const auto u_orig = upper;
    auto niter = 0U;
    auto status = CUTStatus::success;

    for (; niter != options.max_it; ++niter) {
        auto tau = algo::half_nonnegative(upper - lower);
        if (tau < options.tol) {
            status = CUTStatus::smallenough;
            break;
        }

        auto t = lower;  // l may be `int` or `Fraction`
        t += tau;
        if (Omega(t)) {  // feasible sol'n obtained
            upper = t;
        } else {
            lower = t;
        }
    }
    return {upper != u_orig, niter + 1, status};
}

/**
 * @brief
 *
 * @tparam Oracle
 * @tparam Space
 */
template <typename Oracle, typename Space>  //
class bsearch_adaptor {
  private:
    Oracle& _P;
    Space& _S;
    const Options _options;

  public:
    /**
     * @brief Construct a new bsearch adaptor object
     *
     * @param[in,out] P perform assessment on x0
     * @param[in,out] S search Space containing x*
     */
    bsearch_adaptor(Oracle& P, Space& S) : bsearch_adaptor{P, S, Options()} {}

    /**
     * @brief Construct a new bsearch adaptor object
     *
     * @param[in,out] P perform assessment on x0
     * @param[in,out] S search Space containing x*
     * @param[in] options maximum iteration and error tolerance etc.
     */
    bsearch_adaptor(Oracle& P, Space& S, const Options& options)
        : _P{P}, _S{S}, _options{options} {}

    /**
     * @brief get best x
     *
     * @return auto
     */
    auto x_best() const { return this->_S.xc(); }

    /**
     * @brief
     *
     * @param[in,out] t the best-so-far optimal value
     * @return bool
     */
    template <typename opt_type> auto operator()(const opt_type& t) -> bool {
        Space S = this->_S.copy();
        this->_P.update(t);
        const auto ell_info = cutting_plane_feas(this->_P, S, this->_options);
        if (ell_info.feasible) {
            this->_S.set_xc(S.xc());
        }
        return ell_info.feasible;
    }
};


/********************************************************************************
 * Copyright 2017 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_GEOMETRY_HYPERSPHERE_HPP_
#define RW_GEOMETRY_HYPERSPHERE_HPP_

/**
 * @file HyperSphere.hpp
 *
 * \copydoc rw::geometry::HyperSphere
 */

#if !defined(SWIG)
#include <rw/core/Ptr.hpp>

#include <Eigen/Core>
#include <vector>
#endif

namespace rw { namespace geometry {
    //! @addtogroup geometry
#if !defined(SWIG)
    //! @{
#endif
    /**
     * @brief A hyper-sphere of K dimensions.
     *
     * Functions are provided to create (almost) uniform distribution of points on a hyper-sphere as
     * shown in [1].
     *
     * The distribution of points is illustrated below for 2 and 3 dimensional hyper-spheres.
     * Notice that the tessellation is best when \f$ \delta\f$ is small.
     *
     * \image html geometry/hypersphere.gif "Distribution of points for K=2 and K=3."
     *
     * [1] Lovisolo, L., and E. A. B. Da Silva. "Uniform distribution of points on a hyper-sphere
     * with applications to vector bit-plane encoding." IEE Proceedings-Vision, Image and Signal
     * Processing 148.3 (2001): 187-193.
     */
    class HyperSphere
    {
      public:
        //! @brief Smart pointer type for HyperSphere.
        typedef rw::core::Ptr< const HyperSphere > Ptr;

        /**
         * @brief Construct a hyper-sphere of unit size.
         * @param dimensions [in] the number of dimensions.
         */
        HyperSphere (unsigned int dimensions);

        //! @brief Destructor.
        virtual ~HyperSphere ();

        /**
         * @brief Create a uniform distribution in Cartesian coordinates.
         *
         * This uses #uniformDistributionSpherical and maps the spherical coordinates to Cartesian
         * coordinates. The mapping is documented in [1], section 2.1.
         *
         * @param delta [in] the resolution.
         * @return unit vectors, \f$ [x_1 x_2 \dots x_K]^T\f$ , in Cartesian coordinates with
         * dimension K.
         * @note This function is only implemented for \f$ 2 \leq K \leq 6\f$ .
         */
        std::vector< Eigen::VectorXd > uniformDistributionCartesian (double delta) const;

        /**
         * @brief Create a uniform distribution in spherical coordinates.
         *
         * This implements the algorithm in [1], section 2.1, for dimensions \f$ 2 \leq K \leq 6\f$
         * .
         *
         * @param delta [in] the resolution.
         * @return list of vectors, \f$ [\theta_1 \theta_2 \dots \theta_{K-1}]^T\f$ , in spherical
         * coordinates with dimension K-1.
         * @note This function is only implemented for \f$ 2 \leq K \leq 6\f$ .
         */
        std::vector< Eigen::VectorXd > uniformDistributionSpherical (double delta) const;

        /**
         * @brief Get the number of dimensions of the hyper-sphere.
         * @return the number of dimensions, \f$ 2 \leq K \leq 6\f$ .
         */
        unsigned int getDimensions () const;

        /**
         * @brief Calculate the surface area of a hyper-sphere.
         *
         * Calculated for even dimensionality as \f$ \frac{K \pi^{K/2}}{(K/2)!}\f$
         *
         * Calculated for odd dimensionality as \f$ \frac{K 2^K \pi^{(K-1)/2}}{K!}\f$
         *
         * @return the surface area.
         */
        double area () const;

        /**
         * @brief The volume of a hyper-sphere.
         *
         * Calculated for even dimensionality as \f$ \frac{\pi^{K/2}}{(K/2)!}\f$
         *
         * Calculated for odd dimensionality as \f$ \frac{2 (2 \pi)^{(K-1)/2}}{K!!}\f$
         * where the double factorial for odd K means \f$ 1 \cdot 3 \cdot 5 \dots K\f$
         *
         * @return the volume.
         */
        double volume () const;

      private:
        const unsigned int _dimensions;
    };
#if !defined(SWIG)
//! @}
#endif
}}    // namespace rw::geometry

#endif /* RW_GEOMETRY_HYPERSPHERE_HPP_ */


//Author: Dr. Shantanu Shahane
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <float.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <algorithm>
#include "class.hpp"
#include "coefficient_computations.hpp"
#include <unistd.h>
#include <limits.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <Eigen/Eigenvalues>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/SparseGenMatProd.h>
#include <Spectra/GenEigsRealShiftSolver.h>
#include <Spectra/MatOp/SparseGenRealShiftSolve.h>
#include "nanoflann.hpp"
using namespace std;

CLOUD::CLOUD(POINTS &points, PARAMETERS &parameters)
{
    clock_t clock_t1 = clock();
    if (parameters.periodic_bc_index.size() == 0)
        calc_cloud_points_fast(points, parameters); //non-periodic case
    else
        calc_cloud_points_fast_periodic_bc(points, parameters);
    parameters.cloud_id_timer = ((double)(clock() - clock_t1)) / CLOCKS_PER_SEC;
    clock_t1 = clock();
    re_order_points_reverse_cuthill_mckee(points, parameters);
    parameters.rcm_timer = ((double)(clock() - clock_t1)) / CLOCKS_PER_SEC;
    clock_t1 = clock();
    calc_iv_original_nearest_vert(points, parameters);
    calc_charac_dx(points, parameters);
    parameters.cloud_misc_timer = ((double)(clock() - clock_t1)) / CLOCKS_PER_SEC;
    calc_grad_laplace_coeffs(points, parameters);
    EIGEN_set_grad_laplace_matrix(points, parameters);
    EIGEN_set_grad_laplace_matrix_separate(points, parameters);
    cout << "\n";
}

void CLOUD::calc_iv_original_nearest_vert(POINTS &points, PARAMETERS &parameters)
{
    for (int iv0 = 0; iv0 < points.nv_original; iv0++)
        points.iv_original_nearest_vert.push_back(-1);
    double x0, y0, z0 = 0.0, x1, y1, z1 = 0.0, dist_square, temp;
    int dim = parameters.dimension, iv_nearest, offset = 0;
    for (int iv0 = 0; iv0 < points.nv_original; iv0++)
    {
        if (points.corner_edge_vertices[iv0])
        { //these points are deleted; thus, nearest vertex has to be found
            x0 = points.xyz_original[dim * iv0], y0 = points.xyz_original[dim * iv0 + 1];
            if (dim == 3)
                z0 = points.xyz_original[dim * iv0 + 2];
            dist_square = INFINITY;
            for (int iv1 = 0; iv1 < points.nv; iv1++)
            {
                // if (points.boundary_flag[iv1])
                // { //[boundary points coupled to boundary]
                x1 = points.xyz[dim * iv1], y1 = points.xyz[dim * iv1 + 1];
                if (dim == 3)
                    z1 = points.xyz[dim * iv1 + 2];
                temp = (x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0);
                if (dist_square >= temp)
                {
                    dist_square = temp;
                    points.iv_original_nearest_vert[iv0] = iv1;
                }
                // }
            }
            offset++; //deleted vertices are offset
        }
        else
        {
            iv_nearest = rcm_points_order[iv0 - offset];
            points.iv_original_nearest_vert[iv0] = iv_nearest;
        }
    }
}

void CLOUD::EIGEN_set_grad_laplace_matrix_separate(POINTS &points, PARAMETERS &parameters)
{
    vector<Eigen::Triplet<double>> triplet;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
            for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
                triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], grad_x_coeff[i1]));
    points.grad_x_matrix_EIGEN_boundary.resize(points.nv, points.nv);
    points.grad_x_matrix_EIGEN_boundary.setFromTriplets(triplet.begin(), triplet.end());
    points.grad_x_matrix_EIGEN_boundary.makeCompressed();
    triplet.clear();

    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
            for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
                triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], grad_x_coeff[i1]));
    points.grad_x_matrix_EIGEN_internal.resize(points.nv, points.nv);
    points.grad_x_matrix_EIGEN_internal.setFromTriplets(triplet.begin(), triplet.end());
    points.grad_x_matrix_EIGEN_internal.makeCompressed();
    triplet.clear();

    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
            for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
                triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], grad_y_coeff[i1]));
    points.grad_y_matrix_EIGEN_boundary.resize(points.nv, points.nv);
    points.grad_y_matrix_EIGEN_boundary.setFromTriplets(triplet.begin(), triplet.end());
    points.grad_y_matrix_EIGEN_boundary.makeCompressed();
    triplet.clear();

    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
            for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
                triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], grad_y_coeff[i1]));
    points.grad_y_matrix_EIGEN_internal.resize(points.nv, points.nv);
    points.grad_y_matrix_EIGEN_internal.setFromTriplets(triplet.begin(), triplet.end());
    points.grad_y_matrix_EIGEN_internal.makeCompressed();
    triplet.clear();

    if (parameters.dimension == 3)
    {
        for (int iv = 0; iv < points.nv; iv++)
            if (points.boundary_flag[iv])
                for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
                    triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], grad_z_coeff[i1]));
        points.grad_z_matrix_EIGEN_boundary.resize(points.nv, points.nv);
        points.grad_z_matrix_EIGEN_boundary.setFromTriplets(triplet.begin(), triplet.end());
        points.grad_z_matrix_EIGEN_boundary.makeCompressed();
        triplet.clear();

        for (int iv = 0; iv < points.nv; iv++)
            if (!points.boundary_flag[iv])
                for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
                    triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], grad_z_coeff[i1]));
        points.grad_z_matrix_EIGEN_internal.resize(points.nv, points.nv);
        points.grad_z_matrix_EIGEN_internal.setFromTriplets(triplet.begin(), triplet.end());
        points.grad_z_matrix_EIGEN_internal.makeCompressed();
        triplet.clear();
    }

    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
            for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
                triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], laplacian_coeff[i1]));
    points.laplacian_matrix_EIGEN_boundary.resize(points.nv, points.nv);
    points.laplacian_matrix_EIGEN_boundary.setFromTriplets(triplet.begin(), triplet.end());
    points.laplacian_matrix_EIGEN_boundary.makeCompressed();
    triplet.clear();

    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
            for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
                triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], laplacian_coeff[i1]));
    points.laplacian_matrix_EIGEN_internal.resize(points.nv, points.nv);
    points.laplacian_matrix_EIGEN_internal.setFromTriplets(triplet.begin(), triplet.end());
    points.laplacian_matrix_EIGEN_internal.makeCompressed();
    triplet.clear();
}

void CLOUD::EIGEN_set_grad_laplace_matrix(POINTS &points, PARAMETERS &parameters)
{
    vector<Eigen::Triplet<double>> triplet;
    for (int iv = 0; iv < points.nv; iv++)
        for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
            triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], grad_x_coeff[i1]));
    points.grad_x_matrix_EIGEN.resize(points.nv, points.nv);
    points.grad_x_matrix_EIGEN.setFromTriplets(triplet.begin(), triplet.end());
    points.grad_x_matrix_EIGEN.makeCompressed();
    triplet.clear();

    for (int iv = 0; iv < points.nv; iv++)
        for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
            triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], grad_y_coeff[i1]));
    points.grad_y_matrix_EIGEN.resize(points.nv, points.nv);
    points.grad_y_matrix_EIGEN.setFromTriplets(triplet.begin(), triplet.end());
    points.grad_y_matrix_EIGEN.makeCompressed();
    triplet.clear();

    if (parameters.dimension == 3)
    {
        for (int iv = 0; iv < points.nv; iv++)
            for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
                triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], grad_z_coeff[i1]));
        points.grad_z_matrix_EIGEN.resize(points.nv, points.nv);
        points.grad_z_matrix_EIGEN.setFromTriplets(triplet.begin(), triplet.end());
        points.grad_z_matrix_EIGEN.makeCompressed();
        triplet.clear();
    }

    for (int iv = 0; iv < points.nv; iv++)
        for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
            triplet.push_back(Eigen::Triplet<double>(iv, nb_points_col[i1], laplacian_coeff[i1]));
    points.laplacian_matrix_EIGEN.resize(points.nv, points.nv);
    points.laplacian_matrix_EIGEN.setFromTriplets(triplet.begin(), triplet.end());
    points.laplacian_matrix_EIGEN.makeCompressed();
    triplet.clear();
}

void CLOUD::calc_grad_laplace_coeffs(POINTS &points, PARAMETERS &parameters)
{
    clock_t t1 = clock(), t2, t3, t4 = clock();
    vector<double> vert;
    vector<int> central_vert_list;
    Eigen::MatrixXd laplacian, grad_x, grad_y, grad_z;
    int dim = parameters.dimension, iv_nb, i1;
    vector<int> ind_p = parameters.periodic_bc_index, iv_sect;
    central_vert_list.push_back(0);
    double scale[3], time, cond_num, xyz_temp[3];
    t3 = clock();
    cout << endl;
    printf("    CLOUD::calc_grad_laplace_coeffs started prints status after every 5 seconds\n");
    for (int iv = 0; iv < points.nv; iv++)
    {
        t2 = clock();
        central_vert_list[0] = 0;                     //coefficient for first vertex needed
        if (parameters.periodic_bc_index.size() == 0) //non-periodic case
            for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
            {
                iv_nb = nb_points_col[i1];
                for (int i = 0; i < dim; i++)
                    vert.push_back(points.xyz[dim * iv_nb + i]);
            }
        else
        {
            iv_sect = points.periodic_bc_section[iv];
            for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
            {
                iv_nb = nb_points_col[i1];
                for (int id = 0; id < dim; id++)
                    xyz_temp[id] = points.xyz[dim * iv_nb + id];
                for (int ip = 0; ip < ind_p.size(); ip++)
                    if (points.periodic_bc_section[iv_nb][ip] == (-iv_sect[ip])) //shift opposite section (nothing happens for iv_sect=0)
                        xyz_temp[ind_p[ip]] = xyz_temp[ind_p[ip]] + (((double)(iv_sect[ip])) * points.xyz_length[ind_p[ip]]);
                for (int id = 0; id < dim; id++)
                    vert.push_back(xyz_temp[id]);
            }
        }
        shifting_scaling(vert, scale, dim);
        cond_num = calc_PHS_RBF_grad_laplace_single_vert(vert, parameters, laplacian, grad_x, grad_y, grad_z, scale, central_vert_list);
        cond_num_RBF.push_back(cond_num);
        vert.clear();

        for (int i1 = 0; i1 < laplacian.size(); i1++)
        { //(nb_points_row[iv + 1] - nb_points_row[iv]) = laplacian.size()
            grad_x_coeff.push_back(grad_x(0, i1));
            grad_y_coeff.push_back(grad_y(0, i1));
            if (dim == 3)
                grad_z_coeff.push_back(grad_z(0, i1));
            laplacian_coeff.push_back(laplacian(0, i1));
        }

        time = ((double)(clock() - t3)) / CLOCKS_PER_SEC;
        if (time > 5.0)
        {
            printf("    CLOUD::calc_grad_laplace_coeffs iv: %i, nv: %i: completed %.2f percent in %g seconds\n", iv, points.nv, 100.0 * iv / points.nv, ((double)(clock() - t1)) / CLOCKS_PER_SEC);
            t3 = clock();
        }
    }
    cout << endl;
    laplacian.resize(0, 0); //free memory
    grad_x.resize(0, 0);    //free memory
    grad_y.resize(0, 0);    //free memory
    grad_z.resize(0, 0);    //free memory

    cond_num_RBF_max = *max_element(cond_num_RBF.begin(), cond_num_RBF.end());
    cond_num_RBF_min = *min_element(cond_num_RBF.begin(), cond_num_RBF.end());
    cond_num_RBF_avg = accumulate(cond_num_RBF.begin(), cond_num_RBF.end(), 0.0) / cond_num_RBF.size();
    printf("CLOUD::calc_grad_laplace_coeffs RBF condition number max: %g, min: %g, avg: %g\n", cond_num_RBF_max, cond_num_RBF_min, cond_num_RBF_avg);
    parameters.grad_laplace_coeff_timer = ((double)(clock() - t4)) / CLOCKS_PER_SEC;
    printf("CLOUD::calc_grad_laplace_coeffs total grad_laplace_coeff time: %g seconds\n", parameters.grad_laplace_coeff_timer);
}

void CLOUD::calc_charac_dx(POINTS &points, PARAMETERS &parameters)
{
    parameters.avg_dx = 0.0;
    parameters.max_dx = 0.0;
    parameters.min_dx = 1E20;
    double local_min_dx, delx, dely, delz = 0.0, dist;
    int iv_nb, isd, dim = parameters.dimension;
    for (int iv = 0; iv < points.nv; iv++)
    {
        local_min_dx = 1E20;
        for (int i1 = nb_points_row[iv]; i1 < nb_points_row[iv + 1]; i1++)
        {
            iv_nb = nb_points_col[i1];
            if (iv != iv_nb)
            {
                delx = points.xyz[dim * iv] - points.xyz[dim * iv_nb];
                dely = points.xyz[dim * iv + 1] - points.xyz[dim * iv_nb + 1];
                if (dim == 3)
                    delz = points.xyz[dim * iv + 2] - points.xyz[dim * iv_nb + 2];
                dist = sqrt(delx * delx + dely * dely + delz * delz);
                if (local_min_dx > dist)
                    local_min_dx = dist;
            }
        }
        parameters.avg_dx += local_min_dx;
        if (parameters.min_dx > local_min_dx)
            parameters.min_dx = local_min_dx;
        if (parameters.max_dx < local_min_dx)
            parameters.max_dx = local_min_dx;
    }
    parameters.avg_dx = parameters.avg_dx / ((double)(points.nv));
    printf("CLOUD::calc_charac_dx Characteristic mesh dx max: %g, min: %g, avg: %g\n", parameters.max_dx, parameters.min_dx, parameters.avg_dx);
}

void CLOUD::re_order_points_reverse_cuthill_mckee(POINTS &points, PARAMETERS &parameters)
{
    vector<int> temp;
    vector<vector<int>> points_adjacency;
    int iv_1, iv_2;
    for (int iv = 0; iv < points.nv; iv++)
        points_adjacency.push_back(temp); //pushback dummy empty vector
    for (iv_1 = 0; iv_1 < points.nv; iv_1++)
    {
        for (int i1 = nb_points_row[iv_1]; i1 < nb_points_row[iv_1 + 1]; i1++)
        {
            iv_2 = nb_points_col[i1];
            points_adjacency[iv_1].push_back(iv_2);
        }
    }
    reverse_cuthill_mckee_ordering(points_adjacency, rcm_points_order);
    re_order_points(points, parameters);

    for (int iv = 0; iv < points.nv; iv++)
        points_adjacency[iv].clear();
    points_adjacency.clear();
}

void CLOUD::re_order_points(POINTS &points, PARAMETERS &parameters)
{
    vector<int> bc_tag_copy;
    vector<double> xyz_copy, normal_copy;
    vector<bool> boundary_flag_copy;
    vector<vector<int>> nb_points_copy, nb_points;

    int new_iv, dim = parameters.dimension;

    xyz_copy = points.xyz;
    for (int iv = 0; iv < points.nv; iv++)
    { //copy xyz co-ordinates
        new_iv = rcm_points_order[iv];
        for (int i = 0; i < dim; i++)
            xyz_copy[dim * new_iv + i] = points.xyz[dim * iv + i];
    }
    points.xyz = xyz_copy; //update xyz co-ordinates
    xyz_copy.clear();

    if (parameters.periodic_bc_index.size() > 0)
    {
        vector<vector<int>> periodic_bc_section_copy;
        periodic_bc_section_copy = points.periodic_bc_section;
        for (int iv = 0; iv < points.nv; iv++)
        { //copy periodic_bc_section
            new_iv = rcm_points_order[iv];
            periodic_bc_section_copy[new_iv] = points.periodic_bc_section[iv];
        }
        points.periodic_bc_section = periodic_bc_section_copy; //update periodic_bc_section
        for (int iv = 0; iv < points.nv; iv++)
            periodic_bc_section_copy[iv].clear();
        periodic_bc_section_copy.clear();

        vector<vector<bool>> periodic_bc_flag_copy;
        periodic_bc_flag_copy = points.periodic_bc_flag;
        for (int iv = 0; iv < points.nv; iv++)
        { //copy periodic_bc_flag
            new_iv = rcm_points_order[iv];
            periodic_bc_flag_copy[new_iv] = points.periodic_bc_flag[iv];
        }
        points.periodic_bc_flag = periodic_bc_flag_copy; //update periodic_bc_flag
        for (int iv = 0; iv < points.nv; iv++)
            periodic_bc_flag_copy[iv].clear();
        periodic_bc_flag_copy.clear();
    }

    normal_copy = points.normal;
    for (int iv = 0; iv < points.nv; iv++)
    { //copy normals
        new_iv = rcm_points_order[iv];
        for (int i = 0; i < dim; i++)
            normal_copy[dim * new_iv + i] = points.normal[dim * iv + i];
    }
    points.normal = normal_copy; //update normal

    bc_tag_copy = points.bc_tag;
    for (int iv = 0; iv < points.nv; iv++)
    { //copy bc_tag
        new_iv = rcm_points_order[iv];
        bc_tag_copy[new_iv] = points.bc_tag[iv];
    }
    points.bc_tag = bc_tag_copy; //update bc_tag
    bc_tag_copy.clear();

    boundary_flag_copy = points.boundary_flag;
    for (int iv = 0; iv < points.nv; iv++)
    { //copy boundary_flag
        new_iv = rcm_points_order[iv];
        boundary_flag_copy[new_iv] = points.boundary_flag[iv];
    }
    points.boundary_flag = boundary_flag_copy; //update boundary_flag
    boundary_flag_copy.clear();

    vector<int> temp;
    for (int iv_1 = 0; iv_1 < points.nv; iv_1++)
    {
        nb_points.push_back(temp);      //initialize with empty vector
        nb_points_copy.push_back(temp); //initialize with empty vector
        for (int i1 = nb_points_row[iv_1]; i1 < nb_points_row[iv_1 + 1]; i1++)
            nb_points[iv_1].push_back(rcm_points_order[nb_points_col[i1]]);
    }
    for (int iv = 0; iv < points.nv; iv++)
    {
        new_iv = rcm_points_order[iv];
        nb_points_copy[new_iv] = nb_points[iv];
    }
    nb_points_row.clear();
    nb_points_col.clear();
    nb_points_row.push_back(0);
    for (int iv0 = 0; iv0 < points.nv; iv0++)
    {
        nb_points_row.push_back(nb_points_row[iv0] + nb_points_copy[iv0].size());
        nb_points_col.insert(nb_points_col.end(), nb_points_copy[iv0].begin(), nb_points_copy[iv0].end());
    }

    for (int iv = 0; iv < points.nv; iv++)
        nb_points[iv].clear();
    nb_points.clear();
    for (int iv = 0; iv < points.nv; iv++)
        nb_points_copy[iv].clear();
    nb_points_copy.clear();
}

void CLOUD::calc_cloud_points_fast_periodic_bc_shifted(POINTS &points, PARAMETERS &parameters, vector<double> &xyz_shifted, vector<int> &periodic_bc_section_value)
{
    PointCloud<double> cloud_nf_for_interior, cloud_nf_for_boundary;
    int dim = parameters.dimension, iv_nb;
    vector<int> ind_p = parameters.periodic_bc_index;
    cloud_nf_for_interior.pts.resize(points.nv), cloud_nf_for_boundary.pts.resize(points.nv);
    for (int iv = 0; iv < points.nv; iv++)
    {
        cloud_nf_for_interior.pts[iv].x = xyz_shifted[dim * iv];
        cloud_nf_for_interior.pts[iv].y = xyz_shifted[dim * iv + 1];
        if (dim == 3)
            cloud_nf_for_interior.pts[iv].z = xyz_shifted[dim * iv + 2];
        else
            cloud_nf_for_interior.pts[iv].z = 0.0; //does not accept dimension as a parameter in KDTreeSingleIndexAdaptor and index
    }
    for (int iv = 0; iv < points.nv; iv++)
    {
        if (!points.boundary_flag[iv])
        { //internal points are coupled with boundary points
            cloud_nf_for_boundary.pts[iv].x = xyz_shifted[dim * iv];
            cloud_nf_for_boundary.pts[iv].y = xyz_shifted[dim * iv + 1];
            if (dim == 3)
                cloud_nf_for_boundary.pts[iv].z = xyz_shifted[dim * iv + 2];
            else
                cloud_nf_for_boundary.pts[iv].z = 0.0; //does not accept dimension as a parameter in KDTreeSingleIndexAdaptor and index
        }
        else
        { //all boundary co-ordinates set to infinity so that they are never coupled with any boundary point
            cloud_nf_for_boundary.pts[iv].x = numeric_limits<double>::infinity();
            cloud_nf_for_boundary.pts[iv].y = numeric_limits<double>::infinity();
            cloud_nf_for_boundary.pts[iv].z = numeric_limits<double>::infinity(); //does not accept dimension as a parameter in KDTreeSingleIndexAdaptor and index
        }
    }
    typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, PointCloud<double>>, PointCloud<double>, 3> nanoflann_kd_tree_for_interior;
    nanoflann_kd_tree_for_interior index_for_interior(3, cloud_nf_for_interior, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */));
    index_for_interior.buildIndex();

    typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, PointCloud<double>>, PointCloud<double>, 3> nanoflann_kd_tree_for_boundary;
    nanoflann_kd_tree_for_boundary index_for_boundary(3, cloud_nf_for_boundary, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */));
    index_for_boundary.buildIndex();

    vector<size_t> nb_vert(parameters.cloud_size);
    vector<double> nb_dist(parameters.cloud_size);
    double query_pt[3];
    for (int iv = 0; iv < points.nv; iv++)
        if (points.periodic_bc_section[iv] == periodic_bc_section_value)
        {
            query_pt[0] = xyz_shifted[dim * iv], query_pt[1] = xyz_shifted[dim * iv + 1];
            if (dim == 3)
                query_pt[2] = xyz_shifted[dim * iv + 2];
            else
                query_pt[2] = 0.0; //does not accept dimension as a parameter in KDTreeSingleIndexAdaptor and index
            if (points.boundary_flag[iv])
            {
                index_for_boundary.knnSearch(&query_pt[0], parameters.cloud_size, &nb_vert[0], &nb_dist[0]);
                nb_points_col[nb_points_row[iv]] = iv;
                for (int i1 = 0; i1 < nb_vert.size() - 1; i1++)
                { //first entry is "iv": hence "nb_vert.size() - 1"
                    iv_nb = nb_vert[i1];
                    if (points.boundary_flag[iv_nb])
                    {
                        cout << "\n\nERROR from CLOUD::calc_cloud_points_fast boundary iv: " << iv << " (boundary_flag[iv]: " << points.boundary_flag[iv] << ") tried to couple to a boundary vertex: " << iv_nb << " (boundary_flag[iv_nb]: " << points.boundary_flag[iv_nb] << ") \n\n";
                        throw bad_exception();
                    }
                    else
                        nb_points_col[nb_points_row[iv] + i1 + 1] = iv_nb; //first entry is "iv"
                }
            }
            else
            { //internal points
                index_for_interior.knnSearch(&query_pt[0], parameters.cloud_size, &nb_vert[0], &nb_dist[0]);
                for (int i1 = 0; i1 < nb_vert.size(); i1++)
                    nb_points_col[nb_points_row[iv] + i1] = nb_vert[i1];
            }
        }
    cloud_nf_for_interior.pts.resize(0), cloud_nf_for_boundary.pts.resize(0);
}

void CLOUD::calc_cloud_points_fast_periodic_bc(POINTS &points, PARAMETERS &parameters)
{ //Uses KD-Tree algorithm from Nanoflann (https://github.com/jlblancoc/nanoflann)
    nb_points_row.push_back(0);
    for (int iv = 0; iv < points.nv; iv++)
        nb_points_row.push_back(nb_points_row[iv] + parameters.cloud_size);
    for (int i1 = 0; i1 < points.nv * parameters.cloud_size; i1++)
        nb_points_col.push_back(-1);
    int dim = parameters.dimension;
    vector<int> ind_p = parameters.periodic_bc_index, empty_int;

    vector<vector<int>> section_list;
    for (int i1 = 0; i1 < ((int)(pow(3, ind_p.size()))); i1++)
        section_list.push_back(empty_int);
    if (ind_p.size() == 1) //section_list = [[-1], [0], [1]]
        section_list[0].push_back(-1), section_list[1].push_back(0), section_list[2].push_back(1);
    else if (ind_p.size() == 2) //section_list = [[-1,-1], [-1,0], [-1,1], [0,-1], [0,0], [0,1], [1,-1], [1,0], [1,1]]
        for (int i1 = 0; i1 < 3; i1++)
            for (int i2 = 0; i2 < 3; i2++)
                section_list[3 * i1 + i2].push_back(i1 - 1), section_list[3 * i1 + i2].push_back(i2 - 1);
    else if (ind_p.size() == 3 && dim == 3)
        for (int i1 = 0; i1 < 3; i1++)
            for (int i2 = 0; i2 < 3; i2++)
                for (int i3 = 0; i3 < 3; i3++)
                {
                    section_list[9 * i1 + 3 * i2 + i3].push_back(i1 - 1);
                    section_list[9 * i1 + 3 * i2 + i3].push_back(i2 - 1);
                    section_list[9 * i1 + 3 * i2 + i3].push_back(i3 - 1);
                }
    else
    {
        cout << "\n\nCLOUD::calc_cloud_points_fast_periodic_bc number of periodic axes ind_p.size(): " << ind_p.size() << " should not be greater than problem dimension: " << dim << "\n\n";
        throw bad_exception();
    }

    vector<double> xyz_shifted;
    int i_sec;
    for (int i1 = 0; i1 < section_list.size(); i1++)
    {
        xyz_shifted = points.xyz;
        for (int ip = 0; ip < ind_p.size(); ip++)
        {
            i_sec = section_list[i1][ip];
            for (int iv = 0; iv < points.nv; iv++)
                if (points.periodic_bc_section[iv][ip] == -i_sec) //shift opposite section (nothing happens for i_sec=0)
                    xyz_shifted[dim * iv + ind_p[ip]] = xyz_shifted[dim * iv + ind_p[ip]] + (((double)(i_sec)) * points.xyz_length[ind_p[ip]]);
            calc_cloud_points_fast_periodic_bc_shifted(points, parameters, xyz_shifted, section_list[i1]);
        }
    }
    xyz_shifted.clear();
}

void CLOUD::calc_cloud_points_fast(POINTS &points, PARAMETERS &parameters)
{ //Uses KD-Tree algorithm from Nanoflann (https://github.com/jlblancoc/nanoflann)
    PointCloud<double> cloud_nf_for_interior, cloud_nf_for_boundary;
    int dim = parameters.dimension;
    cloud_nf_for_interior.pts.resize(points.nv);
    cloud_nf_for_boundary.pts.resize(points.nv);
    for (int iv = 0; iv < points.nv; iv++)
    {
        cloud_nf_for_interior.pts[iv].x = points.xyz[dim * iv];
        cloud_nf_for_interior.pts[iv].y = points.xyz[dim * iv + 1];
        if (dim == 3)
            cloud_nf_for_interior.pts[iv].z = points.xyz[dim * iv + 2];
        else
            cloud_nf_for_interior.pts[iv].z = 0.0; //does not accept dimension as a parameter in KDTreeSingleIndexAdaptor and index
    }
    for (int iv = 0; iv < points.nv; iv++)
    {
        if (!points.boundary_flag[iv])
        { //internal points are coupled with boundary points
            cloud_nf_for_boundary.pts[iv].x = points.xyz[dim * iv];
            cloud_nf_for_boundary.pts[iv].y = points.xyz[dim * iv + 1];
            if (dim == 3)
                cloud_nf_for_boundary.pts[iv].z = points.xyz[dim * iv + 2];
            else
                cloud_nf_for_boundary.pts[iv].z = 0.0; //does not accept dimension as a parameter in KDTreeSingleIndexAdaptor and index
        }
        else
        { //all boundary co-ordinates set to infinity so that they are never coupled with any boundary point
            cloud_nf_for_boundary.pts[iv].x = numeric_limits<double>::infinity();
            cloud_nf_for_boundary.pts[iv].y = numeric_limits<double>::infinity();
            cloud_nf_for_boundary.pts[iv].z = numeric_limits<double>::infinity(); //does not accept dimension as a parameter in KDTreeSingleIndexAdaptor and index
        }
    }

    typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, PointCloud<double>>, PointCloud<double>, 3> nanoflann_kd_tree_for_interior;
    nanoflann_kd_tree_for_interior index_for_interior(3, cloud_nf_for_interior, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */));
    index_for_interior.buildIndex();

    typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, PointCloud<double>>, PointCloud<double>, 3> nanoflann_kd_tree_for_boundary;
    nanoflann_kd_tree_for_boundary index_for_boundary(3, cloud_nf_for_boundary, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */));
    index_for_boundary.buildIndex();

    vector<size_t> nb_vert(parameters.cloud_size);
    vector<double> nb_dist(parameters.cloud_size);
    double query_pt[3];
    int iv_nb;
    nb_points_row.push_back(0);
    for (int iv = 0; iv < points.nv; iv++)
    {
        query_pt[0] = points.xyz[dim * iv], query_pt[1] = points.xyz[dim * iv + 1];
        if (dim == 3)
            query_pt[2] = points.xyz[dim * iv + 2];
        else
            query_pt[2] = 0.0; //does not accept dimension as a parameter in KDTreeSingleIndexAdaptor and index
        if (points.boundary_flag[iv])
        {
            index_for_boundary.knnSearch(&query_pt[0], parameters.cloud_size, &nb_vert[0], &nb_dist[0]);
            nb_points_row.push_back(nb_points_row[iv] + parameters.cloud_size);
            nb_points_col.push_back(iv);
            for (int i1 = 0; i1 < nb_vert.size() - 1; i1++)
            { //first entry is "iv": hence "nb_vert.size() - 1"
                iv_nb = nb_vert[i1];
                if (points.boundary_flag[iv_nb])
                {
                    cout << "\n\nERROR from CLOUD::calc_cloud_points_fast boundary iv: " << iv << " (boundary_flag[iv]: " << points.boundary_flag[iv] << ") tried to couple to a boundary vertex: " << iv_nb << " (boundary_flag[iv_nb]: " << points.boundary_flag[iv_nb] << ") \n\n";
                    throw bad_exception();
                }
                else
                    nb_points_col.push_back(iv_nb);
            }
        }
        else
        { //internal points
            index_for_interior.knnSearch(&query_pt[0], parameters.cloud_size, &nb_vert[0], &nb_dist[0]);
            nb_points_row.push_back(nb_points_row[iv] + parameters.cloud_size);
            nb_points_col.insert(nb_points_col.end(), nb_vert.begin(), nb_vert.end());
        }
    }
}

void CLOUD::calc_cloud_points_slow(POINTS &points, PARAMETERS &parameters)
{ //calculate neighboring points for all vertices: Computations: Order(points.nv^2)
    vector<double> dist_square, k_min_dist_square;
    vector<int> k_min_points;
    for (int iv = 0; iv < points.nv; iv++)
        dist_square.push_back(0.0); //initialize
    double x0, y0, z0;
    int k = parameters.cloud_size, dim = parameters.dimension;
    nb_points_row.push_back(0);
    if (dim == 2)
    { //2D problem
        for (int iv0 = 0; iv0 < points.nv; iv0++)
        {
            x0 = points.xyz[dim * iv0];
            y0 = points.xyz[dim * iv0 + 1];
            for (int iv = 0; iv < points.nv; iv++)
            {
                if (!points.boundary_flag[iv0])
                { //all points added if iv0 is internal
                    dist_square[iv] = (x0 - points.xyz[dim * iv]) * (x0 - points.xyz[dim * iv]);
                    dist_square[iv] = dist_square[iv] + (y0 - points.xyz[dim * iv + 1]) * (y0 - points.xyz[dim * iv + 1]);
                }
                else
                { //iv0 is boundary: only couple with internal points
                    if (iv0 == iv || !points.boundary_flag[iv])
                    { //self-coupling OR only internal points
                        dist_square[iv] = (x0 - points.xyz[dim * iv]) * (x0 - points.xyz[dim * iv]);
                        dist_square[iv] = dist_square[iv] + (y0 - points.xyz[dim * iv + 1]) * (y0 - points.xyz[dim * iv + 1]);
                    }
                    else
                        dist_square[iv] = numeric_limits<double>::infinity(); //iv is boundary and not equal to iv0 (thus should not be coupled)
                }
            }
            k_smallest_elements(k_min_dist_square, k_min_points, dist_square, k);
            nb_points_row.push_back(nb_points_row[iv0] + k_min_points.size());
            nb_points_col.insert(nb_points_col.end(), k_min_points.begin(), k_min_points.end());
        }
    }
    else
    { //3D problem
        for (int iv0 = 0; iv0 < points.nv; iv0++)
        {
            x0 = points.xyz[dim * iv0];
            y0 = points.xyz[dim * iv0 + 1];
            z0 = points.xyz[dim * iv0 + 2];
            for (int iv = 0; iv < points.nv; iv++)
            {
                if (!points.boundary_flag[iv0])
                { //all points added if iv0 is internal
                    dist_square[iv] = (x0 - points.xyz[dim * iv]) * (x0 - points.xyz[dim * iv]);
                    dist_square[iv] = dist_square[iv] + (y0 - points.xyz[dim * iv + 1]) * (y0 - points.xyz[dim * iv + 1]);
                    dist_square[iv] = dist_square[iv] + (z0 - points.xyz[dim * iv + 2]) * (z0 - points.xyz[dim * iv + 2]);
                }
                else
                { //iv0 is boundary: only couple with internal points
                    if (iv0 == iv || !points.boundary_flag[iv])
                    { //self-coupling OR only internal points
                        dist_square[iv] = (x0 - points.xyz[dim * iv]) * (x0 - points.xyz[dim * iv]);
                        dist_square[iv] = dist_square[iv] + (y0 - points.xyz[dim * iv + 1]) * (y0 - points.xyz[dim * iv + 1]);
                        dist_square[iv] = dist_square[iv] + (z0 - points.xyz[dim * iv + 2]) * (z0 - points.xyz[dim * iv + 2]);
                    }
                    else
                        dist_square[iv] = numeric_limits<double>::infinity(); //iv is boundary and not equal to iv0 (thus should not be coupled)
                }
            }
            k_smallest_elements(k_min_dist_square, k_min_points, dist_square, k);
            nb_points_row.push_back(nb_points_row[iv0] + k_min_points.size());
            nb_points_col.insert(nb_points_col.end(), k_min_points.begin(), k_min_points.end());
        }
    }
    dist_square.clear();
    k_min_dist_square.clear();
    k_min_points.clear();
}


/**
 * \file TimeVaryingSecondOrderSVFFilter.cpp
 */

#include "TimeVaryingSecondOrderSVFFilter.h"

#include <cassert>

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

namespace ATK
{
  template<typename SVFCoefficients>
  struct TimeVaryingSecondOrderSVFFilter<SVFCoefficients>::SVFState
  {
    typename SVFCoefficients::DataType iceq1;
    typename SVFCoefficients::DataType iceq2;

    SVFState()
    :iceq1(0), iceq2(0)
    {
    }
  };

  template<typename SVFCoefficients>
  TimeVaryingSecondOrderSVFFilter<SVFCoefficients>::TimeVaryingSecondOrderSVFFilter(int nb_channels)
  :SVFCoefficients(nb_channels), state(new SVFState[nb_channels])
  {
  }

  template<typename SVFCoefficients>
  TimeVaryingSecondOrderSVFFilter<SVFCoefficients>::~TimeVaryingSecondOrderSVFFilter()
  {
  }

  template<typename SVFCoefficients>
  void TimeVaryingSecondOrderSVFFilter<SVFCoefficients>::full_setup()
  {
    state.reset(new SVFState[nb_input_ports]);
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFFilter<DataType>::process_impl(int64_t size) const
  {
    assert(nb_input_ports == nb_output_ports);

    for(int64_t i = 0; i < size; ++i)
    {
      update_coeffs(converted_inputs[0][i]);

      for(int j = 0; j < nb_input_ports; ++j)
      {
        const DataType* ATK_RESTRICT input = converted_inputs[j+1];
        DataType* ATK_RESTRICT output = outputs[j];

        DataType v3 = input[i] - state[j].iceq2;
        DataType v1 = a1 * state[j].iceq1 + a2 * v3;
        DataType v2 = state[j].iceq2 + a2 * state[j].iceq1 + a3 * v3;
        state[j].iceq1 = 2 * v1 - state[j].iceq1;
        state[j].iceq2 = 2 * v2 - state[j].iceq2;

        output[i] = m0 * input[i] + m1 * v1 + m2 * v2;
      }
    }
  }

  template<typename DataType>
  TimeVaryingSecondOrderSVFBaseCoefficients<DataType>::TimeVaryingSecondOrderSVFBaseCoefficients(int nb_channels)
  :TypedBaseFilter<DataType>(1 + nb_channels, nb_channels),Q(1)
  {
  }

  template<typename DataType_>
  void TimeVaryingSecondOrderSVFBaseCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    this->Q = Q;
    setup();
  }

  template<typename DataType>
  DataType TimeVaryingSecondOrderSVFBaseCoefficients<DataType>::get_Q() const
  {
    return Q;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFLowPassCoefficients<DataType_>::TimeVaryingSecondOrderSVFLowPassCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFLowPassCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1/Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 0;
    m2 = 1;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFBandPassCoefficients<DataType_>::TimeVaryingSecondOrderSVFBandPassCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFBandPassCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 1;
    m2 = 0;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFHighPassCoefficients<DataType_>::TimeVaryingSecondOrderSVFHighPassCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFHighPassCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = -1;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFNotchCoefficients<DataType_>::TimeVaryingSecondOrderSVFNotchCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFNotchCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 2;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFPeakCoefficients<DataType_>::TimeVaryingSecondOrderSVFPeakCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFPeakCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 0;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFBellCoefficients<DataType_>::TimeVaryingSecondOrderSVFBellCoefficients(int nb_channels)
  :Parent(nb_channels), gain(0)
  {

  }

  template<typename DataType_>
  void TimeVaryingSecondOrderSVFBellCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType TimeVaryingSecondOrderSVFBellCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFBellCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / (Q* gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain * gain - 1);
    m2 = 0;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFLowShelfCoefficients<DataType_>::TimeVaryingSecondOrderSVFLowShelfCoefficients(int nb_channels)
  :Parent(nb_channels), gain(0)
  {

  }

  template<typename DataType_>
  void TimeVaryingSecondOrderSVFLowShelfCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType TimeVaryingSecondOrderSVFLowShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFLowShelfCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain - 1);
    m2 = gain * gain - 1;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFHighShelfCoefficients<DataType_>::TimeVaryingSecondOrderSVFHighShelfCoefficients(int nb_channels)
  :Parent(nb_channels), gain(0)
  {
  }

  template<typename DataType_>
  void TimeVaryingSecondOrderSVFHighShelfCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType TimeVaryingSecondOrderSVFHighShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFHighShelfCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / (Q* gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = gain * gain;
    m1 = k * (1 - gain) * gain;
    m2 = 1 - gain * gain;
  }

  template class TimeVaryingSecondOrderSVFBaseCoefficients<float>;
  template class TimeVaryingSecondOrderSVFBaseCoefficients<double>;

  template class TimeVaryingSecondOrderSVFLowPassCoefficients<float>;
  template class TimeVaryingSecondOrderSVFLowPassCoefficients<double>;
  template class TimeVaryingSecondOrderSVFBandPassCoefficients<float>;
  template class TimeVaryingSecondOrderSVFBandPassCoefficients<double>;
  template class TimeVaryingSecondOrderSVFHighPassCoefficients<float>;
  template class TimeVaryingSecondOrderSVFHighPassCoefficients<double>;
  template class TimeVaryingSecondOrderSVFNotchCoefficients<float>;
  template class TimeVaryingSecondOrderSVFNotchCoefficients<double>;
  template class TimeVaryingSecondOrderSVFPeakCoefficients<float>;
  template class TimeVaryingSecondOrderSVFPeakCoefficients<double>;
  template class TimeVaryingSecondOrderSVFBellCoefficients<float>;
  template class TimeVaryingSecondOrderSVFBellCoefficients<double>;
  template class TimeVaryingSecondOrderSVFLowShelfCoefficients<float>;
  template class TimeVaryingSecondOrderSVFLowShelfCoefficients<double>;
  template class TimeVaryingSecondOrderSVFHighShelfCoefficients<float>;
  template class TimeVaryingSecondOrderSVFHighShelfCoefficients<double>;

  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFLowPassCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFLowPassCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFBandPassCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFBandPassCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFHighPassCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFHighPassCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFNotchCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFNotchCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFPeakCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFPeakCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFBellCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFBellCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFLowShelfCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFLowShelfCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFHighShelfCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFHighShelfCoefficients<double> >;
}


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

// Written by Thomas Witkowski

#ifndef AMDIS_ITL_MINRES_INCLUDE
#define AMDIS_ITL_MINRES_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>

namespace itl
{

  /// Minimal Residual method
  template <typename Matrix, typename Vector,
            typename LeftPreconditioner, typename RightPreconditioner,
            typename Iteration>
  int minres(const Matrix& A, Vector& x, const Vector& b,
             const LeftPreconditioner& L, const RightPreconditioner& /*R*/,
             Iteration& iter)
  {
    using std::abs;
    using math::reciprocal;
    typedef typename mtl::Collection<Vector>::value_type Scalar;

    if (size(b) == 0)
      throw mtl::logic_error("empty rhs vector");

    Scalar                zero= math::zero(b[0]), one= math::one(b[0]);
    Vector v0(size(x), zero), v1(b - A * x), v2(v1), z1(solve(L, v1)), z2(size(x), zero);
    Vector w0(size(x), zero), w1(size(x), zero), w2(size(x), zero);

    Scalar s0(zero), s1(zero), c0(one), c1(one), gamma0(one);
    Scalar gamma1(sqrt(dot(z1, v1))), gamma2(zero), eta(gamma1);
    Scalar sigma1(one), alpha0(zero), alpha1(zero), alpha2(zero), alpha3(zero);

    while (!iter.finished(abs(eta)))
    {
      z1 *= reciprocal(gamma1);
      v2 = A * z1;
      sigma1 = dot(v2, z1);
      v2 += -(sigma1 / gamma1) * v1 - (gamma1 / gamma0) * v0;

      z2 = solve(L, v2);

      gamma2 = sqrt(dot(z2, v2));
      alpha0 = c1 * sigma1 - c0 * s1 * gamma1;
      alpha1 = sqrt(alpha0 * alpha0 + gamma2 * gamma2);
      alpha2 = s1 * sigma1 + c0 * c1 * gamma1;
      alpha3 = s0 * gamma1;

      c0 = c1;
      c1 = alpha0 / alpha1;
      s0 = s1;
      s1 = gamma2 / alpha1;

      w2 = z1 - alpha3 * w0 - alpha2 * w1;
      w2 *=  reciprocal(alpha1);

      x += c1 * eta * w2;
      eta *= -s1;

      w0 = w1;
      w1 = w2;
      v0 = v1;
      v1 = v2;
      z1 = z2;

      gamma0 = gamma1;
      gamma1 = gamma2;

      ++iter;
    }

    return iter;
  }

} // namespace itl;

#endif // AMDIS_ITL_MINRES_INCLUDE


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

#ifndef SMOOTH__DERIVATIVES_HPP_
#define SMOOTH__DERIVATIVES_HPP_

#include <Eigen/Core>

#include "lie_group.hpp"

/**
 * @file
 * @brief Various useful derivatives.
 */

namespace smooth {

/**
 * @brief Derivative of matrix product.
 *
 * @param A matrix                                         [N x K]
 * @param dA derivative of A on horizontal Hessian form    [K x N*Nvar]
 * @param B matrix                                         [K x M]
 * @param dB derivative of B on horizontal Hessian form    [M x K*Nvar]
 * @return derivative of A * B on horizontal Hessian form
 */
template<typename At, typename dAt, typename Bt, typename dBt>
inline auto d_matrix_product(const At & A, const dAt & dA, const Bt & B, const dBt & dB)
{
  using Scalar = std::common_type_t<
    typename At::Scalar,
    typename dAt::Scalar,
    typename Bt::Scalar,
    typename dBt::Scalar>;

  static constexpr int N    = At::ColsAtCompileTime;
  static constexpr int M    = Bt::RowsAtCompileTime;
  static constexpr int Nvar = []() -> int {
    if constexpr (dAt::ColsAtCompileTime > 0 && N > 0) {
      return dAt::ColsAtCompileTime / N;
    } else if (dBt::ColsAtCompileTime > 0 && M > 0) {
      return dBt::ColsAtCompileTime / M;
    } else {
      return -1;
    }
  }();

  const auto n                  = A.cols();
  [[maybe_unused]] const auto k = A.rows();
  const auto m                  = B.rows();
  const auto nvar               = dA.cols() / (n);

  assert(k == B.cols());
  assert(nvar == dB.size() / (m * k));

  static constexpr int dAB_cols = (M > 0 && Nvar > 0) ? M * Nvar : -1;

  Eigen::Matrix<Scalar, N, dAB_cols> dAB = B.transpose() * dA;
  for (auto i = 0u; i < n; ++i) {
    for (auto j = 0u; j < m; ++j) {
      dAB.template middleCols<Nvar>(i * nvar, nvar) +=
        A(i, j) * dB.template middleCols<Nvar>(j * Nvar, nvar);
    }
  }
  return dAB;
}

/**
 * @brief Hessian of composed function \f$ (f \circ g)(x) \f$.
 *
 * @param Jf Jacobian of f at y = g(x)  [No x Ny   ]
 * @param Hf Hessian of f at y = g(x)   [Ny x No*Ny]
 * @param Jg Jacobian of g at x         [Ny x Nx   ]
 * @param Hg Hessian of g at x          [Nx x Ny*Nx]
 *
 * @return Hessian of size [No x No*Nx]
 */
template<typename JfT, typename HfT, typename JgT, typename HgT>
inline auto d2_fog(const JfT & Jf, const HfT & Hf, const JgT & Jg, const HgT & Hg)
{
  using Scalar = std::common_type_t<
    typename JfT::Scalar,
    typename HfT::Scalar,
    typename JgT::Scalar,
    typename HgT::Scalar>;

  static constexpr int No = JfT::RowsAtCompileTime;
  static constexpr int Ny = JfT::ColsAtCompileTime;
  static constexpr int Nx = JgT::ColsAtCompileTime;

  const auto no = Jf.rows();
  const auto ny = Jf.cols();

  [[maybe_unused]] const auto ni = Jg.rows();
  const auto nx                  = Jg.cols();

  // check some dimensions
  assert(ny == ni);
  assert(Hf.rows() == ny);
  assert(Hf.cols() == no * ny);
  assert(Hg.rows() == nx);
  assert(Hg.cols() == ni * nx);

  Eigen::Matrix<Scalar, Nx, (No == -1 || Nx == -1) ? -1 : No * Nx> ret(nx, no * nx);
  ret.setZero();

  for (auto i = 0u; i < no; ++i) {
    ret.template block<Nx, Nx>(0, i * nx, nx, nx) +=
      Jg.transpose() * Hf.template middleCols<Ny>(i * ny, ny) * Jg;
  }

  for (auto i = 0u; i < Jf.outerSize(); ++i) {
    for (Eigen::InnerIterator it(Jf, i); it; ++it) {
      ret.template block<Nx, Nx>(0, it.row() * nx) +=
        it.value() * Hg.template middleCols<Nx>(it.col() * nx, nx);
    }
  }

  return ret;
}

/**
 * @brief Jacobian of rminus.
 * @param e value of \f$ x \ominus_r y \f$
 * @return \f$ \mathrm{d}^{r} (x \ominus_r y)_{x} \f$
 */
template<LieGroup G>
TangentMap<G> dr_rminus(const Tangent<G> & e)
{
  return dr_expinv<G>(e);
}
/**
 * @brief Hessian of rminus.
 * @param e value of \f$ x \ominus_r y \f$
 * @return \f$ \mathrm{d}^{2r} (x \ominus_r y)_{xx} \f$
 */
template<LieGroup G>
Hessian<G> d2r_rminus(const Tangent<G> & e)
{
  const auto J = dr_expinv<G>(e);

  auto res = d2r_expinv<G>(e);
  for (auto j = 0u; j < Dof<G>; ++j) {
    res.template block<Dof<G>, Dof<G>>(0, j * e.size(), e.size(), e.size()).applyOnTheRight(J);
  }
  return res;
}

/**
 * @brief Jacobian of the squared norm of rminus.
 * @param e value of \f$ x \ominus_r y \f$
 * @return \f$ \mathrm{d}^r \left( \frac{1}{2} \| x \ominus_r y \|^2 \right)_x \f$
 */
template<LieGroup G>
Eigen::RowVector<Scalar<G>, Dof<G>> dr_rminus_squarednorm(const Tangent<G> & e)
{
  return e.transpose() * dr_expinv<G>(e);
}

/**
 * @brief Hessian of the squared norm of rminus.
 * @param e value of \f$ x \ominus_r y \f$
 * @return \f$ \mathrm{d}^{2r} \left( \frac{1}{2} \| x \ominus_r y \|^2 \right)_{xx} \f$
 */
template<LieGroup G>
Eigen::Matrix<Scalar<G>, Dof<G>, Dof<G>> d2r_rminus_squarednorm(const Tangent<G> & e)
{
  const TangentMap<G> J1 = dr_rminus<G>(e);   // N x N
  const Hessian<G> H1    = d2r_rminus<G>(e);  // N x (N*N)

  return d2_fog(e.transpose(), Eigen::Matrix<Scalar<G>, Dof<G>, Dof<G>>::Identity(), J1, H1);
}

}  // namespace smooth

#endif


#define R_NO_REMAP
#include <R.h>
#include <Rmath.h>
#include <Rinternals.h>
#include <R_ext/BLAS.h>
#include <R_ext/Lapack.h>

#include "omxDefines.h"
#include <Eigen/Core>
#include "omxBuffer.h"
#include "matrix.h"
#include "glue.h"

static const int ERROR_LEN = 80;

static double
_mahalanobis(char *err, int dim, double *loc, double *center, double *origCov)
{
	std::vector<double> cloc(dim);
	for (int dx=0; dx < dim; dx++) {
		cloc[dx] = loc[dx] - center[dx];
	}

	Matrix covMat(origCov, dim, dim);
	omxBuffer<double> icov(dim * dim);
	Matrix icovMat(icov.data(), dim, dim);
	int info = MatrixSolve(covMat, icovMat, true); // can optimize for symmetry TODO
	if (info) {
		snprintf(err, ERROR_LEN, "Sigma is singular and cannot be inverted");
		return nan("Rf_error");
	}

	std::vector<double> half(dim);
	char trans='n';
	double alpha=1;
	double beta=0;
	int inc=1;
	F77_CALL(dgemv)(&trans, &dim, &dim, &alpha, icov.data(), &dim, cloc.data(), &inc, &beta, half.data(), &inc);

	double got=0;
	for (int dx=0; dx < dim; dx++) got += half[dx] * cloc[dx];
	return got;
}

static double
mahalanobis(int dim, double *loc, double *center, double *origCov)
{
	char err[ERROR_LEN];
	err[0] = 0;
	double ret = _mahalanobis(err, dim, loc, center, origCov);
	if (err[0]) Rf_error("%s", err);
	return ret;
}

static double
_dmvnorm(char *err, int dim, double *loc, double *mean, double *origSigma)
{
	double dist = mahalanobis(dim, loc, mean, origSigma);

	std::vector<double> sigma(dim * dim);
	memcpy(sigma.data(), origSigma, sizeof(double) * dim * dim);

	char jobz = 'N';
	char range = 'A';
	char uplo = 'U';
	double vunused;
	int iunused;
	double abstol = 0;
	int m;
	Eigen::VectorXd w(dim);
	Eigen::VectorXd Z(dim);
	int ldz=1;
	Eigen::VectorXi isuppz(2*dim);
	int lwork = -1;
	double optlWork;
	int optliWork;
	int liwork = -1;
	int info;

	F77_CALL(dsyevr)(&jobz, &range, &uplo,
			 &dim, sigma.data(), &dim,
			 &vunused, &vunused,
			 &iunused, &iunused,
			 &abstol, &m, w.data(),
			 Z.data(), &ldz, isuppz.data(),
			 &optlWork, &lwork,
			 &optliWork, &liwork, &info);
	if (info != 0) {
		snprintf(err, ERROR_LEN, "dsyevr failed when requesting work space size");
		return nan("Rf_error");
	}

	lwork = optlWork;
	std::vector<double> work(lwork);
	liwork = optliWork;
	std::vector<int> iwork(liwork);

	F77_CALL(dsyevr)(&jobz, &range, &uplo, &dim, sigma.data(), &dim,
			 &vunused, &vunused, &iunused, &iunused, &abstol, &m, w.data(), Z.data(), &ldz, isuppz.data(),
			 work.data(), &lwork, iwork.data(), &liwork, &info);
	if (info < 0) {
		snprintf(err, ERROR_LEN, "Arg %d is invalid", -info);
		return nan("Rf_error");
	}
	if (info > 0) {
		snprintf(err, ERROR_LEN, "dsyevr: internal Rf_error");
		return nan("Rf_error");
	}
	if (m < dim) {
		snprintf(err, ERROR_LEN, "Sigma not of full rank");
		return nan("Rf_error");
	}

	for (int dx=0; dx < dim; dx++) dist += log(w[dx]);
	double got = -(dim * M_LN_SQRT_2PI*2 + dist)/2;
	return got;
}

double
dmvnorm(int dim, double *loc, double *mean, double *sigma)
{
	char err[ERROR_LEN];
	err[0] = 0;
	double ret = _dmvnorm(err, dim, loc, mean, sigma);
	if (err[0]) Rf_error("%s", err);
	return ret;
}

SEXP dmvnorm_wrapper(SEXP Rloc, SEXP Rmean, SEXP Rsigma)
{
	SEXP ret;
	ScopedProtect p1(ret, Rf_allocVector(REALSXP, 1));
	REAL(ret)[0] = dmvnorm(Rf_length(Rloc), REAL(Rloc), REAL(Rmean), REAL(Rsigma));
	return ret;
}


#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

// Point - Point

template <
    typename DerivedP0,
    typename DerivedP1,
    typename T = typename DerivedP0::Scalar>
inline MatrixMax<T, 3, 2> point_point_tangent_basis(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1)
{
    if (p0.size() == 2) {
        assert(p1.size() == 2);

        MatrixMax<T, 3, 2> basis(2, 1);

        auto p0_to_p1 = (p1 - p0).normalized();

        basis(0) = -p0_to_p1(1);
        basis(1) = p0_to_p1(0);

        return basis;
    } else {
        assert(p0.size() == 3 && p1.size() == 3);

        MatrixMax<T, 3, 2> basis(3, 2);

        auto p0_to_p1 = p1 - p0;

        Vector3<T> cross_x = cross(Vector3<T>::UnitX(), p0_to_p1);
        Vector3<T> cross_y = cross(Vector3<T>::UnitY(), p0_to_p1);

        if (cross_x.squaredNorm() > cross_y.squaredNorm()) {
            basis.col(0) = cross_x.normalized();
            basis.col(1) = cross(p0_to_p1, cross_x).normalized();
        } else {
            basis.col(0) = cross_y.normalized();
            basis.col(1) = cross(p0_to_p1, cross_y).normalized();
        }

        return basis;
    }
}

///////////////////////////////////////////////////////////////////////////////
// Point - Edge

template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename T = typename DerivedP::Scalar>
inline MatrixMax<T, 3, 2> point_edge_tangent_basis(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    if (p.size() == 2) {
        assert(e0.size() == 2 && e1.size() == 2);

        MatrixMax<T, 3, 2> basis(2, 1);

        basis.col(0) = (e1 - e0).normalized();

        return basis;
    } else {
        assert(p.size() == 3 && e0.size() == 3 && e1.size() == 3);

        MatrixMax<T, 3, 2> basis(3, 2);

        auto e = e1 - e0;
        basis.col(0) = e.normalized();
        basis.col(1) = cross(e, p - e0).normalized();

        return basis;
    }
}

///////////////////////////////////////////////////////////////////////////////
// Edge - Edge

/// Compute a basis for the space tangent to the edge-edge pair.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename T = typename DerivedEA0::Scalar>
inline Eigen::Matrix<T, 3, 2> edge_edge_tangent_basis(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3 && ea1.size() == 3);
    assert(eb0.size() == 3 && eb1.size() == 3);

    Eigen::Matrix<T, 3, 2> basis;

    auto ea = ea1 - ea0; // Edge A direction
    // The first basis vector is along edge A.
    basis.col(0) = ea.normalized();
    // The second basis vector is orthogonal to the first and the edge-edge
    // normal.
    auto normal = cross(ea, eb1 - eb0);
    // The normal will be zero if the edges are parallel (i.e. coplanar).
    assert(normal.norm() != 0);
    basis.col(1) = cross(normal, ea).normalized();

    return basis;
}

///////////////////////////////////////////////////////////////////////////////
// Point - Triangle

/// Compute a basis for the space tangent to the point-triangle pair.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename T = typename DerivedP::Scalar>
inline Eigen::Matrix<T, 3, 2> point_triangle_tangent_basis(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    assert(p.size() == 3 && t0.size() == 3 && t1.size() == 3 && t2.size() == 3);

    Eigen::Matrix<T, 3, 2> basis;

    auto e0 = t1 - t0;
    // The first basis vector is along first edge of the triangle.
    basis.col(0) = e0.normalized();
    // The second basis vector is orthogonal to the first and the triangle
    // normal.
    auto normal = cross(e0, t2 - t0);
    assert(normal.norm() != 0);
    basis.col(1) = cross(normal, e0).normalized();

    return basis;
}

} // namespace ipc