/*
 * CurrentFlowGroupCloseness.cpp
 *
 *      Author: gstoszek
 */

#include "CurrentFlowGroupCloseness.h"
#include "Centrality.h"
#include "../algebraic/CSRMatrix.h"
#include "../numerics/LAMG/Lamg.h"
#include "../auxiliary/Log.h"
#include <chrono>
#include <stdlib.h>
#include <cmath>
#include "EffectiveResistanceDistance.h"
#include "ERDLevel.h"
#include <armadillo>

namespace NetworKit {

   CurrentFlowGroupCloseness::CurrentFlowGroupCloseness(const Graph& G,const count k, const count CB) : Centrality(G, true),k(k),CB(CB){
        S.resize(k);
        CFGCC = 0.;
        n=G.upperNodeIdBound();
        vList.resize(n);
        TopMatch.resize(n);
        /*Laplacian*/
        L.set_size(n,n);
        L.zeros();

        ERD.M=L;
        Adj=L;

        for(count i=0;i<L.n_rows;i++){
          vList[i]=i;
          for(count j=0;j<L.n_rows;j++){
            if(G.hasEdge(i,j)){
              L(i,j)=-1.;
              L(i,i)+=1;
              TopMatch[i].push_back(j);
            }
          }
        }
    }

    void CurrentFlowGroupCloseness::run() {
      count ID;
      count nPeripheralMerges;
      count minDegree;
      auto start = std::chrono::high_resolution_clock::now();
      auto end = std::chrono::high_resolution_clock::now();
      std::chrono::duration<double> diff;
      std::vector<std::pair<node,node>> Matching;
      std::vector<std::pair<count,count>> Indices;

      nPeripheralMerges=0;
      if(CB>1){
          minDegree=1;
          std::vector<std::pair<count,count>> c_indices;
          std::vector<std::pair<node,node>> Matching;
          ID=0;
          c_indices.resize(0);

          ERDLevel Level(ID,vList,Matching,TopMatch,Adj);
          LevelList.push_back (Level);
          /*update*/
          minDegree=updateMinDegree(minDegree);

          while(minDegree==1){
            c_indices=peripheralCoarsingIndices();
            Matching=updateMatching(c_indices);
            coarseLaplacian(c_indices);
            ID++;
            Level.set(ID,vList,Matching,TopMatch,Adj);
            LevelList.push_back (Level);
            minDegree=updateMinDegree(minDegree);
            TopMatch=updateTopMatch(minDegree);
          }
          /*
          while(minDegree<CB){
            c_indices=coarsingIndices(minDegree, false);
            Matching=updateMatching(c_indices);
            coarseLaplacian(c_indices);
            ID++;
            Level.set(ID,vList,Matching,TopMatch,Adj);
            LevelList.push_back (Level);
            minDegree=updateMinDegree(minDegree);
            TopMatch=updateTopMatch(minDegree);
          }
          */
        }
        L=arma::pinv(L, 0.01);
        ERD.computeFromPinvL(L,vList);
        if(CB>1){
          ID=LevelList[LevelList.size()-1].get_ID();
          while(ID>1){
            Matching=LevelList[ID].get_Matching();
            for(count i=0;i<Matching.size();i++){
              uncoarse(Matching[i].second, Matching[i].first, ID);
            }
            ID--;
            vList=LevelList[ID].get_vList();
          }
          nPeripheralMerges=mergePeripheralNodes();
        }
        end = std::chrono::high_resolution_clock::now();
        diff = end-start;
        std::cout << "Computation of EffectiveResistanceDistanceMatrice finished in " << diff.count() << "(s)" << "\n\n";

        greedy(nPeripheralMerges);
      }
      /*******************************************************************************************************************************************************/
      std::vector<node> CurrentFlowGroupCloseness::groupMaxCurrentFlowCloseness(){
          return S;
      }
      /*******************************************************************************************************************************************************/
      double CurrentFlowGroupCloseness::getCFGCC() {
          return CFGCC;
      }
      /**************************************************************************************************************/
        void CurrentFlowGroupCloseness::cleanNetwork(){
          count n2;
          n2=0;
          for(count i=0;i<n;i++){
            if(L(i,i)==0){
              vList.erase(vList.begin()+i-n2);
              n2++;
            }
          }
          if(n2>0){
            n-=n2;
            arma::uvec indices(n);
            for(count i=0;i<n;i++){
                indices(i)=vList[i];
            }
            L=L.submat(indices, indices);
          }
        }
        /**************************************************************************************************************/
        void CurrentFlowGroupCloseness::greedy(count n_peripheral_merges){
          count k_max;
          node s;
          node s_next;
          node v;
          double S_CFGCC;
          double S_currentCFGCC;
          double scaling_factor;
          std::vector<bool> V;
          std::vector<double> d;

          k_max=S.size();
          scaling_factor=(double)(n);
          if(k_max>vList.size()){
            k_max=vList.size()-1;
            scaling_factor=(double)((G.upperNodeIdBound()))/(double)(k_max);
          }
          S_CFGCC=0.;
          V.resize(vList.size(),true);
          d.resize(G.upperNodeIdBound(),n*n);
          for(count i=0;i<k_max;i++){
            /*Maximal Gain Loop*/
            for (count j=0; j<vList.size()-n_peripheral_merges;j++) {
                //use vector of bools
                if (V[j]){
                    s=vList[j];
                    /*Sample Loop*/
                    S_currentCFGCC = 0.;
                    for (count l = 0; l < vList.size(); l++) {
                        v=vList[l];
                        if (ERD.M(v,s)< d[v])
                            S_currentCFGCC = S_currentCFGCC + ERD.M(v,s);
                        else {
                            S_currentCFGCC = S_currentCFGCC + d[v];
                        }
                    }
                    S_currentCFGCC =  scaling_factor / S_currentCFGCC;
                    if (S_currentCFGCC > S_CFGCC) {
                        S_CFGCC = S_currentCFGCC;
                        s_next = s;
                    }
                }
            }
            for(count j= 0; j<vList.size(); j++) {
              v=vList[j];
                if (ERD.M(v,s_next)< d[v]) {
                    d[v] = ERD.M(v,s_next);
                }
            }
            S[i]=s_next;
            V[s_next]=false;
        }
        CFGCC=S_CFGCC;
    }

