/*=============================================================================================================
                                                BDSimulations.cpp
===============================================================================================================

 Simulations of genetic rescue

 C++-code accompanying:

		(ms. in prep).

 Written by:
        F.J.H. de Haas
       	Theoretical Biology Group
        University of British Columbia
        the Netherlands

 Program version
		xx/xx/xxxx	:

=============================================================================================================*/

#include <assert.h>
#include <stdlib.h>
#include <iomanip>
#include <string>
#include <iostream>
#include "random.h"
#include "utils.h"
#include <progress.hpp>
#include <mutex>
#include <Rcpp.h>
#include <atomic>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#ifdef _OPENMP
    #include <omp.h>
#endif

enum typenames {AB,Ab,aB,ab};
using namespace boost::accumulators;

struct Parameters{
    Parameters(const int &in_AB0, const int &in_Ab0,const int &in_aB0, const int &in_ab0, const double &in_bA, const double &in_ba, double const& in_dA, double const& in_da, double const&in_r){
        AB0 = in_AB0;
        Ab0 = in_Ab0;
        aB0 = in_aB0;
        ab0 = in_ab0;
        bA = in_bA;
        ba = in_ba;
        dA = in_dA;
        da = in_da;
        r = in_r;
    }

    Parameters(Rcpp::List parslist){
        AB0 = parslist["AB0"];
        Ab0 = parslist["Ab0"];
        aB0 = parslist["aB0"];
        ab0 = parslist["ab0"];
        bA = parslist["bA"];
        ba = parslist["ba"];
        dA = parslist["dA"];
        da = parslist["da"];
        r = parslist["r"];
    }

    int AB0,Ab0,aB0,ab0;
    double bA,ba,dA,da;
    double r;
};

class BDPopulation{
    public:
    BDPopulation(const Parameters &pars) {
        type[AB].push_back(pars.AB0);
        type[Ab].push_back(pars.Ab0);
        type[aB].push_back(pars.aB0);
        type[ab].push_back(pars.ab0);
    }
    bool Iterate(const Parameters &pars){
        /*parents become offspring*/
        type[AB].push_back(type[AB].back());
        type[Ab].push_back(type[Ab].back());
        type[aB].push_back(type[aB].back());
        type[ab].push_back(type[ab].back());
        /*offspring changes*/
        static double f[4];
        assert(pars.da>=0.0);
        assert(pars.dA>=0.0);
        if(CalculateFrequencies(f)==false) {extinct = true; return false;};
        const double sumdeathrate = (f[AB]+f[Ab])*pars.dA+(f[aB]+f[ab])*pars.da;
        const double sumbirthrate = (f[AB]+f[Ab])*pars.bA+(f[aB]+f[ab])*pars.ba;
        const double Pdeath = sumdeathrate/(sumbirthrate+sumdeathrate);
        const double Pbirth = 1-Pdeath;
        const double rD = pars.r*(f[AB]*f[ab]-f[Ab]*f[aB]);
        const double PAB_death = f[AB]*pars.dA/sumdeathrate;
        const double PAb_death = f[Ab]*pars.dA/sumdeathrate;
        const double PaB_death = f[aB]*pars.da/sumdeathrate;
        const double Pab_death = f[ab]*pars.da/sumdeathrate;
        const double PAB_birth = f[AB]-rD;
        const double PAb_birth = f[Ab]+rD;
        const double PaB_birth = f[aB]+rD;
        const double Pab_birth = f[ab]-rD;

        rnd::discrete_distribution birthdeathevent(8);
        birthdeathevent[birth_AB] = Pbirth * PAB_birth;
        birthdeathevent[birth_Ab] = Pbirth * PAb_birth;
        birthdeathevent[birth_aB] = Pbirth * PaB_birth;
        birthdeathevent[birth_ab] = Pbirth * Pab_birth;

        birthdeathevent[death_AB] = Pdeath * PAB_death;
        birthdeathevent[death_Ab] = Pdeath * PAb_death;
        birthdeathevent[death_aB] = Pdeath * PaB_death;
        birthdeathevent[death_ab] = Pdeath * Pab_death;

        const int sample = birthdeathevent.sample();
        switch(sample) {
            case birth_AB:      ++type[AB].back(); break;
            case birth_Ab:      ++type[Ab].back(); break;
            case birth_aB:      ++type[aB].back(); break;
            case birth_ab:      ++type[ab].back(); break;
            case death_AB:      --type[AB].back(); break;
            case death_Ab:      --type[Ab].back(); break;
            case death_aB:      --type[aB].back(); break;
            case death_ab:      --type[ab].back(); break;
        }
        return true;
    }
    inline int returnTypes(const int &index, const int &gen){
        return type[index][gen];
    }
    inline bool getextinction(const int &gen){
        return (type[AB][gen]+type[Ab][gen])!=0 ? false : true;
    }
    inline double getFA(const int &gen){
        //assert(type[AB][gen]+type[Ab][gen] != 0);
        return (double)type[AB][gen] / (double)(type[AB][gen]+type[Ab][gen]);
    }

    inline double getFa(const int &gen){
        //assert(type[aB][gen]+type[aB][gen] != 0);
        return (double)type[aB][gen] / (double)(type[aB][gen]+type[ab][gen]);
    }

    private:
    bool extinct = false;
    enum distribution {birth_AB,birth_Ab,birth_aB,birth_ab,death_AB,death_Ab,death_aB,death_ab};
    std::vector<int> type[4];
    bool CalculateFrequencies(double *f){
        const double sum=(double)(type[AB].back()+type[Ab].back()+type[aB].back()+type[ab].back());
        if(sum<=0){return false;}
        f[AB] = (double)type[AB].back()/sum;
        f[Ab] = (double)type[Ab].back()/sum;
        f[aB] = (double)type[aB].back()/sum;
        f[ab] = (double)type[ab].back()/sum;
        return true;
    }
};

struct RcppOutput{
    RcppOutput(const unsigned int &ngen) : ngennr(ngen) {
        nofixcounter = 0;
        fixcounter = 0;
        type[AB].resize(ngen);
        type[Ab].resize(ngen);
        type[aB].resize(ngen);
        type[ab].resize(ngen);
        FA.resize(ngen);
        Fa.resize(ngen);
    };
    void pushback_protect(BDPopulation* pop){
        if(pop->getextinction(ngennr)){
            ++nofixcounter;
        }
        else{
            ++fixcounter;
            mu_acc.lock();
            for(int t = 0; t < ngennr; ++t){
                type[AB][t](pop->returnTypes(AB,t));
                type[Ab][t](pop->returnTypes(Ab,t));
                type[aB][t](pop->returnTypes(aB,t));
                type[ab][t](pop->returnTypes(ab,t));
                FA[t](pop->getFA(t));
                if(pop->getFa(t) >= 0.0){
                    Fa[t](pop->getFa(t));
                }
            }
            distribution[AB].push_back(pop->returnTypes(AB,ngennr));
            distribution[Ab].push_back(pop->returnTypes(AB,ngennr));
            distribution[aB].push_back(pop->returnTypes(AB,ngennr));
            distribution[ab].push_back(pop->returnTypes(AB,ngennr));
            mu_acc.unlock();
        }
    };
    Rcpp::List pushout(){
        /* make a Rcpp Vector for first 4 entries of the Rlist. */
        Rcpp::NumericVector RcppAB(ngennr);
        Rcpp::NumericVector RcppAb(ngennr);
        Rcpp::NumericVector RcppaB(ngennr);
        Rcpp::NumericVector Rcppab(ngennr);
        Rcpp::NumericVector RcppFA(ngennr);
        Rcpp::NumericVector RcppFa(ngennr);
        Rcpp::NumericVector RcppAB_var(ngennr);
        Rcpp::NumericVector RcppAb_var(ngennr);
        Rcpp::NumericVector RcppaB_var(ngennr);
        Rcpp::NumericVector Rcppab_var(ngennr);
        Rcpp::NumericVector RcppFA_var(ngennr);
        Rcpp::NumericVector RcppFa_var(ngennr);
        Rcpp::NumericVector Time(ngennr);

        for(int t = 0; t < ngennr; ++t){
            RcppAB[t] = mean(type[AB][t]);
            RcppAb[t] = mean(type[Ab][t]);
            RcppaB[t] = mean(type[aB][t]);
            Rcppab[t] = mean(type[ab][t]);
            RcppFA[t] = mean(FA[t]);
            RcppFa[t] = mean(Fa[t]);

            RcppAB_var[t] = variance(type[AB][t]);
            RcppAb_var[t] = variance(type[Ab][t]);
            RcppaB_var[t] = variance(type[aB][t]);
            Rcppab_var[t] = variance(type[ab][t]);
            RcppFA_var[t] = variance(FA[t]);
            RcppFa_var[t] = variance(Fa[t]);

            Time[t] = t;
        }

        return Rcpp::List::create(
            Rcpp::_["t"] = Time,
            Rcpp::_["AB_mean"] = RcppAB,
            Rcpp::_["Ab_mean"] = RcppAb,
            Rcpp::_["aB_mean"] = RcppaB,
            Rcpp::_["ab_mean"] = Rcppab,
            Rcpp::_["FA_mean"] = RcppFA,
            Rcpp::_["Fa_mean"] = RcppFa,

            Rcpp::_["AB_var"] = RcppAB_var,
            Rcpp::_["Ab_var"] = RcppAb_var,
            Rcpp::_["aB_var"] = RcppaB_var,
            Rcpp::_["ab_var"] = Rcppab_var,
            Rcpp::_["FA_var"] = RcppFA_var,
            Rcpp::_["Fa_var"] = RcppFa_var,

            Rcpp::_["extinct"] = static_cast<double>(nofixcounter) / (static_cast<double>(nofixcounter)+static_cast<double>(fixcounter))
        );
    }

    private:
    const int ngennr;
    std::mutex mu_acc;
    std::atomic<int> nofixcounter, fixcounter;
    std::vector<accumulator_set<int, stats<tag::mean, tag::variance > > > type[4];
    std::vector<accumulator_set<double, stats<tag::mean, tag::variance > > > FA,Fa; // Genetic rescue AB/(AB+Ab) & aB/(aB+ab) (a will go extinct)
    std::vector<int> distribution[4];
};

// [[Rcpp::export]]
Rcpp::List BDSim(const int &nrep, const int &tend, const Rcpp::List &parslist, int setthreads = 0, bool progressbar = true){
    rnd::set_seed();
    Parameters pars(parslist);
    #ifdef _OPENMP
        const static int maxthreads = omp_get_max_threads();
        if(setthreads>0) omp_set_num_threads(setthreads);
        else omp_set_num_threads(maxthreads);
        //REprintf("Parallel activated : Number of threads=%i\n",omp_get_max_threads());
    #endif
    Progress p(nrep, progressbar);

    /*collect data */
    RcppOutput dataframe(tend);
    BDPopulation* arrayPopulation[nrep];

    #pragma omp parallel for /* threadprivate(OUTPUT)*/
    for(int j = 0; j < nrep; ++j){
        /* run simulations in parallel*/
        arrayPopulation[j] = new BDPopulation(pars);
        for(int i = 0; i < tend; ++i){arrayPopulation[j]->Iterate(pars);}
        dataframe.pushback_protect(arrayPopulation[j]);
        delete arrayPopulation[j];
        p.increment();
    }

    /*rservoire*/
    return dataframe.pushout();
}


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

#include <nt2/polynomials/functions/laguerre.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/constants/one.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( laguerre_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< integer_<A0> >)(scalar_< arithmetic_<A1> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0,A1>::type result_type;

    NT2_FUNCTOR_CALL(2)
    {
      return nt2::laguerre(a0, result_type(a1));
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A1 is floating_
  /////////////////////////////////////////////////////////////////////////////
  BOOST_DISPATCH_IMPLEMENT  (laguerre_, tag::cpu_,
                            (A0)(A1),
                            ((scalar_<integer_<A0> >))((scalar_<floating_<A1> > ))
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0,A1>::type result_type;
    NT2_FUNCTOR_CALL(2)
    {
      A1 p0 = One<A1>();
      if(a0 == 0) return p0;
      A1 p1 = p0-a1;
      A0 c = 1;
      while(c < a0)
      {
        std::swap(p0, p1);
        p1 = laguerre_next(c, a1, p0, p1);
        ++c;
      }
      return p1;
    }
  private:
    template <class T, class T1, class T2>
    static inline T
    laguerre_next(const uint32_t& n, const T& x, const T1 &Ln, const T2& Lnm1)
    {
      const T np1 = T(oneplus(n));
      return ((T(n) + np1 - x) * Ln - n *Lnm1) / np1;
    }
  };
} }

#endif


//
// $Id$
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// 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 DETERMINEBINWIDTHUTILITIES_H
#define DETERMINEBINWIDTHUTILITIES_H

#include <functional>
#include <boost/cstdint.hpp>
#include <algorithm>

namespace ralab
{
  namespace base
  {
    namespace resample
    {

      typedef boost::int32_t int32_t;
      namespace utilities{

        template<class T>
        struct meanfunctor : std::binary_function<T,T,T>{
          T operator()(const T & x, const T& y){
            return (x+y)/2.;
          }
        };

        template <
            typename InputIterator,
            typename OutputIterator,
            typename TN //= int32_t
            >
        OutputIterator summ
        (
            InputIterator begin, //!< [in] begin
            InputIterator end, //!< [in] end
            OutputIterator destBegin, //!< [out] dest begin
            TN lag = 1//!< [in] an integer indicating which lag to use.
            )
        {
          return( std::transform(begin + lag
                                 , end
                                 , begin
                                 , destBegin
                                 , meanfunctor<typename InputIterator::value_type>())
                  );
        }

        template<typename TRealI>
        double determine(TRealI begin, TRealI end,double maxj=5.){
          //BOOST_ASSERT(!boost::range::is_sorted(begin,end));
          double j = 1.;
          double average = *begin;
          double sum = average;
          int32_t i = 1;
          for(; begin != end ; ++begin, ++i){
              while(*begin > (j+0.5) *average){
                  ++j;
                }
              if(j > maxj){
                  break;
                }
              sum += *begin/j;
              average = sum/static_cast<double>(i);
            }
          return average;

        }
      }

    }
  }
}
#endif


//
// Created by Anshuman Mishra on 11/25/19.
//

// These statements are saying to include source code that is stored somewhere else (these come with most compilers)
#include <iostream>		// For console i/o
#include <vector>		// For arrays that we don't use for linear algebra
#include <string>		// For strings used in file names
#include <fstream>		// For file i/o
#include <iomanip>		// For setprecision when printing numbers to the console or files

#include <boost/math/special_functions/beta.hpp>
#include <algorithm>
#include <Eigen/Dense>

#include "headers/MathUtils.h"
#include "headers/Policy.h"
#include "headers/Cartpole.h"
#include "headers/Gridworld.hpp"
#include "headers/Walk.h"
#include "headers/FourierBasis.h"
#include "headers/HelperFunctions.hh"

using namespace std;			// Some terms below are "inside" std. For example, cout, cin, and endl. Normally you have to write std::cout. This line makes it so that you don't have to write std:: before everything you are using from standard libraries.
using namespace Eigen;
using namespace boost::math;

void checkpoint(){
  cout << "Checkpoint"; getchar();
}

struct HistoryElement{
  vector<double> state;
  int action;
  double reward;

  HistoryElement (vector<double> s, int a, double r){
    state = s;
    action = a;
    reward = r;
  }
};

double PDIS(const vector<HistoryElement>& history, const Policy piE, const Policy piB, double gamma, const FourierBasis& fb){
//  cout << "Called PDIS for history" << endl;
  double value = 0;
  double gammaCoeff = 1.0;
  double piCoeff = 1.0;
  for (HistoryElement el : history){
//    cout << el.state[0] << el.action << el.reward << endl;
    vector<double> phi_vec = fb.basify(el.state);
    Map<VectorXd> phi = VectorXd::Map(&phi_vec[0], phi_vec.size());
    piCoeff *= (piE.getActionProbability(phi, el.action) / piB.getActionProbability(phi, el.action));
    value += gammaCoeff * piCoeff * el.reward;
//    cout << "Gamma: " << gammaCoeff << " Pi: " << piCoeff << " Value: " << value << endl;
//    getchar();
    gammaCoeff *= gamma;
    if (isnan(value)){
      cout << "PDIS is nan" << endl;
      cout << "piCoeff: " << piCoeff << " gammaCoeff: " << gammaCoeff << " Reward: " << el.reward << endl;
      getchar();
    }
  }
//  cout << "PDIS = " << value << endl;
  return value;
}

VectorXd PDIS(const vector<vector<HistoryElement> >& data, const Policy& piE, const Policy& piB, double gamma, const FourierBasis& fb){
//  cout << "Called PDIS for data" << endl;
  VectorXd pdis(data.size());
  for (int h=0; h<data.size(); h++)
    pdis[h] = PDIS(data[h], piE, piB, gamma, fb);
  return pdis;
}

void splitData(
    const vector<vector<HistoryElement> >& data,
    vector<vector<HistoryElement> >& Dc,
    vector<vector<HistoryElement> >& Ds,
    double ratio){
  int d = data.size();
  int i=0;
  while (i<d*ratio){
    Dc.push_back(data[i++]);
  }
  while (i<data.size()){
    Ds.push_back(data[i++]);
  }
  cout << "Split: " << data.size() << " => " << Dc.size() << "," << Ds.size() << endl;
}

bool candidatePassesTest(const vector<vector<HistoryElement>>& Ds, const Policy piE, const Policy piB, const FourierBasis& fb, const double delta, const double confBound){
  cout << "Testing Candidate" << endl;
  VectorXd j = PDIS(Ds, piE, piB, 1.0, fb); // Get the primary objective
  double ub = ttestLowerBound(j, delta);
  cout << "Target: " << confBound << " UB: " << ub << endl;
  return ub > confBound;
}

// The objective function maximized by getCandidateSolution.
double candidateObjective(
    const VectorXd& theta,                // The solution to evaluate
    const void * params[],                // Other terms that we need to compute the objective value, packed into one object - an array of const pointers to objects of unknown types (unknown to CMA-ES, but known to us here as we packed this object)
    mt19937_64& generator)                // The random number generator to use)
{
//  cout << "Evaluating Candidate" << endl;
  const vector<vector<HistoryElement>>* data = (vector<vector<HistoryElement>>*) params[0];
  Policy* piE = (Policy*) params[1];
  const Policy* piB = (Policy*) params[2];
  const FourierBasis* fb = (FourierBasis*) params[3];
  const int* safetyDataSize = (int*) params[4];
  const double* c = (double*) params[5];
  const double* delta = (double*) params[6];

  piE->setTheta(theta);
//  checkpoint();
  VectorXd j = PDIS(*data, *piE, *piB, 1.0, *fb); // Get the primary objective
  double ub = ttestLowerBound(j, *delta, *safetyDataSize);
  double result;
  cout << " Mean: " << j.mean() << " StdDev: " << stddev(j) << " ttest: " << ub << endl;
  if (ub < *c) {
    cout << "Failed Barrier Function" << endl;
    result = -100;
  }
  else
    result = j.mean();
//  cout << "Result : " << result << endl;
  return result;
}

// Use the provided data to get a solution expected to pass the safety test
VectorXd getCandidateSolution(
    const vector<vector<HistoryElement> >& data,
    const double delta,
    const int safetyDataSize,
    Policy& piE,
    const Policy& piB,
    const FourierBasis& fb,
    const double c,
    mt19937_64 & generator) {
  VectorXd initialSolution = piE.getTheta();
  double initialSigma = 2.0*(initialSolution.dot(initialSolution) + 1.0); // A heuristic to select the width of the search based on the weight magnitudes we expect to see.
  int numIterations = 100;                          // Number of iterations that CMA-ES should run. Larger is better, but takes longer.
  bool minimize = false;                            // We want to maximize the candidate objective.
  // Pack parameters of candidate objective into params. In candidateObjective we need to unpack in the same order.
  const void* params[7];

  params[0] = &data;
  params[1] = &piE;
  params[2] = &piB;
  params[3] = &fb;
  params[4] = &safetyDataSize;
  params[5] = &c;
  params[6] = δ

  cout << "Calling CMAES" << endl;
  // Use CMA-ES to get a solution that approximately maximizes candidateObjective
  return CMAES(initialSolution, initialSigma, numIterations, candidateObjective, params, minimize, generator);
}