    void CurrentFlowGroupCloseness::computeInitialERD(count CB){
      count ID;
      count minDegree;

      minDegree=1;
      std::vector<std::pair<count,count>> c_indices;
      std::vector<std::pair<node,node>> Matching;
      ID=0;
      c_indices.resize(0);

      ERDLevel Level(ID,vList,Matching,TopMatch,Adj);
      LevelList.push_back (Level);
      /*update*/
      minDegree=updateMinDegree(minDegree);

      while(minDegree==1){
        c_indices=peripheralCoarsingIndices();
        Matching=updateMatching(c_indices);
        coarseLaplacian(c_indices);
        ID++;
        Level.set(ID,vList,Matching,TopMatch,Adj);
        LevelList.push_back (Level);
        minDegree=updateMinDegree(minDegree);
        TopMatch=updateTopMatch(minDegree);
      }
      /*
      while(minDegree<CB){
        c_indices=coarsingIndices(minDegree, false);
        Matching=updateMatching(c_indices);
        coarseLaplacian(c_indices);
        ID++;
        Level.set(ID,vList,Matching,TopMatch,Adj);
        LevelList.push_back (Level);
        minDegree=updateMinDegree(minDegree);
        TopMatch=updateTopMatch(minDegree);
      }
      */
    }
    /***************************************************************************/
    std::vector<std::vector<node>> CurrentFlowGroupCloseness::updateTopMatch(count minDegree){
      bool search;
      count j;

      node v;
      node w;
      std::vector<std::vector<node>> update_List;
      update_List.resize(G.upperNodeIdBound());
      for(count i=0;i<vList.size();i++){
        v=vList[i];
        if(L(i,i)==minDegree){
          search=true;
          j=0;
          while( (j<vList.size())&&(search)){
            if((L(i,j)!=0) && (i!=j)){
              w=vList[j];
              update_List[v].push_back(w);
              if(update_List[v].size()==minDegree){
                search=false;
              }
            }
            j++;
          }
        }
      }
      return update_List;
    }
    /***************************************************************************/
    count CurrentFlowGroupCloseness::updateMinDegree(count minDegree){
      bool search;
      count min;
      count i;

      search=true;
      min=n;
      i=0;

      while((i<L.n_rows)&&(search)){
        if(L(i,i)<min){
          min=L(i,i);
          if(min==minDegree){
            search=false;
          }
        }
        i++;
      }
      if(min==0){
        std::cout<<"NETWORK_ERROR: min= "<< min << "\n\n";
      }
      for(count i=0;i<L.n_rows;i++){
        if(L(i,i)==0){
          std::cout<<"ERROR\n";
        }
      }
      return min;
    }
    /***************************************************************************/
    std::vector<std::pair<node,node>> CurrentFlowGroupCloseness::updateMatching(std::vector<std::pair<count,count>> indices){
      std::vector<std::pair<node,node>> Matching;
      Matching.resize(indices.size());
        for(count i=0;i<Matching.size();i++){
          Matching[i].first=vList[indices[i].first];
          Matching[i].second=vList[indices[i].second];
        }
      return Matching;
    }
    /***************************************************************************/
    std::vector<std::pair<count,count>> CurrentFlowGroupCloseness::peripheralCoarsingIndices(){
      count v;
      count s;

      std::vector<std::pair<count,count>> indices;
      std::vector<count> reverse;
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vList.size();i++){
        reverse[vList[i]]=i;
      }
      indices.resize(0);
      for(count i=0;i<L.n_rows;i++){
        if(L(i,i)==1){
          v=vList[i];
          s=reverse[TopMatch[v][0]];
          indices.push_back(std::make_pair(i,s));
        }
      }
      return indices;
    }
    /***************************************************************************/
    std::vector<std::pair<count,count>> CurrentFlowGroupCloseness::coarsingIndices(count cDegree, bool Random){
        bool s_found;

        count c_index;
        count s_index;
        count l;

        node c;
        /*free supernodes*/
        std::vector<bool> s_List;
        /*potential candidates*/
        std::vector<count> c_List;
        /*c_index-s_index mapping*/
        std::vector<count> reverse;
        reverse.resize(G.upperNodeIdBound());
        for(count i=0;i<vList.size();i++){
          reverse[vList[i]]=i;
        }
        std::vector<std::pair<count,count>> indices;

        c_List.resize(0);
        s_List.resize(G.upperNodeIdBound(),true);
        /*potential candidates*/
        for(count i=0;i<L.n_rows;i++){
          if(L(i,i)==cDegree){
            c_List.push_back(i);
          }
        }
        if(Random){
          std::random_shuffle (c_List.begin(), c_List.end());
        }
        for(count i=0;i<c_List.size();i++){
          c_index=c_List[i];
          c=vList[c_index];
          if(s_List[c_index]){
            s_found=false;
            l=0;
            while(!(s_found)&&(k<TopMatch[c].size())){
              s_index=reverse[TopMatch[c][l]];
              if(s_List[s_index]){
                s_List[s_index]=false;
                s_found=true;
                indices.push_back(std::make_pair(c_index,s_index));
              }
              else{
                l++;
              }
            }
          }
        }
        return indices;
      }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::uncoarse(node s,node v,count ID){
      node w;
      std::vector<node> v_TopMatch;
      arma::vec v_Adj;
      arma::vec s_Adj;

      v_TopMatch = LevelList[ID].get_vecofTopmatch(v);
      v_Adj = LevelList[ID-1].get_CalofAdj(v);
      s_Adj = LevelList[ID-1].get_CalofAdj(s);
      ERD.firstJoin(vList,s,v,1.);
      vList.push_back (v);
      LevelList[ID].set_i_j_ofAdj(s,v,s_Adj(v));

      for(count i=1;i<v_TopMatch.size();i++){
        w=v_TopMatch[i];
        if(v_Adj(w)!=0){
          if(s_Adj(w)!=0){
            ERD.edgeFire(vList,v,w,1.);
            LevelList[ID].set_i_j_ofAdj(v,w,v_Adj(w));
            LevelList[ID].set_i_j_ofAdj(v,w,s_Adj(w)-v_Adj(w));
          }
          else{
            ERD.edgeFire(vList,v,w,1.);
            LevelList[ID].set_i_j_ofAdj(v,w,v_Adj(w));
            ERD.nonBridgeDelete(vList,s,w,1.);
            LevelList[ID].set_i_j_ofAdj(v,w,0.);
          }
        }
      }
    }
    /***************************************************************************/
    count CurrentFlowGroupCloseness::mergePeripheralNodes(){
      node s;
      node v;
      std::vector<std::pair<node,node>> merge;
      std::vector<std::pair<node,node>> Matching;
      std::vector<count> merge_value;

      Matching=LevelList[1].get_Matching();
      merge_value.resize(G.upperNodeIdBound(),0);
      for(count i=0;i<Matching.size();i++){
        v=Matching[i].first;
        s=Matching[i].second;
        merge_value[s]++;
        if(merge_value[s]==1){
          merge.push_back (std::make_pair(v,s));
        }
      }
      for(count i=0;i<merge.size();i++){
        v=merge[i].first;
        s=merge[i].second;
        ERD.firstJoin(vList,s,v,merge_value[s]);
        LevelList[1].set_i_j_ofAdj(s,v,merge_value[s]);
        vList.push_back (v);
      }
      return merge.size();
    }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::coarseLaplacian(std::vector<std::pair<count,count>> Matchings){
      count a;
      count l;
      /*s=supernode - c = to be coarsed node - w = adjacent node to c*/
      node s;
      node c;
      node w;

      std::vector<count> reverse;
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vList.size();i++){
        reverse[vList[i]]=i;
      }
      for(count i=0;i<Matchings.size();i++){
        c=Matchings[i].first;
        s=Matchings[i].second;
        L(s,s)-=1;
        for(count j=1;j<TopMatch[c].size();j++){
          w=reverse[TopMatch[c][j]];
          if(L(s,w)==0){
            L(s,w)=-1.;
            L(w,s)=-1.;
            /*Diagonal*/
            L(s,s)+=1.;
          }
          Adj(vList[s],vList[w])+=Adj(vList[c],vList[w]);
          Adj(vList[w],vList[s])=Adj(vList[s],vList[w]);
        }
        Adj(vList[s],vList[c])=0;
        Adj(vList[c],vList[s])=0;
      }
      arma::uvec indices(vList.size()-Matchings.size());
      a=0;
      l=0;
      for(count i=0;i<vList.size();i++){
        if(Matchings[l].first==i){
          l++;
        }
        else{
          indices(a)=i;
          a++;
        }
      }
      for(count i=0;i<Matchings.size();i++){
        vList.erase(vList.begin()+Matchings[i].first-i);
      }
      L=L.submat(indices, indices);

    }
} /* namespace NetworKit*/


// Boost.Geometry

// Copyright (c) 2018-2019 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ARITHMETIC_LINE_FUNCTIONS_HPP
#define BOOST_GEOMETRY_ARITHMETIC_LINE_FUNCTIONS_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/config.hpp>
#include <boost/geometry/geometries/infinite_line.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace arithmetic
{

// Calculates intersection point of two infinite lines.
// Returns true if the lines intersect.
// Returns false if lines are parallel (or collinear, possibly opposite)
template <typename Point, typename Type>
inline bool intersection_point(model::infinite_line<Type> const& p,
    model::infinite_line<Type> const& q, Point& ip)
{
    Type const denominator = p.b * q.a - p.a * q.b;

    static Type const zero = 0;
    if (math::equals(denominator, zero))
    {
        // Lines are parallel
        return false;
    }

    // Calculate the intersection coordinates
    geometry::set<0>(ip, (p.c * q.b - p.b * q.c) / denominator);
    geometry::set<1>(ip, (p.a * q.c - p.c * q.a) / denominator);

    return true;
}

//! Return a distance-side-measure for a point to a line
//! Point is located left of the line if value is positive,
//! right of the line is value is negative, and on the line if the value
//! is exactly zero
template <typename Type, typename CoordinateType>
inline
typename select_most_precise<Type, CoordinateType>::type
side_value(model::infinite_line<Type> const& line,
    CoordinateType const& x, CoordinateType const& y)
{
    // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_an_equation
    // Distance from point to line in general form is given as:
    // (a * x + b * y + c) / sqrt(a * a + b * b);
    // In most use cases comparisons are enough, saving the sqrt
    // and often even the division.
    // Also, this gives positive values for points left to the line,
    // and negative values for points right to the line.
    return line.a * x + line.b * y + line.c;
}

template <typename Type, typename Point>
inline
typename select_most_precise
<
    Type,
    typename geometry::coordinate_type<Point>::type
>::type
side_value(model::infinite_line<Type> const& line, Point const& p)
{
    return side_value(line, geometry::get<0>(p), geometry::get<1>(p));
}

// Returns true for two lines which are supposed to be (close to) collinear
// (which is not checked) and have a similar direction
// (in practice up to 45 degrees, TO BE VERIFIED)
// true: -----------------> p -----------------> q
// false: -----------------> p <----------------- q
template <typename Type>
inline
bool similar_direction(const model::infinite_line<Type>& p,
                       const model::infinite_line<Type>& q)
{
    return p.a * q.a >= 0 && p.b * q.b >= 0;
}

template <typename Type>
inline bool is_degenerate(const model::infinite_line<Type>& line)
{
    static Type const zero = 0;
    return math::equals(line.a, zero) && math::equals(line.b, zero);
}

} // namespace arithmetic

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ARITHMETIC_LINE_FUNCTIONS_HPP


#include <Eigen/Core>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <fstream>
#include <iostream>
#include <map>
#include <sstream>
#include <stdexcept>
#include <vector>

class NameValuePair {
  public:
/**
 * @param fname File containing data.
 */
    NameValuePair(const std::string& fname) {
      std::string line;
// open file for reading
      std::ifstream file(fname.c_str());
      if (!file)
        throw std::runtime_error("NameValuePair::NameValuePair: Unable to open input file");
// loop over each line in file
      while (std::getline(file, line)) {
        unsigned commentLoc = line.find('#');
        std::string myLine = line.substr(0, commentLoc);
        if (myLine != "") {
// only work on non-blank line: horribly ugly, but works
          unsigned eqLoc = myLine.find('=');
          std::string lhs, lhsStr, valueStr;
          lhsStr = myLine.substr(0, eqLoc);
          valueStr = myLine.substr(eqLoc+1, std::string::npos);
          std::istringstream lhsToken(lhsStr);
          lhsToken >> lhs;

          double value;
          std::istringstream tokens(valueStr);
          tokens >> value;
// insert it into map
          nvmap[lhs] = value;
        }
      }
    }

/**
 * @param name Name to lookup
 * @return value associated with name
 */
    double getValue(const std::string& name) const {
      std::map<std::string, double>::const_iterator itr =
        nvmap.find(name);
      if (itr != nvmap.end())
        return itr->second;
      throw std::runtime_error("NameValuePair::getValue: not found");
    }

/**
 * @param name Name to lookup
 * @return true if it exists, false otherwise
 */
    bool hasValue(const std::string& name) const {
      std::map<std::string, double>::const_iterator itr =
        nvmap.find(name);
      if (itr != nvmap.end())
        return true;
      return false;
    }

  private:
    std::map<std::string, double> nvmap;
};

/** Store sim data */
struct QuadData {
    int NDIM; /* Number of dimensions */
    int Np, Nq; /* Number of basis, Number of quadrature points */
    void show() {
      std::cout << "Np: " << Np << " Nq: " << Nq << std::endl;
    }
};

int cost(const QuadData& qd)
{
  int np = qd.Np, nq = qd.Nq, NDIM = qd.NDIM;
  return np*nq + NDIM*(2*np*nq+nq);
}

void
vol(int nloop, const QuadData& qd)
{
  int np = qd.Np, nq = qd.Nq;
  int NDIM = qd.NDIM;

  Eigen::VectorXd f(np), alpha(np), result(np);
  Eigen::VectorXd fQuad(nq), alphaQuad(nq);
  Eigen::MatrixXd interpMatrix(nq, np);
  Eigen::MatrixXd bigMatrix(np, nq);

  f = Eigen::VectorXd::Random(np);
  alpha = Eigen::VectorXd::Random(np);
  interpMatrix = Eigen::MatrixXd::Random(nq, np);
  bigMatrix = Eigen::MatrixXd::Random(np, nq);

  for (unsigned n=0; n<nloop; ++n)
  {
// interpolate to quadrature nodes
    fQuad.noalias() = interpMatrix*f; // Np*Nq (do this only once)
    for (unsigned d=0; d<NDIM; ++d)
    {
      alphaQuad.noalias() = interpMatrix*alpha; // Np*Nq
      alphaQuad.cwiseProduct(fQuad); // Nq
// compute updated solution
      result.noalias() = bigMatrix*fQuad; // Np*Nq
    }
  }
}

double run(int nloop, const QuadData& qd)
{
  clock_t t1 = clock();
  vol(nloop, qd);
  clock_t t2 = clock();
  return (double) (t2-t1)/CLOCKS_PER_SEC;
}

void printInfo(const QuadData& qd, double tm)
{
  std::cout << qd.NDIM << " " << qd.Np << " " << qd.Nq << " " << tm << std::endl;
}