double getTarget(const vector<vector<HistoryElement>>& data){
  double gamma = 1.0;
  int numEpisodes = data.size();
  VectorXd returns(data.size());
  for (int e = 0; e<data.size(); e++){
    double episodeReturn = 0.0;
    double gammaCoeff = 1.0;
    for (HistoryElement he : data[e]){
      episodeReturn += gammaCoeff*he.reward;
      gammaCoeff *= gamma;
    }
    returns[e] = episodeReturn;
//    if (data[e].size() > 10 || episodeReturn < -10)
//      checkpoint();
  }
  double avgReturn = returns.mean();
  return (avgReturn > 0 ? 1.1 : 0.9)*avgReturn;
}

template <typename Environment>
double getAverageReturn(Environment& e, Policy& pi, int numEpisodes, int maxEpisodeLength, const FourierBasis& fb){
//  Cartpole e;
  mt19937_64 gen(0);
  double gamma = 1.0;
  VectorXd returns(numEpisodes);

  for (int eps = 0; eps < numEpisodes; eps++){
    double curGamma = 1.0;					// We plot the discounted return - this stores gamma^t, which starts at 1.
    bool inTerminalState = false;			// We will use this flag to determine when we should terminate the loop below. If environment[trial].inTerminalState() is slow to call, this saves us from calling it a couple times. For our MDPs it really doesn't matter that we're doing this more efficiently.
    e.newEpisode(gen);	// Reset the environment, telling it to start a new episode.
    vector<double> state = e.getState(gen);	// Get the initial state.
    double episodeReturn = 0.0;
    for (int t = 0; (t < maxEpisodeLength) && (!inTerminalState); t++) { // Loop over time steps in the episode, stopping when we hit the max episode length or when we enter a terminal state.
      vector<double> phi_vec = fb.basify(state);
      VectorXd phi = VectorXd::Map(&phi_vec[0], phi_vec.size());
      int action = pi.getAction(phi);
      double reward = e.update(action, gen); // Apply the action by updating the environment with the chosen action, and get the resulting reward.

      vector<double> nextState = e.getState(gen); // Get the resulting state of the environment from this transition
      inTerminalState = e.inTerminalState(); // Store whether this is next-state is a terminal state.

      episodeReturn += curGamma*reward;
      state = nextState; // Prepare for the next iteration of the loop with this line and the next.
      curGamma *= gamma;
    }
    returns[eps] = episodeReturn;
  }
  cout << "AvgReturn: " << returns.mean() << endl;
  return returns.mean();
}

template <typename Environment>
void generateData(Environment& e, int numEpisodes, int maxEpisodeLength, int order){
  cout << "Generating Data" << endl;
  // Generate Histories for some policy
  // Write to file
  ofstream out("../../../output/data.csv");
//  Cartpole e;
  int stateDim = e.getStateDim();
//  int order = 3;
  int numActions = e.getNumActions();
  FourierBasis fb;
  fb.init(stateDim, 0, order);
  mt19937_64 gen(0);

  out << stateDim << endl;
//  cout << "StateDim in env " << stateDim << " " << e.getState(gen).size() << endl;
//  checkpoint();
  out << numActions << endl;
  out << order << endl;

  Policy p(numActions, fb.getNumOutputs(), 1234);
//  VectorXd params(numActions*fb.getNumOutputs());
//  params << 0.452687,-0.123691,-0.187805,-0.0419247,-0.3103,0.0169222,-0.440849,0.171773,0.122356,-0.0208773,-0.377911,0.24458,-0.345339,0.221081,0.181478,0.0931315,0.316947,-0.237248,-0.0783118,0.102174,0.197309,0.0129477,0.237053,-0.283958,0.0931003,-0.159108,-0.11468,-0.0336945,-0.312665,0.0494717,-0.437631,0.185128,0.13921,0.124541,0.269549,-0.0445983,-0.311025,0.186034,0.181678,0.065481,0.311506,-0.245937,0.320217,-0.266973,-0.10468,-0.0398326,0.24824,-0.237777,0.0789096,-0.109312,-0.131987,-0.0215236,-0.190011,0.263565,-0.391198,0.144332,0.109239,0.0786274,0.245533,-0.0450382,0.407343,-0.221437,-0.0711912,-0.023617,0.332143,-0.184834,0.315291,-0.193174,-0.122204,-0.08163,-0.262694,0.218761,0.136993,-0.0538062,-0.220642,0.0300469,-0.256064,0.217929,-0.118554,0.118296,0.102564,0.289925,-0.14454,0.0308677,-0.036806,-0.119257,-0.000854144,-0.27288,0.174216,-0.101032,0.0331704,-0.236314,0.194932,-0.219322,0.183163,-0.00102567,0.0264519,0.160525,-0.151711,-0.0274355,0.101995,0.0911902,-0.0420769,0.179202,-0.249171,0.0371926,-0.122774,-0.0147314,-0.0374526,-0.1332,0.0527589,-0.267015,0.200302,-0.0525608,0.117418,0.0845154,-0.0264594,-0.182052,0.195832,0.0153543,0.0274128,0.176015,-0.22169,0.192534,-0.244502,0.0506698,-0.0728419,0.158957,-0.293461,-0.0106893,-0.141654,-0.00546437,0.0100509,-0.113205,0.273374,-0.185785,0.167286,-0.0742353,0.0805248,0.0537695,-0.0490843,0.207685,-0.225989,0.101884,-0.0554201,0.15327,-0.205815,0.138937,-0.206245,0.060542,-0.052369,-0.0965512,0.20772,-0.0393728,-0.0666479,-0.00757391,0.10165,-0.104987,0.26088,0.0429262,0.137972,-0.0754444;
//  p.setTheta(params);
  VectorXd params = p.getTheta();
  for (int i=0; i<params.size(); i++){
    out << params[i] << ",";
  }
  out << endl;

  out << numEpisodes << endl;

  double gamma = 1.0;

  vector<HistoryElement> firstEps;
  for (int eps = 0; eps < numEpisodes; eps++){
    double curGamma = 1.0;					// We plot the discounted return - this stores gamma^t, which starts at 1.
    bool inTerminalState = false;			// We will use this flag to determine when we should terminate the loop below. If environment[trial].inTerminalState() is slow to call, this saves us from calling it a couple times. For our MDPs it really doesn't matter that we're doing this more efficiently.
    e.newEpisode(gen);	// Reset the environment, telling it to start a new episode.
    vector<double> state = e.getState(gen);	// Get the initial state.
    for (int t = 0; (t < maxEpisodeLength) && (!inTerminalState); t++) { // Loop over time steps in the episode, stopping when we hit the max episode length or when we enter a terminal state.
      vector<double> phi_vec = fb.basify(state);
      VectorXd phi = VectorXd::Map(&phi_vec[0], phi_vec.size());
      int action = p.getAction(phi);
      double reward = e.update(action, gen); // Apply the action by updating the environment with the chosen action, and get the resulting reward.

      for (double val : state)
        out << val << ",";
      out << action << ",";
      out << reward;

      if (eps == 0)
        firstEps.push_back(HistoryElement(state, action, reward));

      vector<double> nextState = e.getState(gen); // Get the resulting state of the environment from this transition
      inTerminalState = e.inTerminalState(); // Store whether this is next-state is a terminal state.

      if ((t < maxEpisodeLength-1) && (!inTerminalState))
        out << ",";

      state = nextState; // Prepare for the next iteration of the loop with this line and the next.
      curGamma *= gamma;
    }
    out<<endl;
  }

  for (int t=0; t<firstEps.size(); t++){
    HistoryElement ht = firstEps[t];
    vector<double> phi_vec = fb.basify(ht.state);
    VectorXd phi = VectorXd::Map(&phi_vec[0], phi_vec.size());
    out<< p.getActionProbability(phi, ht.action);
    out << ((t==firstEps.size()-1) ? '\n' : ',');
  }

  cout << "Data Generation Complete" << endl;
}

vector<Policy> HCOPE(
    vector<vector<HistoryElement>>& data,
    const int numPolicies,
    const double delta,
    const double target,
    const Policy& piB,
    const FourierBasis& fb,
    const string outputDir){
  // Split Data into Ds and Dc
  vector<vector<HistoryElement> > Ds, Dc;
  double dataSplitRatio = 0.6;
  splitData(data, Dc, Ds, dataSplitRatio);

//  int numPolicies = 100;

  mt19937_64 generator(123);
  cout << "Target: " << target << endl;
//  Policy piE(numActions, fb.getNumOutputs(), 123);
//  Policy piE (piB, 123);

//  VectorXd theta(numActions*fb.getNumOutputs());
//  theta << 1, 1, 0.01, -0.01;
//  piE.setTheta(theta);
//  VectorXd pdis = PDIS(data, piE, piB, 1.0, fb);
//  cout << "PDIS " << pdis.mean() << endl;
//
//  pdis = PDIS(data, piB, piB, 1.0, fb);
//  cout << "PDIS " << pdis.mean() << endl;
//
//  cout << "Mean Return in Data " << target/1.1 << endl;
//  checkpoint();

  // Loop while more policies need to be found
//  ofstream policyOut("../../../output/result.csv");

  vector<Policy> policies(numPolicies, Policy(piB, 123));
  #pragma omp parallel for
  for (int p=0; p<numPolicies; p++){
//  while(policies.size() < numPolicies){
    bool passedTest = false;
    VectorXd candidate;
    while(!passedTest) {
      //   Select Candidate Policy
      policies[p].setTheta(piB.getTheta());
      candidate = getCandidateSolution(Dc, delta, Ds.size(), policies[p], piB,
                                       fb, target, generator);

      cout << "Generated Candidate" << endl;
      policies[p].setTheta(candidate);

      passedTest = candidatePassesTest(Ds, policies[p], piB, fb, delta, target);
      if (passedTest)
        cout << "****************** CANDIDATE PASSED TEST ******************" << endl;
      else
        cout << "****************** CANDIDATE FAILED TEST ******************" << endl;
    }
    //   Store Candidate Policy if Test Passed
    ofstream policyOut(outputDir + to_string(p) + ".csv");
    for (int i=0; i<candidate.size(); i++){
      policyOut << candidate[i];
      policyOut << (i == candidate.size()-1 ? "\n":",") << flush;
      cout << candidate[i];
      cout << (i == candidate.size()-1 ? "\n":",");
    }
    //checkpoint()
  }
  return policies;
}

void readData(
    string filename,
    int& stateDim,
    int& numActions,
    int& order,
    Policy& piB,
    int& numEpisodes,
    FourierBasis& fb,
    vector<vector<HistoryElement> >& data){
  cout << "Reading Data" << endl;
//  ifstream in("../../../output/data.csv");
//  ifstream in("../../../input/data.csv");
  ifstream in(filename);

  in >> stateDim;
  in >> numActions;
  in >> order;

  fb.init(stateDim, 0, order);

  VectorXd thetaB(numActions*fb.getNumOutputs());
//  cout << "ThetaB " << thetaB.size() << endl;

  cout << "Reading Policy" << endl;
  string s;
  for (int i=0; i<thetaB.size(); i++){
    char delim = (i == thetaB.size()-1 ? '\n': ',');
    getline(in, s, delim);
    thetaB[i] = stod(s);
//    cout << thetaB[i] << delim;
  }
  // Complete reading the line
//  getline(in, s);

  piB = Policy(thetaB, numActions, fb.getNumOutputs(), 123);

  in >> numEpisodes;

  for (int eps = 0; eps < numEpisodes; eps++){
    vector<HistoryElement> episode;
//    cout << "Reading Episode " << eps << endl;
    string ep;
    in >> ep;
//    cout << ep << endl;

    stringstream ss(ep);
    while(ss.good()){
      vector<double> state(stateDim, 0.0);
//      cout << "State: ";
      for (int st = 0; st < stateDim; st++) {
        getline(ss, s, ',');
        state[st] = stod(s);
//        cout << state[st] << ",";
      }
//      cout << " Action: ";

      getline(ss, s, ',');
      int action = stoi(s);
//      cout << action << ", Reward: ";
//      cout << action << ",";

      getline(ss, s, ',');
      double reward = stod(s);
//      cout << reward << ",";
//      cout << endl;
//      cin >> s;
      episode.push_back(HistoryElement(state, action, reward));
    }
//    cout << endl;
    data.push_back(episode);
  }

  cout << "Validating Policy Representation" << endl;
  string valPi;
  in >> valPi;
  stringstream ss(valPi);
  VectorXd expectedPi(data[0].size());
  VectorXd actualPi(data[0].size());
  bool match = true;
  for (int t=0; t<data[0].size(); t++){
    HistoryElement ht = data[0][t];
    vector<double> phi_vec = fb.basify(ht.state);
    VectorXd phi = VectorXd::Map(&phi_vec[0], phi_vec.size());
    actualPi[t] = piB.getActionProbability(phi, ht.action);

    string s;
    getline(ss, s, ',');
//    cout << s << ", ";
    expectedPi[t] = stod(s);

    if (abs(expectedPi[t] - actualPi[t]) > 1e-5)
      match = false;
  }

  if (!match){
    cout << "Policies Dont Match!"  << endl;
    cout << "Expected: " << expectedPi.transpose() << endl;
    cout << "Actual: " << actualPi.transpose() << endl;
    checkpoint();
    return;
  }

  cout << "Data Read Complete" << endl;
}

void run() {

  bool test = true;

  /****************************************************************/

  bool genData = false;
  int genEpisodes = 100000, genMaxEpisodeLength = 15, genOrder = 1;
//  Cartpole genEnv;
  Gridworld genEnv;
//  Walk genEnv;
  if (test && genData)
    generateData(genEnv, genEpisodes, genMaxEpisodeLength, genOrder);

  /*********************** End of Data Prep ***********************/

  int stateDim, numActions, order, numEpisodes;
  Policy piB(0,0,0);
  FourierBasis fb;
  vector<vector<HistoryElement> > data;
  if (test)
    readData("../../../output/data.csv", stateDim, numActions, order, piB, numEpisodes, fb, data);
  else
    readData("../../../input/data.csv", stateDim, numActions, order, piB, numEpisodes, fb, data);

  cout << "stateDim: " << stateDim << endl;
  cout << "numActions: " << numActions << endl;
  cout << "order: " << order << endl;
  cout << "Policy: " << piB.getTheta().transpose() << endl;
  cout << "numEpisodes: " << numEpisodes << endl;

  /*********************** End of Data Read ***********************/

//  VectorXd testTheta(piB.getTheta().size());
//  testTheta << 0.33304397, -1.3536084, -0.391217, -2.93140976, -4.07265164, -2.13137546, -1.94398795, 2.53769193;
//  Policy testPolicy (piB);
//  testPolicy.setTheta(testTheta);
//  double avgReturn = getAverageReturn(genEnv, testPolicy, genEpisodes, genMaxEpisodeLength, fb);
//  return;

  /********************* End of Testing Space *********************/

  int numPolicies = 50;
  double delta = 0.05;
  double target = getTarget(data);
  string outputDir;
  if (test)
    outputDir = "../../../result/" + to_string(delta) + "_";
  else
    outputDir = "../../../final_result/";
  vector<Policy> policies = HCOPE(data, numPolicies, delta, target, piB, fb, outputDir);

  /********************* End of Policy Search *********************/

  if (test) {
      // Validate policies found
      cout << "Validating Result Policies" << endl;
      int worked = 0;
      for (auto policy : policies){
        double avgReturn = getAverageReturn(genEnv, policy, genEpisodes, genMaxEpisodeLength, fb);
        if (avgReturn >= target/1.1)
          worked++;
      }

      cout << "Reality Check! #Policies that worked: " << worked << "/" << policies.size() << endl;
  }
}

int main(int argc, char* argv[])
{
  run();
//  VectorXd a(4);
//  a  << 1.0, 2.0, 3.0, 4.0;
//  cout << a << endl;
//  MatrixXd m = MatrixXd::Map(a.data(), 2,2);
//  cout << m << endl;
//
//  VectorXd b(4);
//  b << 2.0,3.0,4.0,5.0;
//  m = MatrixXd::Map(b.data(), m.rows(), m.cols());
//  cout << m << endl;
  cout << "Done." << endl;
}


#pragma once
// value.hpp: definition of a (sign, scale, fraction) representation of an approximation to a real value
//
// Copyright (C) 2017-2018 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.
#include <cassert>
#include <limits>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include "bit_functions.hpp"
#include "trace_constants.hpp"

using boost::multiprecision::cpp_dec_float_50;
using boost::multiprecision::cpp_dec_float_100;

namespace sw {
	namespace unum {

		// Forward definitions
		template<size_t fbits> class value;
		template<size_t fbits> value<fbits> abs(const value<fbits>& v);