void
runSimulation(const NameValuePair& nvpair)
{
  QuadData forceVolQuad, streamVolQuad;
  forceVolQuad.Np = nvpair.getValue("NpVolForce");
  forceVolQuad.Nq = nvpair.getValue("NqVolForce");
  forceVolQuad.NDIM = nvpair.getValue("VDIM");

  streamVolQuad.Np = nvpair.getValue("NpVolStream");
  streamVolQuad.Nq = nvpair.getValue("NqVolStream");
  streamVolQuad.NDIM = nvpair.getValue("CDIM");

  int nloop = nvpair.getValue("nloop");

  std::cout << std::endl;
  std::cout << "# Nloop  " << nloop << std::endl;

  std::cout << "# NDIM | Basis | Quadrature | Time (Force terms) " << std::endl;
// force terms
  double tForce = run(nloop, forceVolQuad);
  printInfo(forceVolQuad, tForce);

  std::cout << "# NDIM | Basis | Quadrature | Time (Streaming terms) " << std::endl;
// stream terms
  double tStream = run(nloop, streamVolQuad);
  printInfo(streamVolQuad, tStream);

// stats
  std::cout << std::endl;
  std::cout << "# Total time | Time per DOF " << std::endl;
  double tm = (tForce+tStream)/nloop;
  double tmPerDof = tm/forceVolQuad.Np;
  std::cout << tm << " " << tmPerDof << std::endl;

// cost
  std::cout << std::endl;
  std::cout << "# Theoretical Cost (Arb. Units)" << std::endl;
  std::cout << "Force vol. terms " << cost(forceVolQuad) << std::endl;
  std::cout << "Streaming vol. terms " << cost(streamVolQuad) << std::endl;
  std::cout << "Net: " << cost(forceVolQuad) + cost(streamVolQuad)  << std::endl;

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

int
main (int argc, char **argv)
{
  if (argc != 2) {
    std::cout << "Usage::" << std::endl;
    std::cout << " dg-int <input-file>" << std::endl;
    std::cout << "  input-file: Name of input file" << std::endl;

    exit(1);
  }
  std::string inFile(argv[1]); // input file
  NameValuePair nvpair(inFile);
  runSimulation(nvpair);

  return 0;
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial:  Iterative solvers in ViennaCL (iterative.cpp and iterative.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

//
// include necessary system headers
//
#include <iostream>

//
// Necessary to obtain a suitable performance in ublas
#ifndef NDEBUG
 #define NDEBUG
#endif

//
// ublas includes
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

//
// ViennaCL includes
//
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/jacobi_precond.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"

using namespace boost::numeric;

int main()
{
  typedef float       ScalarType;

  //
  // Set up some ublas objects
  //
  ublas::vector<ScalarType> rhs;
  ublas::vector<ScalarType> rhs2;
  ublas::vector<ScalarType> ref_result;
  ublas::vector<ScalarType> result;
  ublas::compressed_matrix<ScalarType> ublas_matrix;

  //
  // Read system from file
  //
  if (!viennacl::io::read_matrix_market_file(ublas_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return 0;
  }
  //std::cout << "done reading matrix" << std::endl;

  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", rhs))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return 0;
  }
  //std::cout << "done reading rhs" << std::endl;

  if (!readVectorFromFile("../examples/testdata/result65025.txt", ref_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return 0;
  }
  //std::cout << "done reading result" << std::endl;

  //
  // Set up some ViennaCL objects
  //
  std::size_t vcl_size = rhs.size();
  viennacl::compressed_matrix<ScalarType> vcl_compressed_matrix;
  viennacl::coordinate_matrix<ScalarType> vcl_coordinate_matrix;
  viennacl::vector<ScalarType> vcl_rhs(vcl_size);
  viennacl::vector<ScalarType> vcl_result(vcl_size);
  viennacl::vector<ScalarType> vcl_ref_result(vcl_size);

  viennacl::copy(rhs.begin(), rhs.end(), vcl_rhs.begin());
  viennacl::copy(ref_result.begin(), ref_result.end(), vcl_ref_result.begin());

  //
  // Transfer ublas-matrix to GPU:
  //
  viennacl::copy(ublas_matrix, vcl_compressed_matrix);

  //
  // alternative way: via STL. Sparse matrix as std::vector< std::map< unsigned int, ScalarType> >
  //
  std::vector< std::map< unsigned int, ScalarType> > stl_matrix(rhs.size());
  for (ublas::compressed_matrix<ScalarType>::iterator1 iter1 = ublas_matrix.begin1();
       iter1 != ublas_matrix.end1();
       ++iter1)
  {
    for (ublas::compressed_matrix<ScalarType>::iterator2 iter2 = iter1.begin();
         iter2 != iter1.end();
         ++iter2)
         stl_matrix[iter2.index1()][static_cast<unsigned int>(iter2.index2())] = *iter2;
  }
  viennacl::copy(stl_matrix, vcl_coordinate_matrix);
  viennacl::copy(vcl_coordinate_matrix, stl_matrix);

  //
  // set up ILUT preconditioners for ublas and ViennaCL objects:
  //
  std::cout << "Setting up preconditioners for uBLAS-matrix..." << std::endl;
  viennacl::linalg::ilut_precond< ublas::compressed_matrix<ScalarType> >    ublas_ilut(ublas_matrix, viennacl::linalg::ilut_tag());
  viennacl::linalg::ilu0_precond< ublas::compressed_matrix<ScalarType> >    ublas_ilu0(ublas_matrix, viennacl::linalg::ilu0_tag());
  viennacl::linalg::block_ilu_precond< ublas::compressed_matrix<ScalarType>,
                                       viennacl::linalg::ilu0_tag>          ublas_block_ilu0(ublas_matrix, viennacl::linalg::ilu0_tag());

  std::cout << "Setting up preconditioners for ViennaCL-matrix..." << std::endl;
  viennacl::linalg::ilut_precond< viennacl::compressed_matrix<ScalarType> > vcl_ilut(vcl_compressed_matrix, viennacl::linalg::ilut_tag());
  viennacl::linalg::ilu0_precond< viennacl::compressed_matrix<ScalarType> > vcl_ilu0(vcl_compressed_matrix, viennacl::linalg::ilu0_tag());
  viennacl::linalg::block_ilu_precond< viennacl::compressed_matrix<ScalarType>,
                                       viennacl::linalg::ilu0_tag>          vcl_block_ilu0(vcl_compressed_matrix, viennacl::linalg::ilu0_tag());

  //
  // set up Jacobi preconditioners for ViennaCL and ublas objects:
  //
  viennacl::linalg::jacobi_precond< ublas::compressed_matrix<ScalarType> >    ublas_jacobi(ublas_matrix, viennacl::linalg::jacobi_tag());
  viennacl::linalg::jacobi_precond< viennacl::compressed_matrix<ScalarType> > vcl_jacobi(vcl_compressed_matrix, viennacl::linalg::jacobi_tag());

  //
  // Conjugate gradient solver:
  //
  std::cout << "----- CG Test -----" << std::endl;

  //
  // for ublas objects:
  //
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag());
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag(1e-6, 20), ublas_ilut);
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag(1e-6, 20), ublas_jacobi);

  //
  // for ViennaCL objects:
  //
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::cg_tag());
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::cg_tag(1e-6, 20), vcl_ilut);
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::cg_tag(1e-6, 20), vcl_jacobi);

  //
  // Stabilized BiConjugate gradient solver:
  //
  std::cout << "----- BiCGStab Test -----" << std::endl;

  //
  // for ublas objects:
  //
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag());          //without preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), ublas_ilut); //with preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), ublas_jacobi); //with preconditioner

  //
  // for ViennaCL objects:
  //
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::bicgstab_tag());   //without preconditioner
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), vcl_ilut); //with preconditioner
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), vcl_jacobi); //with preconditioner

  //
  // GMRES solver:
  //
  std::cout << "----- GMRES Test -----" << std::endl;

  //
  // for ublas objects:
  //
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag());   //without preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag(1e-6, 20), ublas_ilut);//with preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag(1e-6, 20), ublas_jacobi);//with preconditioner

  //
  // for ViennaCL objects:
  //
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::gmres_tag());   //without preconditioner
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::gmres_tag(1e-6, 20), vcl_ilut);//with preconditioner
  vcl_result = viennacl::linalg::solve(vcl_compressed_matrix, vcl_rhs, viennacl::linalg::gmres_tag(1e-6, 20), vcl_jacobi);//with preconditioner

  //
  //  That's it.
  //
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return 0;
}


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REM_PIO2_STRAIGHT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REM_PIO2_STRAIGHT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the Computes the remainder modulo \f$\pi/2\f$.

    @par Header <boost/simd/function/rem_pio2_straight.hpp>

    @par Notes

    - @c rem_pio2_straight computes the remainder modulo \f$\pi/2\f$ with "straight" algorithm,
    and returns an angle quadrant which is always 1.
    This is a very quick version only correct if the input
    is in \f$[\pi/4,\pi/2]\f$.

    - In fact it only substracts \f$\pi/2\f$ to the input
    so it can be viewed as a specially accurate minuspio_2 function outside
    the interval in which it can be used as a substitute to @ref rem_pio2.

    - The reduction of the argument modulo \f$\pi/2\f$ is generally
    the most difficult part of trigonometric evaluations.
    The accurate algorithm over the whole floating point range
    is over costly and implies the knowledge
    of a few hundred \f$\pi\f$ decimals
    some simpler algorithms as this one
    can be used, but the precision is only insured on specific intervals.

    @see rem_pio2, rem_pio2_medium,rem_2pi, rem_pio2_cephes,

    @par Example:

      @snippet rem_pio2_straight.cpp rem_pio2_straight

    @par Possible output:

      @snippet rem_pio2_straight.txt rem_pio2_straight

  **/
  std::pair<IEEEValue, IEEEValue> rem_pio2_straight(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/rem_pio2_straight.hpp>
#include <boost/simd/function/simd/rem_pio2_straight.hpp>

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2016-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP

#include <algorithm>

#include <boost/config.hpp>
#include <boost/concept_check.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_void.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/geographic/azimuth.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>

#include <boost/geometry/formulas/vincenty_direct.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

#include <boost/geometry/formulas/result_direct.hpp>
#include <boost/geometry/formulas/mean_radius.hpp>

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
#include <boost/geometry/io/dsv/write.hpp>
#endif

#ifndef BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS
#define BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS 100
#endif

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
#include <iostream>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Strategy functor for distance point to segment calculation on ellipsoid
       Algorithm uses direct and inverse geodesic problems as subroutines.
       The algorithm approximates the distance by an iterative Newton method.
\ingroup strategies
\details Class which calculates the distance of a point to a segment, for points
on the ellipsoid
\see C.F.F.Karney - Geodesics on an ellipsoid of revolution,
      https://arxiv.org/abs/1102.1215
\tparam FormulaPolicy underlying point-point distance strategy
\tparam Spheroid is the spheroidal model used
\tparam CalculationType \tparam_calculation
\tparam EnableClosestPoint computes the closest point on segment if true
*/
template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void,
    bool EnableClosestPoint = false
>
class geographic_cross_track
{
public :
    template <typename Point, typename PointOfSegment>
    struct return_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point,
                      PointOfSegment,
                      CalculationType
                  >::type
          >
    {};

    struct distance_strategy
    {
        typedef geographic<FormulaPolicy, Spheroid, CalculationType> type;
    };

    inline typename distance_strategy::type get_distance_strategy() const
    {
        typedef typename distance_strategy::type distance_type;
        return distance_type(m_spheroid);
    }

    explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
        : m_spheroid(spheroid)
    {}

    template <typename Point, typename PointOfSegment>
    inline typename return_type<Point, PointOfSegment>::type
    apply(Point const& p, PointOfSegment const& sp1, PointOfSegment const& sp2) const
    {
        typedef typename coordinate_system<Point>::type::units units_type;

        return (apply<units_type>(get<0>(sp1), get<1>(sp1),
                                  get<0>(sp2), get<1>(sp2),
                                  get<0>(p), get<1>(p),
                                  m_spheroid)).distance;
    }

private :

    template <typename CT>
    struct result_distance_point_segment
    {
        result_distance_point_segment()
            : distance(0)
            , closest_point_lon(0)
            , closest_point_lat(0)
        {}

        CT distance;
        CT closest_point_lon;
        CT closest_point_lat;
    };

    template <typename CT>
    result_distance_point_segment<CT>
    static inline non_iterative_case(CT lon, CT lat, CT distance)
    {
        result_distance_point_segment<CT> result;
        result.distance = distance;

        if (EnableClosestPoint)
        {
            result.closest_point_lon = lon;
            result.closest_point_lat = lat;
        }
        return result;
    }

    template <typename CT>
    result_distance_point_segment<CT>
    static inline non_iterative_case(CT lon1, CT lat1, //p1
                                     CT lon2, CT lat2, //p2
                                     Spheroid const& spheroid)
    {
        CT distance = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
                              ::apply(lon1, lat1, lon2, lat2, spheroid);

        return non_iterative_case(lon1, lat1, distance);
    }

    template <typename CT>
    CT static inline normalize(CT g4, CT& der)
    {
        CT const pi = math::pi<CT>();
        if (g4 < -1.25*pi)//close to -270
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "g4=" << g4 <<  ", close to -270" << std::endl;
#endif
            return g4 + 1.5 * pi;
        }
        else if (g4 > 1.25*pi)//close to 270
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "g4=" << g4 <<  ", close to 270" << std::endl;
#endif
            return - g4 + 1.5 * pi;
        }
        else if (g4 < 0 && g4 > -0.75*pi)//close to -90
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "g4=" << g4 <<  ", close to -90" << std::endl;
#endif
            der = -der;
            return -g4 - pi/2;
        }
        return g4 - pi/2;
    }

    template <typename Units, typename CT>
    result_distance_point_segment<CT>
    static inline apply(CT lon1, CT lat1, //p1
                        CT lon2, CT lat2, //p2
                        CT lon3, CT lat3, //query point p3
                        Spheroid const& spheroid)
    {
        typedef typename FormulaPolicy::template inverse<CT, true, false, false, true, true>
                inverse_distance_quantities_type;
        typedef typename FormulaPolicy::template inverse<CT, false, true, false, false, false>
                inverse_azimuth_type;
        typedef typename FormulaPolicy::template inverse<CT, false, true, true, false, false>
                inverse_azimuth_reverse_type;
        typedef typename FormulaPolicy::template direct<CT, true, false, false, false>
                direct_distance_type;

        CT const earth_radius = geometry::formula::mean_radius<CT>(spheroid);

        result_distance_point_segment<CT> result;

        // Constants
        //CT const f = geometry::formula::flattening<CT>(spheroid);
        CT const pi = math::pi<CT>();
        CT const half_pi = pi / CT(2);
        CT const c0 = CT(0);

        // Convert to radians
        lon1 = math::as_radian<Units>(lon1);
        lat1 = math::as_radian<Units>(lat1);
        lon2 = math::as_radian<Units>(lon2);
        lat2 = math::as_radian<Units>(lat2);
        lon3 = math::as_radian<Units>(lon3);
        lat3 = math::as_radian<Units>(lat3);

        if (lon1 > lon2)
        {
            std::swap(lon1, lon2);
            std::swap(lat1, lat2);
        }

        //segment on equator
        //Note: antipodal points on equator does not define segment on equator
        //but pass by the pole
        CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);

        typedef typename formula::elliptic_arc_length<CT> elliptic_arc_length;

        bool meridian_not_crossing_pole =
              elliptic_arc_length::meridian_not_crossing_pole(lat1, lat2, diff);

        bool meridian_crossing_pole =
              elliptic_arc_length::meridian_crossing_pole(diff);

        //bool meridian_crossing_pole = math::equals(math::abs(diff), pi);
        //bool meridian_not_crossing_pole = math::equals(math::abs(diff), c0);

        if (math::equals(lat1, c0) && math::equals(lat2, c0) && !meridian_crossing_pole)
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "Equatorial segment" << std::endl;
            std::cout << "segment=(" << lon1 * math::r2d<CT>();
            std::cout << "," << lat1 * math::r2d<CT>();
            std::cout << "),(" << lon2 * math::r2d<CT>();
            std::cout << "," << lat2 * math::r2d<CT>();
            std::cout << ")\np=(" << lon3 * math::r2d<CT>();
            std::cout << "," << lat3 * math::r2d<CT>() << ")\n";