		// template class representing a value in scientific notation, using a template size for the number of fraction bits
		template<size_t fbits>
		class value {
		public:
			static constexpr size_t fhbits = fbits + 1;    // number of fraction bits including the hidden bit
			value() : _sign(false), _scale(0), _nrOfBits(fbits), _zero(true), _inf(false), _nan(false) {}
			value(bool sign, int scale, const bitblock<fbits>& fraction_without_hidden_bit, bool zero = true, bool inf = false) : _sign(sign), _scale(scale), _nrOfBits(fbits), _fraction(fraction_without_hidden_bit), _inf(inf), _zero(zero), _nan(false) {}
			value(signed char initial_value) {
				*this = initial_value;
			}
			value(short initial_value) {
				*this = initial_value;
			}
			value(int initial_value) {
				*this = initial_value;
			}
			value(long long initial_value) {
				*this = initial_value;
			}
			value(unsigned long long initial_value) {
				*this = initial_value;
			}
			value(float initial_value) {
				*this = initial_value;
			}
			value(double initial_value) {
				*this = initial_value;
			}
			value(long double initial_value) {
				*this = initial_value;
			}
			value(cpp_dec_float_50 initial_value) {
				*this = initial_value;
			}
			value(cpp_dec_float_100 initial_value) {
				*this = initial_value;
			}
			value(const value& rhs) {
				*this = rhs;
			}
			value& operator=(const value& rhs) {
				_sign	  = rhs._sign;
				_scale	  = rhs._scale;
				_fraction = rhs._fraction;
				_nrOfBits = rhs._nrOfBits;
				_inf      = rhs._inf;
				_zero     = rhs._zero;
				_nan      = rhs._nan;
				return *this;
			}
			value<fbits>& operator=(signed char rhs) {
				*this = (long long)(rhs);
				return *this;
			}
			value<fbits>& operator=(short rhs) {
				*this = (long long)(rhs);
				return *this;
			}
			value<fbits>& operator=(int rhs) {
				*this = (long long)(rhs);
				return *this;
			}
			value<fbits>& operator=(long long rhs) {
				if (_trace_conversion) std::cout << "---------------------- CONVERT -------------------" << std::endl;
				if (rhs == 0) {
					setToZero();
					return *this;
				}
				reset();
				_sign = (0x8000000000000000 & rhs);  // 1 is negative, 0 is positive
				if (_sign) {
					// process negative number: process 2's complement of the input
					_scale = findMostSignificantBit(-rhs) - 1;
					uint64_t _fraction_without_hidden_bit = (-rhs << (64 - _scale));
					_fraction = copy_integer_fraction<fbits>(_fraction_without_hidden_bit);
					//take_2s_complement();
					_nrOfBits = fbits;
					if (_trace_conversion) std::cout << "int64 " << rhs << " sign " << _sign << " scale " << _scale << " fraction b" << _fraction << std::dec << std::endl;
				}
				else {
					// process positive number
					if (rhs != 0) {
						_scale = findMostSignificantBit(rhs) - 1;
						uint64_t _fraction_without_hidden_bit = (rhs << (64 - _scale));
						_fraction = copy_integer_fraction<fbits>(_fraction_without_hidden_bit);
						_nrOfBits = fbits;
						if (_trace_conversion) std::cout << "int64 " << rhs << " sign " << _sign << " scale " << _scale << " fraction b" << _fraction << std::dec << std::endl;
					}
				}
				return *this;
			}
			value<fbits>& operator=(unsigned long long rhs) {
				if (_trace_conversion) std::cout << "---------------------- CONVERT -------------------" << std::endl;
				if (rhs == 0) {
					setToZero();
				}
				else {
					reset();
					_scale = findMostSignificantBit(rhs) - 1;
					uint64_t _fraction_without_hidden_bit = (rhs << (64 - _scale));
					_fraction = copy_integer_fraction<fbits>(_fraction_without_hidden_bit);
					_nrOfBits = fbits;
				}
				if (_trace_conversion) std::cout << "uint64 " << rhs << " sign " << _sign << " scale " << _scale << " fraction b" << _fraction << std::dec << std::endl;
				return *this;
			}
			value<fbits>& operator=(float rhs) {
				reset();
				if (_trace_conversion) std::cout << "---------------------- CONVERT -------------------" << std::endl;

				switch (std::fpclassify(rhs)) {
				case FP_ZERO:
					_nrOfBits = fbits;
					_zero = true;
					break;
				case FP_INFINITE:
					_inf  = true;
					break;
				case FP_NAN:
					_nan = true;
					break;
				case FP_SUBNORMAL:
					std::cerr << "TODO: subnormal number: returning 0" << std::endl;
					break;
				case FP_NORMAL:
				{
					float _fr;
					unsigned int _23b_fraction_without_hidden_bit;
					int _exponent;
					extract_fp_components(rhs, _sign, _exponent, _fr, _23b_fraction_without_hidden_bit);
					_scale = _exponent - 1;
					_fraction = extract_23b_fraction<fbits>(_23b_fraction_without_hidden_bit);
					_nrOfBits = fbits;
					if (_trace_conversion) std::cout << "float " << rhs << " sign " << _sign << " scale " << _scale << " 23b fraction 0x" << std::hex << _23b_fraction_without_hidden_bit << " _fraction b" << _fraction << std::dec << std::endl;
				}
				break;
				}
				return *this;
			}
			value<fbits>& operator=(double rhs) {
				reset();
				if (_trace_conversion) std::cout << "---------------------- CONVERT -------------------" << std::endl;

				switch (std::fpclassify(rhs)) {
				case FP_ZERO:
					_nrOfBits = fbits;
					_zero = true;
					break;
				case FP_INFINITE:
					_inf = true;
					break;
				case FP_NAN:
					_nan = true;
					break;
				case FP_SUBNORMAL:
					std::cerr << "TODO: subnormal number: returning 0" << std::endl;
					break;
				case FP_NORMAL:
				{
					double _fr;
					unsigned long long _52b_fraction_without_hidden_bit;
					int _exponent;
					extract_fp_components(rhs, _sign, _exponent, _fr, _52b_fraction_without_hidden_bit);
					_scale = _exponent - 1;
					_fraction = extract_52b_fraction<fbits>(_52b_fraction_without_hidden_bit);
					_nrOfBits = fbits;
					if (_trace_conversion) std::cout << "double " << rhs << " sign " << _sign << " scale " << _scale << " 52b fraction 0x" << std::hex << _52b_fraction_without_hidden_bit << " _fraction b" << _fraction << std::dec << std::endl;
				}
				break;
				}
				return *this;
			}
			value<fbits>& operator=(long double rhs) {
				reset();
				if (_trace_conversion) std::cout << "---------------------- CONVERT -------------------" << std::endl;

				switch (std::fpclassify(rhs)) {
				case FP_ZERO:
					_nrOfBits = fbits;
					_zero = true;
					break;
				case FP_INFINITE:
					_inf = true;
					break;
				case FP_NAN:
					_nan = true;
					break;
				case FP_SUBNORMAL:
					std::cerr << "TODO: subnormal number: returning 0" << std::endl;
					break;
				case FP_NORMAL:
				{
					long double _fr;
					unsigned long long _63b_fraction_without_hidden_bit;
					int _exponent;
					extract_fp_components(rhs, _sign, _exponent, _fr, _63b_fraction_without_hidden_bit);
					_scale = _exponent - 1;
					// how to interpret the fraction bits: TODO: this should be a static compile-time code block
					if (sizeof(long double) == 8) {
						// we are just a double and thus only have 52bits of fraction
						_fraction = extract_52b_fraction<fbits>(_63b_fraction_without_hidden_bit);
						if (_trace_conversion) std::cout << "long double " << rhs << " sign " << _sign << " scale " << _scale << " 52b fraction 0x" << std::hex << _63b_fraction_without_hidden_bit << " _fraction b" << _fraction << std::dec << std::endl;

					}
					else if (sizeof(long double) == 16) {
						// how to differentiate between 80bit and 128bit formats?
						_fraction = extract_63b_fraction<fbits>(_63b_fraction_without_hidden_bit);
						if (_trace_conversion) std::cout << "long double " << rhs << " sign " << _sign << " scale " << _scale << " 63b fraction 0x" << std::hex << _63b_fraction_without_hidden_bit << " _fraction b" << _fraction << std::dec << std::endl;

					}
					_nrOfBits = fbits;
				}
				break;
				}
				return *this;
			}

			// operators
			value<fbits> operator-() const {
				return value<fbits>(!_sign, _scale, _fraction, _zero, _inf);
			}

			// modifiers
			void reset() {
				_sign  = false;
				_scale = 0;
				_nrOfBits = 0;
				_inf = false;
				_zero = false;
				_nan = false;
				_fraction.reset();
			}
			void set(bool sign, int scale, bitblock<fbits> fraction_without_hidden_bit, bool zero, bool inf, bool nan = false) {
				_sign     = sign;
				_scale    = scale;
				_fraction = fraction_without_hidden_bit;
				_zero     = zero;
				_inf      = inf;
				_nan      = nan;
			}
			void setToZero() {
				_zero     = true;
				_sign     = false;
				_inf      = false;
				_nan      = false;
				_scale    = 0;
				_nrOfBits = fbits;
				_fraction.reset();
			}
			void setToInfinite() {
				_inf      = true;
				_sign     = false;
				_zero     = false;
				_nan      = false;
				_scale    = 0;
				_nrOfBits = fbits;
				_fraction.reset();
			}
			void setToNan() {
				_nan      = true;
				_sign     = false;
				_zero     = false;
				_inf      = false;
				_scale    = 0;
				_nrOfBits = fbits;
				_fraction.reset();
			}
			inline void setExponent(int e) { _scale = e; }
			inline bool isNegative() const { return _sign; }
			inline bool isZero() const { return _zero; }
			inline bool isInfinite() const { return _inf; }
			inline bool isNaN() const { return _nan; }
			inline bool sign() const { return _sign; }
			inline int scale() const { return _scale; }
			bitblock<fbits> fraction() const { return _fraction; }
			/// Normalized shift (e.g., for addition).
			template <size_t Size>
			bitblock<Size> nshift(long shift) const {
				bitblock<Size> number;

				// Check range
				if (long(fbits) + shift >= long(Size))
					throw shift_too_large{};

				const long hpos = fbits + shift;       // position of hidden bit

				if (hpos <= 0) {   // If hidden bit is LSB or beyond just set uncertainty bit and call it a day
					number[0] = true;
					return number;
				}
				number[hpos] = true;                   // hidden bit now safely set

													   // Copy fraction bits into certain part
				for (long npos = hpos - 1, fpos = long(fbits) - 1; npos > 0 && fpos >= 0; --npos, --fpos)
					number[npos] = _fraction[fpos];

				// Set uncertainty bit
				bool uncertainty = false;
				for (long fpos = std::min(long(fbits) - 1, -shift); fpos >= 0 && !uncertainty; --fpos)
					uncertainty |= _fraction[fpos];
				number[0] = uncertainty;
				return number;
			}
			// get a fixed point number by making the hidden bit explicit: useful for multiply units
			bitblock<fhbits> get_fixed_point() const {
				bitblock<fbits + 1> fixed_point_number;
				fixed_point_number.set(fbits, true); // make hidden bit explicit
				for (unsigned int i = 0; i < fbits; i++) {
					fixed_point_number[i] = _fraction[i];
				}
				return fixed_point_number;
			}
			// get the fraction value including the implicit hidden bit (this is at an exponent level 1 smaller)
			template<typename Ty = double>
			Ty get_implicit_fraction_value() const {
				if (_zero) return (long double)0.0;
				Ty v = 1.0;
				Ty scale = 0.5;
				for (int i = int(fbits) - 1; i >= 0; i--) {
					if (_fraction.test(i)) v += scale;
					scale *= 0.5;
					if (scale == 0.0) break;
				}
				return v;
			}
			int sign_value() const { return (_sign ? -1 : 1); }
			double scale_value() const {
				if (_zero) return (long double)(0.0);
				return std::pow((long double)2.0, (long double)_scale);
			}
			template<typename Ty = double>
			Ty fraction_value() const {
				if (_zero) return (long double)0.0;
				Ty v = 1.0;
				Ty scale = 0.5;
				for (int i = int(fbits) - 1; i >= 0; i--) {
					if (_fraction.test(i)) v += scale;
					scale *= 0.5;
					if (scale == 0.0) break;
				}
				return v;
			}
			cpp_dec_float_50 to_cpp_dec_float_50() const {
				return sign_value() * scale_value() * fraction_value<cpp_dec_float_50>();
			}
			cpp_dec_float_100 to_cpp_dec_float_100() const {
				return sign_value() * scale_value() * fraction_value<cpp_dec_float_100>();
			}
			long double to_long_double() const {
				return sign_value() * scale_value() * fraction_value<long double>();
			}
			double to_double() const {
				return sign_value() * scale_value() * fraction_value<double>();
			}
			float to_float() const {
				return float(sign_value() * scale_value() * fraction_value<float>());
			}
			// Maybe remove explicit
			explicit operator cpp_dec_float_50() const { return to_cpp_dec_float_50(); }
			explicit operator cpp_dec_float_100() const { return to_cpp_dec_float_100(); }
			explicit operator long double() const { return to_long_double(); }
			explicit operator double() const { return to_double(); }
			explicit operator float() const { return to_float(); }

			template<size_t tgt_size>
			value<tgt_size> round_to() {
				bitblock<tgt_size> rounded_fraction;
				if (tgt_size == 0) {
					bool round_up = false;
					if (fbits >= 2) {
						bool blast = _fraction[int(fbits) - 1];
						bool sb = anyAfter(_fraction, int(fbits) - 2);
						if (blast && sb) round_up = true;
					}
					else if (fbits == 1) {
						round_up = _fraction[0];
					}
					return value<tgt_size>(_sign, (round_up ? _scale + 1 : _scale), rounded_fraction, _zero, _inf);
				}
				else {
					if (!_zero || !_inf) {
						if (tgt_size < fbits) {
							int rb = int(tgt_size) - 1;
							int lb = int(fbits) - int(tgt_size) - 1;
							for (int i = int(fbits) - 1; i > lb; i--, rb--) {
								rounded_fraction[rb] = _fraction[i];
							}
							bool blast = _fraction[lb];
							bool sb = false;
							if (lb > 0) sb = anyAfter(_fraction, lb-1);
							if (blast || sb) rounded_fraction[0] = true;
						}
						else {
							int rb = int(tgt_size) - 1;
							for (int i = int(fbits) - 1; i >= 0; i--, rb--) {
								rounded_fraction[rb] = _fraction[i];
							}
						}
					}
				}
				return value<tgt_size>(_sign, _scale, rounded_fraction, _zero, _inf);
			}
		private:
			bool				_sign;
			int					_scale;
			bitblock<fbits>	    _fraction;
			int					_nrOfBits;  // in case the fraction is smaller than the full fbits
			bool                _inf;
			bool                _zero;
			bool                _nan;

			// template parameters need names different from class template parameters (for gcc and clang)
			template<size_t nfbits>
			friend std::ostream& operator<< (std::ostream& ostr, const value<nfbits>& r);
			template<size_t nfbits>
			friend std::istream& operator>> (std::istream& istr, value<nfbits>& r);

			template<size_t nfbits>
			friend bool operator==(const value<nfbits>& lhs, const value<nfbits>& rhs);
			template<size_t nfbits>
			friend bool operator!=(const value<nfbits>& lhs, const value<nfbits>& rhs);
			template<size_t nfbits>
			friend bool operator< (const value<nfbits>& lhs, const value<nfbits>& rhs);
			template<size_t nfbits>
			friend bool operator> (const value<nfbits>& lhs, const value<nfbits>& rhs);
			template<size_t nfbits>
			friend bool operator<=(const value<nfbits>& lhs, const value<nfbits>& rhs);
			template<size_t nfbits>
			friend bool operator>=(const value<nfbits>& lhs, const value<nfbits>& rhs);
		};

		////////////////////// VALUE operators
		template<size_t nfbits>
		inline std::ostream& operator<<(std::ostream& ostr, const value<nfbits>& v) {
			if (v._inf) {
				ostr << FP_INFINITE;
			}
			else {
				ostr << (long double)v;
			}
			return ostr;
		}

		template<size_t nfbits>
		inline std::istream& operator>> (std::istream& istr, const value<nfbits>& v) {
			istr >> v._fraction;
			return istr;
		}

		template<size_t nfbits>
		inline bool operator==(const value<nfbits>& lhs, const value<nfbits>& rhs) { return lhs._sign == rhs._sign && lhs._scale == rhs._scale && lhs._fraction == rhs._fraction && lhs._nrOfBits == rhs._nrOfBits && lhs._zero == rhs._zero && lhs._inf == rhs._inf; }
		template<size_t nfbits>
		inline bool operator!=(const value<nfbits>& lhs, const value<nfbits>& rhs) { return !operator==(lhs, rhs); }
		template<size_t nfbits>
		inline bool operator< (const value<nfbits>& lhs, const value<nfbits>& rhs) { return lhs.to_long_double() < rhs.to_long_double(); }
		template<size_t nfbits>
		inline bool operator> (const value<nfbits>& lhs, const value<nfbits>& rhs) { return  operator< (rhs, lhs); }
		template<size_t nfbits>
		inline bool operator<=(const value<nfbits>& lhs, const value<nfbits>& rhs) { return !operator> (lhs, rhs); }
		template<size_t nfbits>
		inline bool operator>=(const value<nfbits>& lhs, const value<nfbits>& rhs) { return !operator< (lhs, rhs); }

		template<size_t fbits>
		inline std::string components(const value<fbits>& v) {
			std::stringstream s;
			if (v.isZero()) {
				s << " zero b" << std::setw(fbits) << v.fraction();
				return s.str();
			}
			else if (v.isInfinite()) {
				s << " infinite b" << std::setw(fbits) << v.fraction();
				return s.str();
			}
			s << "(" << (v.sign() ? "-" : "+") << "," << v.scale() << "," << v.fraction() << ")";
			return s.str();
		}

		/// Magnitude of a scientific notation value (equivalent to turning the sign bit off).
		template<size_t nfbits>
		value<nfbits> abs(const value<nfbits>& v) {
			return value<nfbits>(false, v.scale(), v.fraction(), v.isZero());
		}

		// add module
		template<size_t fbits, size_t abits>
		void module_add(const value<fbits>& lhs, const value<fbits>& rhs, value<abits + 1>& result) {
			// with sign/magnitude adders it is customary to organize the computation
			// along the four quadrants of sign combinations
			//  + + = +
			//  + - =   lhs > rhs ? + : -
			//  - + =   lhs > rhs ? - : +
			//  - - =
			// to simplify the result processing assign the biggest
			// absolute value to R1, then the sign of the result will be sign of the value in R1.

			if (lhs.isInfinite() || rhs.isInfinite()) {
				result.setToInfinite();
				return;
			}
			int lhs_scale = lhs.scale(), rhs_scale = rhs.scale(), scale_of_result = std::max(lhs_scale, rhs_scale);

			// align the fractions
			bitblock<abits> r1 = lhs.template nshift<abits>(lhs_scale - scale_of_result + 3);
			bitblock<abits> r2 = rhs.template nshift<abits>(rhs_scale - scale_of_result + 3);
			bool r1_sign = lhs.sign(), r2_sign = rhs.sign();
			bool signs_are_different = r1_sign != r2_sign;

			if (signs_are_different && sw::unum::abs(lhs) < sw::unum::abs(rhs)) {
				std::swap(r1, r2);
				std::swap(r1_sign, r2_sign);
			}

			if (signs_are_different) r2 = twos_complement(r2);

			if (_trace_add) {
				std::cout << (r1_sign ? "sign -1" : "sign  1") << " scale " << std::setw(3) << scale_of_result << " r1       " << r1 << std::endl;
				std::cout << (r2_sign ? "sign -1" : "sign  1") << " scale " << std::setw(3) << scale_of_result << " r2       " << r2 << std::endl;
			}

			bitblock<abits + 1> sum;
			const bool carry = add_unsigned(r1, r2, sum);

			if (_trace_add) std::cout << (r1_sign ? "sign -1" : "sign  1") << " carry " << std::setw(3) << (carry ? 1 : 0) << " sum     " << sum << std::endl;

			long shift = 0;
			if (carry) {
				if (r1_sign == r2_sign) {  // the carry && signs== implies that we have a number bigger than r1
					shift = -1;
				}
				else {
					// the carry && signs!= implies r2 is complement, result < r1, must find hidden bit (in the complement)
					for (int i = abits - 1; i >= 0 && !sum[i]; i--) {
						shift++;
					}
				}
			}
			assert(shift >= -1);

			if (shift >= long(abits)) {            // we have actual 0
				sum.reset();
				result.set(false, 0, sum, true, false, false);
				return;
			}

			scale_of_result -= shift;
			const int hpos = abits - 1 - shift;         // position of the hidden bit
			sum <<= abits - hpos + 1;
			if (_trace_add) std::cout << (r1_sign ? "sign -1" : "sign  1") << " scale " << std::setw(3) << scale_of_result << " sum     " << sum << std::endl;
			result.set(r1_sign, scale_of_result, sum, false, false, false);
		}

		// subtract module: use ADDER
		template<size_t fbits, size_t abits>
		void module_subtract(const value<fbits>& lhs, const value<fbits>& rhs, value<abits + 1>& result) {
			if (lhs.isInfinite() || rhs.isInfinite()) {
				result.setToInfinite();
				return;
			}
			int lhs_scale = lhs.scale(), rhs_scale = rhs.scale(), scale_of_result = std::max(lhs_scale, rhs_scale);

			// align the fractions
			bitblock<abits> r1 = lhs.template nshift<abits>(lhs_scale - scale_of_result + 3);
			bitblock<abits> r2 = rhs.template nshift<abits>(rhs_scale - scale_of_result + 3);
			bool r1_sign = lhs.sign(), r2_sign = !rhs.sign();
			bool signs_are_different = r1_sign != r2_sign;

			if (sw::unum::abs(lhs) < sw::unum::abs(rhs)) {
				std::swap(r1, r2);
				std::swap(r1_sign, r2_sign);
			}

			if (signs_are_different) r2 = twos_complement(r2);

			if (_trace_sub) {
				std::cout << (r1_sign ? "sign -1" : "sign  1") << " scale " << std::setw(3) << scale_of_result << " r1       " << r1 << std::endl;
				std::cout << (r2_sign ? "sign -1" : "sign  1") << " scale " << std::setw(3) << scale_of_result << " r2       " << r2 << std::endl;
			}

			bitblock<abits + 1> sum;
			const bool carry = add_unsigned(r1, r2, sum);

			if (_trace_sub) std::cout << (r1_sign ? "sign -1" : "sign  1") << " carry " << std::setw(3) << (carry ? 1 : 0) << " sum     " << sum << std::endl;

			long shift = 0;
			if (carry) {
				if (r1_sign == r2_sign) {  // the carry && signs== implies that we have a number bigger than r1
					shift = -1;
				}
				else {
					// the carry && signs!= implies r2 is complement, result < r1, must find hidden bit (in the complement)
					for (int i = abits - 1; i >= 0 && !sum[i]; i--) {
						shift++;
					}
				}
			}
			assert(shift >= -1);

			if (shift >= long(abits)) {            // we have actual 0
				sum.reset();
				result.set(false, 0, sum, true, false, false);
				return;
			}

			scale_of_result -= shift;
			const int hpos = abits - 1 - shift;         // position of the hidden bit
			sum <<= abits - hpos + 1;
			if (_trace_sub) std::cout << (r1_sign ? "sign -1" : "sign  1") << " scale " << std::setw(3) << scale_of_result << " sum     " << sum << std::endl;
			result.set(r1_sign, scale_of_result, sum, false, false, false);
		}