#endif
            if (lon3 <= lon1)
            {
                return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
            }
            if (lon3 >= lon2)
            {
                return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
            }
            return non_iterative_case(lon3, lat1, lon3, lat3, spheroid);
        }

        if ( (meridian_not_crossing_pole || meridian_crossing_pole ) && lat1 > lat2)
        {
            std::swap(lat1,lat2);
        }

        if (meridian_crossing_pole)
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "Meridian segment" << std::endl;
#endif
            result_distance_point_segment<CT> d1 = apply<geometry::radian>(lon1, lat1, lon1, half_pi, lon3, lat3, spheroid);
            result_distance_point_segment<CT> d2 = apply<geometry::radian>(lon2, lat2, lon2, half_pi, lon3, lat3, spheroid);
            if (d1.distance < d2.distance)
            {
                return d1;
            }
            else
            {
                return d2;
            }
        }

        CT d1 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
                ::apply(lon1, lat1, lon3, lat3, spheroid);

        CT d3 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
                ::apply(lon1, lat1, lon2, lat2, spheroid);

        if (geometry::math::equals(d3, c0))
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "Degenerate segment" << std::endl;
            std::cout << "distance between points=" << d1 << std::endl;
#endif
            return non_iterative_case(lon1, lat2, d1);
        }

        CT d2 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
                ::apply(lon2, lat2, lon3, lat3, spheroid);

        // Compute a12 (GEO)
        geometry::formula::result_inverse<CT> res12 =
                inverse_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid);
        CT a12 = res12.azimuth;
        CT a13 = inverse_azimuth_type::apply(lon1, lat1, lon3, lat3, spheroid).azimuth;

        CT a312 = a13 - a12;

        if (geometry::math::equals(a312, c0))
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "point on segment" << std::endl;
#endif
            return non_iterative_case(lon3, lat3, c0);
        }

        CT projection1 = cos( a312 ) * d1 / d3;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
        std::cout << "segment=(" << lon1 * math::r2d<CT>();
        std::cout << "," << lat1 * math::r2d<CT>();
        std::cout << "),(" << lon2 * math::r2d<CT>();
        std::cout << "," << lat2 * math::r2d<CT>();
        std::cout << ")\np=(" << lon3 * math::r2d<CT>();
        std::cout << "," << lat3 * math::r2d<CT>();
        std::cout << ")\na1=" << a12 * math::r2d<CT>() << std::endl;
        std::cout << "a13=" << a13 * math::r2d<CT>() << std::endl;
        std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl;
        std::cout << "cos(a312)=" << cos(a312) << std::endl;