		// subtract module using SUBTRACTOR: CURRENTLY BROKEN FOR UNKNOWN REASON
		template<size_t fbits, size_t abits>
		void module_subtract_BROKEN(const value<fbits>& lhs, const value<fbits>& rhs, value<abits + 1>& result) {

			if (lhs.isInfinite() || rhs.isInfinite()) {
				result.setToInfinite();
				return;
			}
			int lhs_scale = lhs.scale(), rhs_scale = rhs.scale(), scale_of_result = std::max(lhs_scale, rhs_scale);

			// align the fractions
			bitblock<abits> r1 = lhs.template nshift<abits>(lhs_scale - scale_of_result + 3);
			bitblock<abits> r2 = rhs.template nshift<abits>(rhs_scale - scale_of_result + 3);
			bool r1_sign = lhs.sign(), r2_sign = rhs.sign();
			bool signs_are_equal = r1_sign == r2_sign;

			if (r1_sign) r1 = twos_complement(r1);
			if (r1_sign) r2 = twos_complement(r2);

			if (_trace_sub) {
				std::cout << (r1_sign ? "sign -1" : "sign  1") << " scale " << std::setw(3) << scale_of_result << " r1       " << r1 << std::endl;
				std::cout << (r2_sign ? "sign -1" : "sign  1") << " scale " << std::setw(3) << scale_of_result << " r2       " << r2 << std::endl;
			}

			bitblock<abits + 1> difference;
			const bool borrow = subtract_unsigned(r1, r2, difference);

			if (_trace_sub) std::cout << (r1_sign ? "sign -1" : "sign  1") << " borrow" << std::setw(3) << (borrow ? 1 : 0) << " diff    " << difference << std::endl;

			long shift = 0;
			if (borrow) {   // we have a negative value result
				difference = twos_complement(difference);
			}
			// find hidden bit
			for (int i = abits - 1; i >= 0 && difference[i]; i--) {
				shift++;
			}
			assert(shift >= -1);

			if (shift >= long(abits)) {            // we have actual 0
				difference.reset();
				result.set(false, 0, difference, true, false, false);
				return;
			}

			scale_of_result -= shift;
			const int hpos = abits - 1 - shift;         // position of the hidden bit
			difference <<= abits - hpos + 1;
			if (_trace_sub) std::cout << (borrow ? "sign -1" : "sign  1") << " scale " << std::setw(3) << scale_of_result << " result  " << difference << std::endl;
			result.set(borrow, scale_of_result, difference, false, false, false);
		}

		// multiply module
		template<size_t fbits, size_t mbits>
		void module_multiply(const value<fbits>& lhs, const value<fbits>& rhs, value<mbits>& result) {
			static constexpr size_t fhbits = fbits + 1;  // fraction + hidden bit
			if (_trace_mul) std::cout << "lhs  " << components(lhs) << std::endl << "rhs  " << components(rhs) << std::endl;

			if (lhs.isInfinite() || rhs.isInfinite()) {
				result.setToInfinite();
				return;
			}
			if (lhs.isZero() || rhs.isZero()) {
				result.setToZero();
				return;
			}

			bool new_sign = lhs.sign() ^ rhs.sign();
			int new_scale = lhs.scale() + rhs.scale();
			bitblock<mbits> result_fraction;

			if (fbits > 0) {
				// fractions are without hidden bit, get_fixed_point adds the hidden bit back in
				bitblock<fhbits> r1 = lhs.get_fixed_point();
				bitblock<fhbits> r2 = rhs.get_fixed_point();
				multiply_unsigned(r1, r2, result_fraction);

				if (_trace_mul) std::cout << "r1  " << r1 << std::endl << "r2  " << r2 << std::endl << "res " << result_fraction << std::endl;
				// check if the radix point needs to shift
				int shift = 2;
				if (result_fraction.test(mbits - 1)) {
					shift = 1;
					if (_trace_mul) std::cout << " shift " << shift << std::endl;
					new_scale += 1;
				}
				result_fraction <<= shift;    // shift hidden bit out
			}
			else {   // posit<3,0>, <4,1>, <5,2>, <6,3>, <7,4> etc are pure sign and scale
				// multiply the hidden bits together, i.e. 1*1: we know the answer a priori
			}
			if (_trace_mul) std::cout << "sign " << (new_sign ? "-1 " : " 1 ") << "scale " << new_scale << " fraction " << result_fraction << std::endl;

			result.set(new_sign, new_scale, result_fraction, false, false, false);
		}

		// divide module
		template<size_t fbits, size_t divbits>
		void module_divide(const value<fbits>& lhs, const value<fbits>& rhs, value<divbits>& result) {
			static constexpr size_t fhbits = fbits + 1;  // fraction + hidden bit
			if (_trace_div) std::cout << "lhs  " << components(lhs) << std::endl << "rhs  " << components(rhs) << std::endl;

			if (lhs.isInfinite() || rhs.isInfinite()) {
				result.setToInfinite();
				return;
			}
			if (lhs.isZero() || rhs.isInfinite()) {
				result.setToZero();
				return;
			}

			bool new_sign = lhs.sign() ^ rhs.sign();
			int new_scale = lhs.scale() - rhs.scale();
			bitblock<divbits> result_fraction;

			if (fbits > 0) {
				// fractions are without hidden bit, get_fixed_point adds the hidden bit back in
				bitblock<fhbits> r1 = lhs.get_fixed_point();
				bitblock<fhbits> r2 = rhs.get_fixed_point();
				divide_with_fraction(r1, r2, result_fraction);
				if (_trace_div) std::cout << "r1     " << r1 << std::endl << "r2     " << r2 << std::endl << "result " << result_fraction << std::endl << "scale  " << new_scale << std::endl;
				// check if the radix point needs to shift
				// radix point is at divbits - fhbits
				int msb = divbits - fhbits;
				int shift = fhbits;
				if (!result_fraction.test(msb)) {
					msb--; shift++;
					while (!result_fraction.test(msb)) { // search for the first 1
						msb--; shift++;
					}
				}
				result_fraction <<= shift;    // shift hidden bit out
				new_scale -= (shift - fhbits);
				if (_trace_div) std::cout << "shift  " << shift << std::endl << "result " << result_fraction << std::endl << "scale  " << new_scale << std::endl;;
			}
			else {   // posit<3,0>, <4,1>, <5,2>, <6,3>, <7,4> etc are pure sign and scale
					 // no need to multiply the hidden bits together, i.e. 1*1: we know the answer a priori
			}
			if (_trace_div) std::cout << "sign " << (new_sign ? "-1 " : " 1 ") << "scale " << new_scale << " fraction " << result_fraction << std::endl;

			result.set(new_sign, new_scale, result_fraction, false, false, false);
		}

	}  // namespace unum

}  // namespace sw


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 *

 *
 *
 * This program was contributed by Peter Munch. This work and the required
 * generalizations of the internal data structures of deal.II form part of the
 * project "Virtual Materials Design" funded by the Helmholtz Association of
 * German Research Centres.
 *
 *
 *
 *
 *
Introduction

 *
 *
Motivation

 *
 * Many freely available mesh-generation tools produce meshes that consist of
 * simplices (triangles in 2D; tetrahedra in 3D). The reason for this is that
 * generating such kind of meshes for complex geometries is simpler than the
 * generation of hex-only meshes. This tutorial shows how to work on such kind
 * of meshes with the experimental simplex features in deal.II. For this
 * purpose, we solve the Poisson problem from step-3 in 2D with a mesh only
 * consisting of triangles.
 *
 *
 *
Working on simplex meshes

 *
 * To be able to work on simplex meshes, one has to select appropriate finite
 * elements, quadrature rules, and mapping objects. In step-3, we used FE_Q,
 * QGauss, and (implicitly by not specifying a mapping) MappingQ1. The
 * equivalent classes for the first two classes in the context of simplices are
 * FE_SimplexP and QGaussSimplex, which we will utilize here. For mapping
 * purposes, we use the class MappingFE, which implements an isoparametric
 * mapping. We initialize it with an FE_SimplexP object so that it can be
 * applied on simplex meshes.
 *
 *
 *
Mesh generation

 *
 * In contrast to step-3, we do not use a function from the GridGenerator
 * namespace, but rather read an externally generated mesh. For this tutorial,
 * we have created the mesh (square with width and height of one) with Gmsh with
 * the following journal file "box_2D_tri.geo":
 *
 * @code
 * Rectangle(1) = {0, 0, 0, 1, 1, 0};
 * Mesh 2;
 * Save "box_2D_tri.msh";
 * @endcode
 *
 * The journal file can be processed by Gmsh generating the actual mesh with the
 * ending ".geo":
 *
 * @code
 * gmsh box_2D_tri.geo
 * @endcode
 *
 * We have included in the tutorial folder both the journal file and the mesh
 * file in the event that one does not have access to Gmsh.
 *
 * The mesh can be simply read by deal.II with methods provided by the GridIn
 * class, as shown below.
 *
 */

// @sect3{Include files}

// Include files, as used in step-3:
#include <deal.II/base/function.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_values.h>