#endif
        if (projection1 < 0.0)
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "projection closer to p1" << std::endl;
#endif
            // projection of p3 on geodesic spanned by segment (p1,p2) fall
            // outside of segment on the side of p1
            return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
        }

        CT a21 = res12.reverse_azimuth - pi;
        CT a23 = inverse_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid).azimuth;

        CT a321 = a23 - a21;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
        std::cout << "a21=" << a21 * math::r2d<CT>() << std::endl;
        std::cout << "a23=" << a23 * math::r2d<CT>() << std::endl;
        std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl;
        std::cout << "cos(a321)=" << cos(a321) << std::endl;
#endif
        CT projection2 = cos( a321 ) * d2 / d3;

        if (projection2 < 0.0)
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "projection closer to p2" << std::endl;
#endif
            // projection of p3 on geodesic spanned by segment (p1,p2) fall
            // outside of segment on the side of p2
            return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
        }

        // Guess s14 (SPHERICAL)
        typedef geometry::model::point
                <
                    CT, 2,
                    geometry::cs::spherical_equatorial<geometry::radian>
                > point;

        point p1 = point(lon1, lat1);
        point p2 = point(lon2, lat2);
        point p3 = point(lon3, lat3);

        geometry::strategy::distance::cross_track<CT> cross_track(earth_radius);
        CT s34 = cross_track.apply(p3, p1, p2);

        geometry::strategy::distance::haversine<CT> str(earth_radius);
        CT s13 = str.apply(p1, p3);
        CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
        std::cout << "s34=" << s34 << std::endl;
        std::cout << "s13=" << s13 << std::endl;
        std::cout << "s14=" << s14 << std::endl;
        std::cout << "===============" << std::endl;
#endif

        // Update s14 (using Newton method)
        CT prev_distance = 0;
        geometry::formula::result_direct<CT> res14;
        geometry::formula::result_inverse<CT> res34;

        int counter = 0; // robustness
        CT g4;
        CT delta_g4;

        do{
            prev_distance = res34.distance;

            // Solve the direct problem to find p4 (GEO)
            res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);

            // Solve an inverse problem to find g4
            // g4 is the angle between segment (p1,p2) and segment (p3,p4) that meet on p4 (GEO)

            CT a4 = inverse_azimuth_type::apply(res14.lon2, res14.lat2,
                                                lon2, lat2, spheroid).azimuth;
            res34 = inverse_distance_quantities_type::apply(res14.lon2, res14.lat2,
                                                            lon3, lat3, spheroid);
            g4 = res34.azimuth - a4;

            CT M43 = res34.geodesic_scale; // cos(s14/earth_radius) is the spherical limit
            CT m34 = res34.reduced_length;
            CT der = (M43 / m34) * sin(g4);

            // normalize (g4 - pi/2)
            delta_g4 = normalize(g4, der);

            s14 = s14 - delta_g4 / der;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
                         "," << res14.lat2 * math::r2d<CT>() << std::endl;
            std::cout << "a34=" << res34.azimuth * math::r2d<CT>() << std::endl;
            std::cout << "a4=" << a4 * math::r2d<CT>() << std::endl;
            std::cout << "g4=" << g4 * math::r2d<CT>() << std::endl;
            std::cout << "delta_g4=" << delta_g4 * math::r2d<CT>()  << std::endl;
            std::cout << "der=" << der  << std::endl;
            std::cout << "M43=" << M43 << std::endl;
            std::cout << "spherical limit=" << cos(s14/earth_radius) << std::endl;
            std::cout << "m34=" << m34 << std::endl;
            std::cout << "new_s14=" << s14 << std::endl;
            std::cout << std::setprecision(16) << "dist     =" << res34.distance << std::endl;
            std::cout << "---------end of step " << counter << std::endl<< std::endl;
#endif
            result.distance = prev_distance;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            if (g4 == half_pi)
            {
                std::cout << "Stop msg: g4 == half_pi" << std::endl;
            }
            if (res34.distance >= prev_distance && prev_distance != 0)
            {
                std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
            }
            if (delta_g4 == 0)
            {
                std::cout << "Stop msg: delta_g4 == 0" << std::endl;
            }
            if (counter == 19)
            {
                std::cout << "Stop msg: counter" << std::endl;
            }
#endif

        } while (g4 != half_pi
                 && (prev_distance > res34.distance || prev_distance == 0)
                 && delta_g4 != 0
                 && ++counter < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS ) ;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
        std::cout << "distance=" << res34.distance << std::endl;

        point p4(res14.lon2, res14.lat2);
        CT s34_sph = str.apply(p4, p3);

        std::cout << "s34(sph) =" << s34_sph << std::endl;
        std::cout << "s34(geo) ="
                  << inverse_distance_quantities_type::apply(get<0>(p4), get<1>(p4), lon3, lat3, spheroid).distance
                  << ", p4=(" << get<0>(p4) * math::r2d<double>() << ","
                              << get<1>(p4) * math::r2d<double>() << ")"
                  << std::endl;

        CT s31 = inverse_distance_quantities_type::apply(lon3, lat3, lon1, lat1, spheroid).distance;
        CT s32 = inverse_distance_quantities_type::apply(lon3, lat3, lon2, lat2, spheroid).distance;

        CT a4 = inverse_azimuth_type::apply(get<0>(p4), get<1>(p4), lon2, lat2, spheroid).azimuth;
        geometry::formula::result_direct<CT> res4 = direct_distance_type::apply(get<0>(p4), get<1>(p4), .04, a4, spheroid);
        CT p4_plus = inverse_distance_quantities_type::apply(res4.lon2, res4.lat2, lon3, lat3, spheroid).distance;

        geometry::formula::result_direct<CT> res1 = direct_distance_type::apply(lon1, lat1, s14-.04, a12, spheroid);
        CT p4_minus = inverse_distance_quantities_type::apply(res1.lon2, res1.lat2, lon3, lat3, spheroid).distance;

        std::cout << "s31=" << s31 << "\ns32=" << s32
                  << "\np4_plus=" << p4_plus << ", p4=(" << res4.lon2 * math::r2d<double>() << "," << res4.lat2 * math::r2d<double>() << ")"
                  << "\np4_minus=" << p4_minus << ", p4=(" << res1.lon2 * math::r2d<double>() << "," << res1.lat2 * math::r2d<double>() << ")"
                  << std::endl;

        if (res34.distance <= p4_plus && res34.distance <= p4_minus)
        {
            std::cout << "Closest point computed" << std::endl;
        }
        else
        {
            std::cout << "There is a closer point nearby" << std::endl;
        }
#endif

        return result;
    }

    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

//tags
template <typename FormulaPolicy>
struct tag<geographic_cross_track<FormulaPolicy> >
{
    typedef strategy_tag_distance_point_segment type;
};

template
<
        typename FormulaPolicy,
        typename Spheroid
>
struct tag<geographic_cross_track<FormulaPolicy, Spheroid> >
{
    typedef strategy_tag_distance_point_segment type;
};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType
>
struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_segment type;
};

//return types
template <typename FormulaPolicy, typename P, typename PS>
struct return_type<geographic_cross_track<FormulaPolicy>, P, PS>
    : geographic_cross_track<FormulaPolicy>::template return_type<P, PS>
{};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename P,
        typename PS
>
struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS>
    : geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS>
{};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType,
        typename P,
        typename PS
>
struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
    : geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS>
{};

//comparable types
template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType
>
struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef geographic_cross_track
        <
            FormulaPolicy, Spheroid, CalculationType
        >  type;
};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType
>
struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef typename comparable_type
        <
            geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>
        >::type comparable_type;
public :
    static inline comparable_type
    apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& )
    {
        return comparable_type();
    }
};

template
<
    typename FormulaPolicy,
    typename P,
    typename PS