#include <deal.II/grid/tria.h>

#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <fstream>
#include <iostream>

// Include files that contain appropriate quadrature rules, finite elements,
// and mapping objects for simplex meshes.
#include <deal.II/base/quadrature_lib.h>

#include <deal.II/fe/fe_simplex_p.h>
#include <deal.II/fe/mapping_fe.h>

// The following file contains the class GridIn, which allows us to read
// external meshes.
#include <deal.II/grid/grid_in.h>

using namespace dealii;

// @sect3{The Step3 class}
//
// This is the main class of the tutorial. Since it is very similar to the
// version from step-3, we will only point out and explain the relevant
// differences that allow to perform simulations on simplex meshes.

class Step3
{
public:
  Step3();

  void run();

private:
  void make_grid();
  void setup_system();
  void assemble_system();
  void solve();
  void output_results() const;

  Triangulation<2> triangulation;

  // Here, we select a mapping object, a finite element, and a quadrature rule
  // that are compatible with simplex meshes.
  const MappingFE<2>     mapping;
  const FE_SimplexP<2>   fe;
  const QGaussSimplex<2> quadrature_formula;

  DoFHandler<2> dof_handler;

  SparsityPattern      sparsity_pattern;
  SparseMatrix<double> system_matrix;

  Vector<double> solution;
  Vector<double> system_rhs;
};

// @sect4{Step3::Step3}
//
// In the constructor, we set the polynomial degree of the finite element and
// the number of quadrature points. Furthermore, we initialize the MappingFE
// object with a (linear) FE_SimplexP object so that it can work on simplex
// meshes.
Step3::Step3()
  : mapping(FE_SimplexP<2>(1))
  , fe(2)
  , quadrature_formula(3)
  , dof_handler(triangulation)
{}

// @sect4{Step3::make_grid}
//
// Read the external mesh file "box_2D_tri.msh" as in step-3.
void Step3::make_grid()
{
  GridIn<2>(triangulation).read("box_2D_tri.msh");

  std::cout << "Number of active cells: " << triangulation.n_active_cells()
            << std::endl;
}

// @sect4{Step3::setup_system}
//
// From here on, nothing has changed. Not even, the
// cell integrals have been changed depending on whether one operates on
// hypercube or simplex meshes. This is astonishing and is possible due to the
// design of the following two classes:
//  - DoFHandler: this class stores degrees of freedom in a flexible way and
//    allows simple access to them depending on the element type independent of
//    the cell type.
//  - FEValues: this class hides the details of finite element, quadrature rule,
//    and mapping (even if the implementations are inherently different)
//    behind a unified interface.
void Step3::setup_system()
{
  dof_handler.distribute_dofs(fe);
  std::cout << "Number of degrees of freedom: " << dof_handler.n_dofs()
            << std::endl;
  DynamicSparsityPattern dsp(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dsp);
  sparsity_pattern.copy_from(dsp);

  system_matrix.reinit(sparsity_pattern);

  solution.reinit(dof_handler.n_dofs());
  system_rhs.reinit(dof_handler.n_dofs());
}

// @sect4{Step3::assemble_system}
//
// Nothing has changed here.
void Step3::assemble_system()
{
  FEValues<2> fe_values(mapping,
                        fe,
                        quadrature_formula,
                        update_values | update_gradients | update_JxW_values);

  const unsigned int dofs_per_cell = fe.n_dofs_per_cell();
  FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  Vector<double>     cell_rhs(dofs_per_cell);
  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

  for (const auto &cell : dof_handler.active_cell_iterators())
    {
      fe_values.reinit(cell);

      cell_matrix = 0;
      cell_rhs    = 0;

      for (const unsigned int q_index : fe_values.quadrature_point_indices())
        {
          for (const unsigned int i : fe_values.dof_indices())
            for (const unsigned int j : fe_values.dof_indices())
              cell_matrix(i, j) +=
                (fe_values.shape_grad(i, q_index) * // grad phi_i(x_q)
                 fe_values.shape_grad(j, q_index) * // grad phi_j(x_q)
                 fe_values.JxW(q_index));           // dx

          for (const unsigned int i : fe_values.dof_indices())
            cell_rhs(i) += (fe_values.shape_value(i, q_index) * // phi_i(x_q)
                            1. *                                // f(x_q)
                            fe_values.JxW(q_index));            // dx
        }
      cell->get_dof_indices(local_dof_indices);

      for (const unsigned int i : fe_values.dof_indices())
        for (const unsigned int j : fe_values.dof_indices())
          system_matrix.add(local_dof_indices[i],
                            local_dof_indices[j],
                            cell_matrix(i, j));

      for (const unsigned int i : fe_values.dof_indices())
        system_rhs(local_dof_indices[i]) += cell_rhs(i);
    }

  std::map<types::global_dof_index, double> boundary_values;
  VectorTools::interpolate_boundary_values(
    mapping, dof_handler, 0, Functions::ZeroFunction<2>(), boundary_values);
  MatrixTools::apply_boundary_values(boundary_values,
                                     system_matrix,
                                     solution,
                                     system_rhs);
}

// @sect4{Step3::solve}
//
// Nothing has changed here.
void Step3::solve()
{
  SolverControl            solver_control(1000, 1e-12);
  SolverCG<Vector<double>> solver(solver_control);
  solver.solve(system_matrix, solution, system_rhs, PreconditionIdentity());
}

// @sect4{Step3::output_results}
//
// Nothing has changed here.
void Step3::output_results() const
{
  DataOut<2> data_out;

  DataOutBase::VtkFlags flags;
  flags.write_higher_order_cells = true;
  data_out.set_flags(flags);

  data_out.attach_dof_handler(dof_handler);
  data_out.add_data_vector(solution, "solution");
  data_out.build_patches(mapping, 2);
  std::ofstream output("solution.vtk");
  data_out.write_vtk(output);
}

// @sect4{Step3::run}
//
// Nothing has changed here.
void Step3::run()
{
  make_grid();
  setup_system();
  assemble_system();
  solve();
  output_results();
}

// @sect3{The main function}
//
// Nothing has changed here.
int main()
{
  deallog.depth_console(2);

  Step3 laplace_problem;
  laplace_problem.run();

  return 0;
}

/*
 *
Results

 *
 * The following figures show the mesh and the result obtained by executing this
 * program:
 *
 *

 *

 *
 *         @image html step_3_simplex_0.png
 *
 *
 *         @image html step_3_simplex_1.png
 *
 *

 *

 *
 * Not surprisingly, the result looks as expected.
 *
 *
 *
Possibilities for extensions

 *
 * In this tutorial, we presented how to use the deal.II simplex infrastructure
 * to solve a simple Poisson problem on a simplex mesh in 2D. In this scope, we
 * could only present a small section of the capabilities. In the following, we
 * point out further capabilities briefly.
 *
 *
 *
3D meshes and codim-1 meshes in 3D

 *
 * An extension to 3D is quite straightforward. Both FE_SimplexP and
 * QGaussSimplex are implemented in a dimensional-independent way so that simply
 * replacing everywhere dim=2 with dim=3 should work out of the box.
 *
 * Furthermore, embedding of a 2D mesh consisting of triangles in 3D space is
 * possible.
 *
 *
 *
Mixed meshes

 *
 * In step-3, we considered meshes only consisting of quadrilaterals. In this
 * tutorial, we took a look at the case that the mesh only consists of
 * triangles. In the general case (also known as mixed mesh), the mesh consists
 * of both cell types. In 3D, meshes might even consist of more cell types, like
 * wedges/prisms and pyramids. We consider such meshes in the tutorial
 * step-3mixed.
 *
 *
 *
Alternative finite elements, quadrature rules, and mapping objects

 *
 * In this tutorial, we used the most basic finite-element, quadrature-rule, and
 * mapping classes. However, more classes are compatible with simplices. The
 * following list gives an overview of these classes:
 * - finite elements: FE_SimplexP, FE_SimplexDGP, FE_SimplexP_Bubbles
 * - quadrature rules: QGaussSimplex, QWitherdenVincentSimplex, QDuffy
 * - mapping objects: MappingFE, MappingFEField
 *
 * It should be also pointed out that FESystems can also handle simplex finite
 * elements which is crucial to solve vector-valued problems, as needed, e.g.,
 * to solve elasticity and fluid problems (see also step-17).
 *
 *
 *
Alternative mesh generation approaches

 *
 * In this tutorial, we have created the mesh externally and read it with the
 * help of GridIn. Since we believe that the main motivation to work on simplex
 * meshes is that one has a complex geometry that can only be meshed with
 * an external tool with simplices, deal.II does not have too many functions in
 * the GridGenerator namespace, targeting simplex meshes. However, we would like
 * to point out the following functions:
 *  - GridGenerator::subdivided_hyper_cube_with_simplices() and
 *    GridGenerator::subdivided_hyper_rectangle_with_simplices(), which fill a
 *    hypercube and a hyperrectangle domain with simplices
 *  - GridGenerator::convert_hypercube_to_simplex_mesh(), which converts meshes
 *    consisting of hypercube cells to simplex meshes by replacing one
 *    quadrilateral with 4 triangles and one hexahedron with 24 tetrahedrons
 *
 *
 *
hp-adaptivity

 *
 * Here, we considered a mesh without refinements and with all cells assigned
 * the same type of element with the same polynomial degree. However, one is not
 * restricted to this. For further details on hp-methods, see step-27.
 *
 *
 *
Parallelization

 *
 * To parallelize the code, one needs to replace the Triangulation object either
 * with parallel::shared::Triangulation or
 * parallel::fullydistributed::Triangulation and make some minor adjustments, as
 * discussed in step-6.
 *
 *
 *
Face integrals and discontinuous Galerkin methods

 *
 * The classes FEFaceValues and FEInterfaceValues are also compatible with
 * simplex meshes.
 *
 *
 *
Matrix-free operator evaluation

 *
 * In this tutorial, we showed a matrix-based approach. However, one could also
 * rewrite the code using MatrixFree, FEEvaluation, and FEFaceEvaluation, which
 * are also compatible with simplex meshes.
 *
 */