>
struct result_from_distance<geographic_cross_track<FormulaPolicy>, P, PS>
{
private :
    typedef typename geographic_cross_track
        <
            FormulaPolicy
        >::template return_type<P, PS>::type return_type;
public :
    template <typename T>
    static inline return_type
    apply(geographic_cross_track<FormulaPolicy> const& , T const& distance)
    {
        return distance;
    }
};

template
<
    typename FormulaPolicy,
    typename Spheroid,
    typename CalculationType,
    typename P,
    typename PS
>
struct result_from_distance<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
{
private :
    typedef typename geographic_cross_track
        <
            FormulaPolicy, Spheroid, CalculationType
        >::template return_type<P, PS>::type return_type;
public :
    template <typename T>
    static inline return_type
    apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& , T const& distance)
    {
        return distance;
    }
};

template <typename Point, typename PointOfSegment>
struct default_strategy
    <
        point_tag, segment_tag, Point, PointOfSegment,
        geographic_tag, geographic_tag
    >
{
    typedef geographic_cross_track<> type;
};

template <typename PointOfSegment, typename Point>
struct default_strategy
    <
        segment_tag, point_tag, PointOfSegment, Point,
        geographic_tag, geographic_tag
    >
{
    typedef typename default_strategy
        <
            point_tag, segment_tag, Point, PointOfSegment,
            geographic_tag, geographic_tag
        >::type type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry
#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP


#pragma once
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <vector>

#include "basis.hpp"
#include "triangulation_view.hpp"

namespace space {

struct OperatorOptions {
  // Options for all operators.
  bool dirichlet_boundary = true;

  // Whether or not to build the matrix for a forward operator.
  bool build_mat = false;

  // Options for stiff plus scaled mass operator.
  size_t time_level = 0;
  double alpha = 1;

  // Options for multigrid preconditioner.
  size_t mg_cycles = 3;

  OperatorOptions() = default;

  friend std::ostream &operator<<(std::ostream &os,
                                  const OperatorOptions &opts) {
    os << "OperatorOptions:" << std::endl;
    os << "\tdirichlet_boundary: "
       << (opts.dirichlet_boundary ? "true" : "false") << std::endl;
    os << "\tbuild_mat: " << (opts.build_mat ? "true" : "false") << std::endl;
    os << "\ttime_level: " << opts.time_level << std::endl;
    os << "\talpha: " << opts.alpha << std::endl;
    os << "\tcycles: " << opts.mg_cycles << std::endl;
    return os;
  }
};

class Operator {
 public:
  Operator(const TriangulationView &triang,
           OperatorOptions opts = OperatorOptions())
      : triang_(triang), opts_(std::move(opts)) {}

  virtual ~Operator() {}

  // Apply the operator in the hierarchical basis.
  virtual void Apply(Eigen::VectorXd &vec_in) const = 0;

  // Does the given vertex correspond to a dof?
  inline bool IsDof(uint vertex) const {
    return !triang_.OnBoundary(vertex) || !opts_.dirichlet_boundary;
  }

  // Verify that the given vector satisfy the boundary conditions.
  bool FeasibleVector(const Eigen::VectorXd &vec) const;
  inline bool DirichletBoundary() const { return opts_.dirichlet_boundary; }

  // Overloads required to for Eigen.
  Eigen::VectorXd operator*(const Eigen::VectorXd &vec_in) const {
    Eigen::VectorXd result = vec_in;
    Apply(result);
    return result;
  }
  size_t rows() const { return triang_.V; }
  size_t cols() const { return triang_.V; }

  // Debug function for turning this operator into a mtrix.
  Eigen::MatrixXd ToMatrix() const;

 protected:
  const TriangulationView &triang_;
  OperatorOptions opts_;
};

template <class ForwardOp>
class ForwardOperator : public Operator {
 public:
  ForwardOperator(const TriangulationView &triang,
                  OperatorOptions opts = OperatorOptions());

  // Apply the operator in the hierarchical basis.
  virtual void Apply(Eigen::VectorXd &vec_in) const final;

  // Apply the operator in single scale, to be implemented by derived.
  void ApplySingleScale(Eigen::VectorXd &vec_SS) const;

  const Eigen::SparseMatrix<double> &MatrixSingleScale() const {
    assert(matrix_.nonZeros());
    return matrix_;
  }

  // Hierarhical Basis Transformations from HB to SS, and its transpose.
  void ApplyHierarchToSingle(Eigen::VectorXd &vec_HB) const;
  void ApplyTransposeHierarchToSingle(Eigen::VectorXd &vec_SS) const;

 protected:
  void InitializeMatrixSingleScale();
  Eigen::SparseMatrix<double> matrix_;
};

class BackwardOperator : public Operator {
 public:
  BackwardOperator(const TriangulationView &triang,
                   OperatorOptions opts = OperatorOptions());

  // Apply the operator in the hierarchical basis.
  virtual void Apply(Eigen::VectorXd &vec_in) const final;

  // Apply the operator in single scale, to be implemented by derived.
  virtual void ApplySingleScale(Eigen::VectorXd &vec_SS) const = 0;

 protected:
  // Inverse Hierarhical Basis Transformations.
  void ApplyInverseHierarchToSingle(Eigen::VectorXd &vec_SS) const;
  void ApplyTransposeInverseHierarchToSingle(Eigen::VectorXd &vec_HB) const;

  // Vertex-to-DoF transformation matrices.
  Eigen::SparseMatrix<double> transform_;
  Eigen::SparseMatrix<double> transformT_;
};

/**
 *  Implementation of the actual operators.
 */
class MassOperator : public ForwardOperator<MassOperator> {
 public:
  // Inherit constructor.
  using ForwardOperator::ForwardOperator;

  // Returns the element matrix for the given element.
  inline static Eigen::Matrix3d ElementMatrix(const Element2D *elem,
                                              const OperatorOptions &opts);
};

class StiffnessOperator : public ForwardOperator<StiffnessOperator> {
 public:
  // Inherit constructor.
  using ForwardOperator::ForwardOperator;

  // Returns the element matrix for the given element.
  inline static const Eigen::Matrix3d &ElementMatrix(
      const Element2D *elem, const OperatorOptions &opts);
};

class StiffPlusScaledMassOperator
    : public ForwardOperator<StiffPlusScaledMassOperator> {
 public:
  // Inherit constructor.
  using ForwardOperator::ForwardOperator;

  // Returns the element matrix for the given element.
  inline static Eigen::Matrix3d ElementMatrix(const Element2D *elem,
                                              const OperatorOptions &opts);
};

template <typename ForwardOp>
class DirectInverse : public BackwardOperator {
 public:
  DirectInverse(const TriangulationView &triang,
                OperatorOptions opts = OperatorOptions());

  void ApplySingleScale(Eigen::VectorXd &vec_SS) const final;

 protected:
  Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
      solver_;
};

template <typename ForwardOp>
class CGInverse : public BackwardOperator {
 public:
  CGInverse(const TriangulationView &triang,
            OperatorOptions opts = OperatorOptions());

  void ApplySingleScale(Eigen::VectorXd &vec_SS) const final;

 protected:
  Eigen::ConjugateGradient<Eigen::SparseMatrix<double>,
                           Eigen::Lower | Eigen::Upper>
      solver_;
};

template <typename ForwardOp>
class MultigridPreconditioner : public BackwardOperator {
 public:
  MultigridPreconditioner(const TriangulationView &triang,
                          OperatorOptions opts = OperatorOptions());

  void ApplySingleScale(Eigen::VectorXd &vec_SS) const final;

  inline void Prolongate(uint vertex, Eigen::VectorXd &vec_SS) const {
    for (auto gp : triang_.Godparents(vertex))
      vec_SS[vertex] += 0.5 * vec_SS[gp];
  }

  inline void Restrict(uint vertex, Eigen::VectorXd &vec_SS) const {
    for (auto gp : triang_.Godparents(vertex))
      vec_SS[gp] += 0.5 * vec_SS[vertex];
  }

  inline void RestrictInverse(uint vertex, Eigen::VectorXd &vec_SS) const {
    for (auto gp : triang_.Godparents(vertex))
      vec_SS[gp] -= 0.5 * vec_SS[vertex];
  }

 protected:
  // Initializes the static variable row_mat.
  void InitializeMultigridMatrix() const;

  // Returns a row of the _forward_ matrix on the given multilevel triang.
  // NOTE: The result might not be compressed.
  inline void RowMatrix(uint vertex,
                        std::vector<std::pair<uint, double>> &result) const;

  // Forward operator on the finest level.
  ForwardOp forward_op_;

  // Solver on the coarsest level.
  DirectInverse<ForwardOp> initial_triang_solver_;

  // (Static) variables reused for calculation of the multigrid matrix.
  static std::vector<std::vector<std::pair<uint, double>>> row_mat;
  static std::vector<std::vector<Element2D *>> patches;
  static std::vector<std::vector<uint>> vertices_relaxation;
};

template <template <typename> class InverseOp>
class XPreconditionerOperator : public BackwardOperator {
 public:
  XPreconditionerOperator(const TriangulationView &triang,
                          OperatorOptions opts = OperatorOptions());

  void ApplySingleScale(Eigen::VectorXd &vec_SS) const final;

 protected:
  StiffnessOperator stiff_op_;
  InverseOp<StiffPlusScaledMassOperator> inverse_op_;
};

extern template class ForwardOperator<MassOperator>;
extern template class ForwardOperator<StiffnessOperator>;
extern template class ForwardOperator<StiffPlusScaledMassOperator>;

extern template class DirectInverse<MassOperator>;
extern template class DirectInverse<StiffnessOperator>;
extern template class DirectInverse<StiffPlusScaledMassOperator>;

extern template class MultigridPreconditioner<MassOperator>;
extern template class MultigridPreconditioner<StiffnessOperator>;
extern template class MultigridPreconditioner<StiffPlusScaledMassOperator>;

extern template class XPreconditionerOperator<DirectInverse>;
extern template class XPreconditionerOperator<MultigridPreconditioner>;

}  // namespace space


#include <iostream>
#include <vector>
#include <locale>
#include <omp.h>

#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;

#include "ThreadPool.h"

// Included from collat_asm.asm (.o/.obj)
int16_t collatz(uint64_t *, int64_t);

struct record_t {
	int64_t number;
	int16_t steps;
};

int64_t overflowCollatz(uint64_t number_, int64_t bufferSize, int16_t &extraSteps) {
	cpp_int number = number_;

	while (number > bufferSize) {
		if (number % 2 == 0) {
			number = number / 2;
		}
		else {
			number = number * 3 + 1;
		}
		extraSteps++;
	}

	return number.convert_to<int64_t>();
}

void fillLUT(std::vector<int16_t> &LUT, record_t &largest) {

	std::vector<int64_t> sequence;
	int64_t LUTSum = 0;

	// Fill the lookup table with distances
	for (int64_t i = 3; i < LUT.size(); i++) {

		// If the number has already been added to the look up table
		if (LUT[i] != 0) {
			if (LUT[i] > largest.steps) {
				largest.steps = LUT[i];
				largest.number = i;
				// VT100 escape character for clearing line only works if running through visual studio
				std::cout << "\r                                                                                ";//"\33[2K";
				std::cout << "\r" << largest.number << " : " << largest.steps << "\n";
			}
			continue;
		}

		int64_t number = i;

		// Compute sequence until we find a number that has already been computed
		while (true) {
			if (number < LUT.size()) {
				if (LUT[number] != 0) break;
				sequence.push_back(number);
			}
			else {
				// Don't need to save the number if the sequence goes above the lookup table size
				// but 0 is used as a place holder to get the correct amount of steps
				sequence.push_back(0);
			}

			if (number % 2 == 0) {
				number = number / 2;
			}
			else {
				number = 3 * number + 1;
			}
		}

		// Iterate through the sequence and add all steps to the lookup table
		int16_t sequenceSteps = 0;
		int16_t LUTSteps = LUT[number];
		for (auto it = sequence.rbegin(); it != sequence.rend(); it++) {
			sequenceSteps++;

			if (*it > 0) {
				LUT[*it] = LUTSteps + sequenceSteps;
				LUTSum += static_cast<int64_t>(LUTSteps) + sequenceSteps;
			}
		}

		sequence.clear();

		if (LUT[i] > largest.steps) {
			largest.steps = LUT[i];
			largest.number = i;
			// VT100 escape character for clearing line only works if running through visual studio
			std::cout << "\r                                                                                ";//"\33[2K";
			std::cout << "\r" << i << " : " << largest.steps << "\n";
			std::cout << "Filling lookup table. Currently on: " << i << " of " << LUT.size() << std::flush;
		}
		else if (i % 1000000 == 0) {
			std::cout << "\rFilling lookup table. Currently on: " << i << " of " << LUT.size() << std::flush;
		}
	}

	std::cout << "\33[2K";
	std::cout << "\r" << "Lookup table filled. Avg steps: " << LUTSum / LUT.size() << "\n";
}

class multiThreadCollatz {
public:
	std::vector<int16_t> &LUT;
	std::vector<std::vector<record_t>> &potentialRecords;
	int64_t start;
	int64_t end;
	int64_t blockIndex;
	record_t largest;

	void operator()() {
		for (int64_t j = start; j < end; j++) {
			uint64_t number = j;

			int16_t steps = collatz(&number, LUT.size());
			if (number > LUT.size()) {
				number = overflowCollatz(number, LUT.size(), steps);
			}

			int16_t totalSteps = LUT[number] + steps;
			if (totalSteps > largest.steps) {
				largest = record_t{ j, totalSteps };
				potentialRecords[blockIndex].push_back(largest);
			}
		}
	}
};

enum LUT_SIZE : long long { L1 = 16000, L2 = 130000, L3 = 1000000, RAM = 5000000000 };

int main() {
	// Adding space as a thousands separator for cout
	struct separate_thousands : std::numpunct<char> {
		char_type do_thousands_sep() const override { return ' '; }  // separate with space
		string_type do_grouping() const override { return "\3"; } // groups of 3 digit
	};
	auto thousands = std::make_unique<separate_thousands>();
	std::cout.imbue(std::locale(std::cout.getloc(), thousands.release()));

	const int64_t LUTSize = LUT_SIZE::RAM;
	const int64_t blocks = 100;
	const int64_t iterationsPerBlock = 1e6;

	// Allocate memory for the lookup table
	std::cout << "Allocting " << LUTSize * sizeof(int16_t) << " Bytes of memory" << std::endl;
	std::vector<int16_t> LUT(LUTSize);

	// Setting up the beginning of the lookup table
	LUT[2] = 1;
	std::cout << "1 : " << LUT[1] << std::endl;
	std::cout << "2 : " << LUT[2] << std::endl;
	record_t largest{ 2, LUT[2] };

	// Fill the lookup table and update the largest distance
	fillLUT(LUT, largest);

	ThreadPool<multiThreadCollatz> pool(10);

	// Each block stores all potential records in their own vector
	std::vector<std::vector<record_t>> potentialRecords(blocks);

	std::vector<record_t> records;
	for (int64_t i = LUTSize; i < INT64_MAX; i += (blocks * iterationsPerBlock)) {
		std::cout << "\rCurrently on " << i << std::flush;

		// Start filling buffer 1
		for (int64_t blockIndex = 0; blockIndex < blocks; blockIndex++) {
			pool.addWork(multiThreadCollatz{ LUT, potentialRecords, i + blockIndex * iterationsPerBlock, i + (blockIndex + 1) * iterationsPerBlock, blockIndex, largest });
		}

		pool.waitForThreads();

		// Combine all potential records in a vector and sort it
		for (std::vector<record_t> &blockRecords : potentialRecords) {
			std::copy(blockRecords.begin(), blockRecords.end(), std::back_inserter(records));
			blockRecords.clear();
		}
		std::sort(records.begin(), records.end(), [](record_t const &rhs, record_t const &lhs) { return rhs.number < lhs.number; });

		for (const record_t record : records) {
			if (record.steps > largest.steps) {
				largest = record;
				std::cout << "\r                                                                                ";//"\33[2K";
				std::cout << "\r" << largest.number << " : " << largest.steps << std::endl;
			}
		}
		records.clear();
	}
}