#include <cstdlib>
#include <sstream>
#include <iostream>
#include <vector>
#include <limits>
#include <cmath>

#if HAVE_NEW_CXX
# include <unordered_map>
# include <unordered_set>
#else
# include <tr1/unordered_map>
# include <tr1/unordered_set>
namespace std { using std::tr1::unordered_map; using std::tr1::unordered_set; }
#endif

#include <signal.h>

#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "json_feature_map_lexer.h"
#include "fdict.h"
#include "feature_map.h"
#include "prob.h"
#include "filelib.h"

#include <adept.h>
#include <Eigen/Eigen>

using adept::adouble;

#define INPUT_DIM 20
template <typename F> using FVector = Eigen::Matrix<F,INPUT_DIM,1>;

using namespace std;
namespace po = boost::program_options;

volatile bool* requested_stop = NULL;

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("x,x", po::value<vector<string> >(), "Files containing training instance features")
        ("y,y", po::value<string>(), "File containing training instance responses (if unspecified, do prediction only)")
        ("tx", po::value<vector<string> >(), "Files containing training instance features")
        ("ty", po::value<string>(), "File containing training instance responses (if unspecified, do prediction only)")
        ("z,z", po::value<string>(), "Write learned weights to this file (optional)")
        ("l1",po::value<double>()->default_value(0.0), "l_1 regularization strength")
        ("l2",po::value<double>()->default_value(1e-10), "l_2 regularization strength")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

enum RegressionType { kLINEAR, kLOGISTIC };

struct TrainingInstance {
  FrozenFeatureMap x;
  union {
    unsigned label;  // for categorical & ordinal predictions
    float value;     // for continuous predictions
  } y;
};

struct ReaderHelper {
  explicit ReaderHelper(vector<TrainingInstance>* xyp,
                        bool h,
                        vector<string>* iids) : xy_pairs(xyp), lc(), flag(), has_labels(h), ids(iids), merge() {}
  unordered_map<string, unsigned> id2ind;
  FeatureMapStorage* fms;
  vector<TrainingInstance>* xy_pairs;
  int lc;
  bool flag;
  bool has_labels;
  vector<string>* ids;
  vector<bool> merged;
  bool merge;
};

void ReaderCB(const string& id,
              const std::pair<int,float>* begin,
              const std::pair<int,float>* end,
              void* extra) {
  ReaderHelper& rh = *reinterpret_cast<ReaderHelper*>(extra);
  ++rh.lc;
  if (rh.lc % 1000  == 0) { cerr << '.'; rh.flag = true; }
  if (rh.lc % 50000 == 0) { cerr << " [" << rh.lc << "]\n"; rh.flag = false; }
  if (rh.ids && !rh.merge) rh.ids->push_back(id);
  if (rh.has_labels) {
    const unordered_map<string, unsigned>::iterator it = rh.id2ind.find(id);
    if (it == rh.id2ind.end()) {
      cerr << "\nUnlabeled example in line " << rh.lc << " (key=" << id << ')' << endl;
      abort();
    } else {
      if (rh.merge) {
        rh.merged[it->second - 1] = true;
        const FrozenFeatureMap& prev = (*rh.xy_pairs)[it->second - 1].x;
        (*rh.xy_pairs)[it->second - 1].x = rh.fms->AddFeatureMap(begin,end,prev);
      } else {
        (*rh.xy_pairs)[it->second - 1].x = rh.fms->AddFeatureMap(begin,end);
      }
    }
  } else {
    TrainingInstance x_no_y;
    if (rh.merge) {
      if (!rh.ids) {
        cerr << "Missing IDs\n";
        abort();
      }
      unsigned& ind = rh.id2ind[id];
      rh.merged[ind - 1] = true;
      const FrozenFeatureMap& prev = (*rh.xy_pairs)[ind - 1].x;
      (*rh.xy_pairs)[ind - 1].x = rh.fms->AddFeatureMap(begin,end,prev);
    } else {
      x_no_y.x = rh.fms->AddFeatureMap(begin,end);
      unsigned& ind = rh.id2ind[id];
      assert(ind == 0);
      rh.xy_pairs->push_back(x_no_y);
      ind = rh.xy_pairs->size();
    }
  }
}

void ReadLabeledInstances(const vector<string>& ffeats,
                          const string& fresp,
                          const RegressionType resptype,
                          FeatureMapStorage* fms,
                          vector<TrainingInstance>* xy_pairs,
                          vector<string>* labels,
                          vector<string>* instance_ids = NULL) {
  bool flag = false;
  xy_pairs->clear();
  int lc = 0;
  ReaderHelper rh(xy_pairs, fresp.size() > 0, instance_ids);
  rh.merge = false;
  unordered_map<string, unsigned> label2id;
  if (fresp.size() == 0) {
    cerr << "No gold standard responses provided to learn from!" << endl;
  } else {
    cerr << "Reading responses from " << fresp << " ..." << endl;
    ReadFile fr(fresp);
    for (unsigned i = 0; i < labels->size(); ++i)
      label2id[(*labels)[i]] = i;
    istream& in = *fr.stream();
    string line;
    while(getline(in, line)) {
      ++lc;
      if (lc % 1000 == 0) { cerr << '.'; flag = true; }
      if (lc % 40000 == 0) { cerr << " [" << lc << "]\n"; flag = false; }
      if (line.size() == 0) continue;
      if (line[0] == '#') {
        if (line.size() > 1 && line[1] == '#') {
          if (lc != 1) {
            cerr << "[WARNING] Line " << lc << " appears to be label declaration ... ignoring\n";
          } else {
            istringstream is(line);
            string label;
            is >> label;
            while(is >> label) {
              unordered_map<string, unsigned>::iterator it = label2id.find(label);
              if (it == label2id.end()) {
                it = label2id.insert(make_pair(label, labels->size())).first;
                labels->push_back(label);
              }
            }
          }
        }
        continue;
      }
      unsigned p = 0;
      while (p < line.size() && line[p] != ' ' && line[p] != '\t') { ++p; }
      unsigned& ind = rh.id2ind[line.substr(0, p)];
      if (ind != 0) { cerr << "ID " << line.substr(0, p) << " duplicated in line " << lc << endl; abort(); }
      while (p < line.size() && (line[p] == ' ' || line[p] == '\t')) { ++p; }
      assert(p < line.size());
      xy_pairs->push_back(TrainingInstance());
      ind = xy_pairs->size();
      switch (resptype) {
        case kLINEAR:
          xy_pairs->back().y.value = strtof(&line[p], 0);
          break;
        case kLOGISTIC:
          {
            unordered_map<string, unsigned>::iterator it = label2id.find(line.substr(p));
            if (it == label2id.end()) {
              const string label = line.substr(p);
              it = label2id.insert(make_pair(label, labels->size())).first;
              labels->push_back(label);
            }
            xy_pairs->back().y.label = it->second;  // label id
          }
          break;
      }
    }
    if (flag) cerr << endl;
    if (resptype == kLOGISTIC) {
      cerr << "LABELS:";
      for (unsigned j = 0; j < labels->size(); ++j)
        cerr << " " << (*labels)[j];
      cerr << endl;
    }
  }
  FeatureMapStorage* pfms = NULL;
  FeatureMapStorage* tfms = NULL;
  for (unsigned i = 0; i < ffeats.size(); ++i) {
    if (i == ffeats.size() - 1) {
      pfms = tfms;
      rh.fms = fms;
    } else {
      delete pfms;
      pfms = tfms;
      tfms = new FeatureMapStorage;
      rh.fms = tfms;
    }
    if (pfms) {
      rh.merge = true;
      rh.merged.clear();
      rh.merged.resize(rh.xy_pairs->size(), false);
    }
    const string& ffeat = ffeats[i];
    cerr << "Reading features from " << ffeat << " ..." << endl;
    ReadFile ff(ffeat);
    JSONFeatureMapLexer::ReadRules(ff.stream(), ReaderCB, &rh);
    if (pfms) {
      for (unsigned j = 0; j < rh.xy_pairs->size(); ++j)
        if (!rh.merged[j])
          (*rh.xy_pairs)[j].x = rh.fms->AddFeatureMap(NULL, NULL, (*rh.xy_pairs)[j].x);
    }
    if (rh.flag) cerr << endl;
  }
  delete pfms;
}

void signal_callback_handler(int /* signum */) {
  if (!requested_stop || *requested_stop) {
    cerr << "\nReceived SIGINT again, quitting.\n";
    _exit(1);
  }
  cerr << "\nReceived SIGINT terminating optimization early.\n";
  *requested_stop = true;
}

template <typename F>
struct Model {
  Model(unsigned feats, unsigned labels) :
      input_reps(feats, FVector<F>::Zero()), output_reps(labels, FVector<F>::Zero()) {}

  // number of parameters
  size_t size() const {
    return input_reps.size() * input_reps[0].size() + output_reps.size() * output_reps[0].size();
  }

  void Randomize() {
    for (auto& iv: input_reps)
      iv = FVector<F>::Random() / 5.;
    for (auto& ov: output_reps)
      ov = FVector<F>::Random() / 2.;
  }

  template <typename T>
  void copyfrom(const Model<T>& o) {
    for (unsigned i = 0; i < input_reps.size(); ++i)
      for (unsigned j = 0; j < INPUT_DIM; ++j)
        input_reps[i](j,0) = o.input_reps[i](j,0);
    for (unsigned i = 0; i < output_reps.size(); ++i)
      for (unsigned j = 0; j < INPUT_DIM; ++j)
        output_reps[i](j,0) = o.output_reps[i](j,0);
  }

  void update(const Model<adouble>& g, Model<double>& h) {
    double eta = 0.1;
    for (unsigned i = 0; i < input_reps.size(); ++i)
      for (unsigned j = 0; j < INPUT_DIM; ++j) {
        double d = g.input_reps[i](j,0).get_gradient();
        if (d) {
          double s = h.input_reps[i](j,0) += d * d;
          input_reps[i](j,0) -= eta * d / sqrt(s);
        }
      }
    for (unsigned i = 0; i < output_reps.size(); ++i)
      for (unsigned j = 0; j < INPUT_DIM; ++j) {
        double d = g.output_reps[i](j,0).get_gradient();
        if (d) {
          double s = h.output_reps[i](j,0) += d * d;
          output_reps[i](j,0) -= eta * d / sqrt(s);
        }
      }
  }

  inline static void nonlinearity(FVector<F>& v) {
    for (unsigned i = 0; i < v.size(); ++i)
      v(i,0) = tanh(v(i,0));
  }

  F log_prob(const FrozenFeatureMap& x, unsigned y) const {
    FVector<F> h = FVector<F>::Zero();
    for (auto& it : x)
      h += input_reps[it.first] * it.second;
    nonlinearity(h);
    F z = 0;
    F res = 0;
    unsigned c = 0;
    for (auto& ov : output_reps) {
      F s = h.dot(ov);
      if (c == y) res = s;
      z += exp(s);
      ++c;
    }
    return res - log(z);
  }

  unsigned predict(const FrozenFeatureMap& x) const {
    FVector<F> h = FVector<F>::Zero();
    for (auto& it : x)
      h += input_reps[it.first] * it.second;
    nonlinearity(h);
    unsigned c = 0;
    F cur_best = 0.0;
    unsigned cur_best_i = output_reps.size();
    for (auto& ov : output_reps) {
      F s = h.dot(ov);
      if (s > cur_best || cur_best_i == output_reps.size()) {
        cur_best = s; cur_best_i = c;
      }
      ++c;
    }
    return cur_best_i;
  }

  vector<FVector<F>> input_reps;
  vector<FVector<F>> output_reps;
};

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  string line;
  double l1 = conf["l1"].as<double>();
  double l2 = conf["l2"].as<double>();
  if (l1 < 0.0) {
    cerr << "L1 strength must be >= 0\n";
    return 1;
  }
  if (l2 < 0.0) {
    cerr << "L2 strength must be >= 0\n";
    return 2;
  }
  bool stop = false;
  requested_stop = &stop;
  RegressionType resptype = kLOGISTIC;
  if (conf.count("linear")) {
    resptype = kLINEAR;
  }
  if (!(conf.count("x") && conf.count("y"))) {
    cerr << "You must specify both training_features (-x) and training_responses (-y)!\n";
    return 1;
  }
  vector<string> labels; // only populated for non-continuous models
  vector<TrainingInstance> training, test;
  FeatureMapStorage fms;
  vector<string> xfile = conf["x"].as<vector<string> >();
  string yfile = conf["y"].as<string>();
  ReadLabeledInstances(xfile, yfile, resptype, &fms, &training, &labels);
  if (conf.count("tx") && conf.count("ty")) {
    vector<string> xfile = conf["tx"].as<vector<string> >();
    string yfile = conf["ty"].as<string>();
    ReadLabeledInstances(xfile, yfile, resptype, &fms, &test, &labels);
  }

  adept::Stack s;
  cerr << "         Number of features: " << FD::NumFeats() << endl;
  cerr << "Number of training examples: " << training.size() << endl;
  const unsigned p = FD::NumFeats();

  Model<double> dmodel(p,labels.size());
  dmodel.Randomize();
  Model<double> hmodel(p,labels.size());
  Model<adouble> amodel(p,labels.size());
  cout.precision(15);
  ostream* out = &cout;
  cerr << "Model size: " << dmodel.size() << " parameters\n";
  // set up signal handler to catch SIGINT
  signal(SIGINT, signal_callback_handler);

  if (conf.count("linear")) {  // linear regression
    cerr << "Not implemented!\n";
    return 1;
  } else {                     // logistic regression
    for (unsigned iter = 0; !stop && iter < 100000; ++iter) {
      amodel.copyfrom(dmodel);
      s.new_recording();
      adouble llh = 0;
      double right = 0;
      for (auto& xy: training) {
        adouble lp = amodel.log_prob(xy.x, xy.y.label);
        unsigned pred_y = dmodel.predict(xy.x);
        if (pred_y == xy.y.label) right++;
        //cerr << xy.y.label << " ||| " << lp << endl;
        llh -= lp;
      }
      llh.set_gradient(1.0);
      s.compute_adjoint();
      double hright = 0.0;
      double hllh = 0;
      unsigned lc = 0;
      for (auto& xy: test) {
        ++lc;
        double lp = dmodel.log_prob(xy.x, xy.y.label);
        unsigned pred_y = dmodel.predict(xy.x);
        if (pred_y == xy.y.label) hright++;
        else if (lc < 100) {
          cerr << "line " << lc << " which is in " << labels[xy.y.label] << " was confused for " << labels[pred_y] << endl;
        }
        //cerr << xy.y.label << " ||| " << lp << endl;
        hllh -= lp;
      }
      double perp = exp(llh.value() / training.size());
      double perpho = exp(hllh / test.size());
      cerr << "i=" << iter << "\tPerplexity: " << perp << "\ttrain-acc: " << (right / training.size());
      if (test.size()) cerr << "\td-perp: " << perpho << "\ttd-acc: " << (hright / test.size());
      cerr << endl;
      dmodel.update(amodel, hmodel);
    }
  }
  for (unsigned i = 0; i < labels.size(); ++i) {
    cerr << labels[i] << " " << dmodel.output_reps[i].transpose() << endl;
  }
  if (out && out != &cout)
    delete out;

  return 0;
}


#include <fstream>
#include <iostream>
#include <algorithm>
#include <chrono>

#include <yaml-cpp/yaml.h>

// #include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>

// OMPL headers
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/planners/sst/SST.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>

#include "robots.h"
#include "robotStatePropagator.hpp"
#include "fclStateValidityChecker.hpp"

namespace ob = ompl::base;
namespace oc = ompl::control;
namespace og = ompl::geometric;

int main(int argc, char* argv[]) {
  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");
  std::string inputFile;
  std::string outputFile;
  std::string plannerDesc;
  int timelimit;
  float goalRegion;
  std::string robotType;
  desc.add_options()
    ("help", "produce help message")
    ("input,i", po::value<std::string>(&inputFile)->required(), "input file (yaml)")
    ("output,o", po::value<std::string>(&outputFile)->required(), "output file (yaml)")
    ("planner,p", po::value<std::string>(&plannerDesc)->default_value("rrt"), "Planner")
    ("timelimit", po::value<int>(&timelimit)->default_value(10), "Time limit for planner")
    ("goalregion", po::value<float>(&goalRegion)->default_value(0.1), "radius around goal to count success")
    ("robottype", po::value<std::string>(&robotType)->required(), "type to use for planning");

  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") != 0u) {
      std::cout << desc << "\n";
      return 0;
    }
  } catch (po::error& e) {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  // load problem description
  YAML::Node env = YAML::LoadFile(inputFile);

  std::vector<fcl::CollisionObjectf *> obstacles;
  for (const auto &obs : env["environment"]["obstacles"])
  {
    if (obs["type"].as<std::string>() == "box")
    {
      const auto &size = obs["size"];
      std::shared_ptr<fcl::CollisionGeometryf> geom;
      geom.reset(new fcl::Boxf(size[0].as<float>(), size[1].as<float>(), 1.0));
      const auto &center = obs["center"];
      auto co = new fcl::CollisionObjectf(geom);
      co->setTranslation(fcl::Vector3f(center[0].as<float>(), center[1].as<float>(), 0));
      co->computeAABB();
      obstacles.push_back(co);
    }
    else
    {
      throw std::runtime_error("Unknown obstacle type!");
    }
  }
  std::shared_ptr<fcl::BroadPhaseCollisionManagerf> bpcm_env(new fcl::DynamicAABBTreeCollisionManagerf());
  // std::shared_ptr<fcl::BroadPhaseCollisionManagerf> bpcm_env(new fcl::NaiveCollisionManagerf());
  bpcm_env->registerObjects(obstacles);
  bpcm_env->setup();

  const auto& robot_node = env["robots"][0];
  const auto &env_min = env["environment"]["min"];
  const auto &env_max = env["environment"]["max"];
  ob::RealVectorBounds position_bounds(env_min.size());
  for (size_t i = 0; i < env_min.size(); ++i) {
    position_bounds.setLow(i, env_min[i].as<double>());
    position_bounds.setHigh(i, env_max[i].as<double>());
  }
  std::shared_ptr<Robot> robot = create_robot(robotType, position_bounds);

  auto si = robot->getSpaceInformation();

  // set state validity checking for this space
  auto stateValidityChecker(std::make_shared<fclStateValidityChecker>(si, bpcm_env, robot));
  si->setStateValidityChecker(stateValidityChecker);

  // set the state propagator
  std::shared_ptr<oc::StatePropagator> statePropagator(new RobotStatePropagator(si, robot));
  si->setStatePropagator(statePropagator);

  si->setup();

  // create a problem instance
  auto pdef(std::make_shared<ob::ProblemDefinition>(si));

  // create and set a start state
  auto startState = si->allocState();
  std::vector<double> reals;
  for (const auto& v : robot_node["start"]) {
    reals.push_back(v.as<double>());
    if (reals.size() == si->getStateSpace()->getDimension()) break;
  }
  si->getStateSpace()->copyFromReals(startState, reals);
  pdef->addStartState(startState);
  si->freeState(startState);

  // set goal state
  auto goalState = si->allocState();
  reals.clear();
  for (const auto &v : robot_node["goal"]) {
    reals.push_back(v.as<double>());
    if (reals.size() == si->getStateSpace()->getDimension()) break;
  }
  si->getStateSpace()->copyFromReals(goalState, reals);
  pdef->setGoalState(goalState, goalRegion);
  si->freeState(goalState);

  // create a planner for the defined space
  std::shared_ptr<ob::Planner> planner;
  if (plannerDesc == "rrt*") {
    planner.reset(new og::RRTstar(si));
  } else if (plannerDesc == "sst") {
    planner.reset(new og::SST(si));
  }
  // rrt->setGoalBias(params["goalBias"].as<float>());
  // auto planner(rrt);

  // auto start = std::chrono::steady_clock::now();
  bool has_solution = false;
  std::chrono::steady_clock::time_point previous_solution;
  pdef->setIntermediateSolutionCallback(
      [&previous_solution, &has_solution](const ob::Planner *, const std::vector<const ob::State *> &, const ob::Cost cost)
      {
        // double t = std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count();
        // double dt = std::chrono::duration_cast<std::chrono::milliseconds>(now - previous_solution).count();
        std::cout << "Intermediate solution! " << cost.value() << std::endl;
        has_solution = true;
        // last_solution_in_sec = dt / 1000.0f;
        previous_solution = std::chrono::steady_clock::now();
      });

  // set the problem we are trying to solve for the planner
  planner->setProblemDefinition(pdef);

  // perform setup steps for the planner
  // (this will set the optimization objective)
  planner->setup();

  // set a really high cost threshold, so that planner stops after first solution was found
  // pdef->getOptimizationObjective()->setCostThreshold(ob::Cost(1e6));

  // print the settings for this space
  si->printSettings(std::cout);

  // print the problem settings
  pdef->print(std::cout);

  // attempt to solve the problem within timelimit
  ob::PlannerStatus solved;

  // solved = planner->ob::Planner::solve(timelimit);
  // terminate if no better solution is found within the timelimit
  solved = planner->solve(
    ob::PlannerTerminationCondition([&previous_solution, &has_solution]
    {
      if (!has_solution) {
        return false;
      }
      auto now = std::chrono::steady_clock::now();
      double dt = std::chrono::duration_cast<std::chrono::milliseconds>(now - previous_solution).count() / 1000.0f;
      return dt > 1.0;
    }));
  std::cout << solved << std::endl;

  if (solved)
  {
    // get the goal representation from the problem definition (not the same as the goal state)
    // and inquire about the found path
    // ob::PathPtr path = pdef->getSolutionPath();
    std::cout << "Found solution:" << std::endl;

    // print the path to screen
    // path->print(std::cout);
  }
  else {
    std::cout << "No solution found" << std::endl;
  }

  // std::cout << solved << std::endl;
  auto path = pdef->getSolutionPath()->as<og::PathGeometric>();

  path->interpolate(); // normalize to a single control step
  std::cout << path->getStateCount() << std::endl;

  std::ofstream out(outputFile);
  out << "result:" << std::endl;
  out << "  - pathlength: " << path->length() << std::endl;
  out << "    states:" << std::endl;
  for (size_t i = 0; i < path->getStateCount(); ++i) {
    const auto state = path->getState(i);

    std::vector<double> reals;
    si->getStateSpace()->copyToReals(reals, state);
    out << "      - [";
    for (size_t i = 0; i < reals.size(); ++i) {
      out << reals[i];
      if (i < reals.size() - 1) {
        out << ",";
      }
    }
    out << "]" << std::endl;
  }

  return 0;
}


#include "DayOfWeek.hpp"

#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/date_defs.hpp>
#include <Util/Except.hpp>

using namespace boost::gregorian;
using namespace EnjoLib;

DayOfWeek::~DayOfWeek(){}
DayOfWeek::DayOfWeek(int year, int month, int day)
: m_year(year), m_month(month), m_day(day)
{
}

DayOfWeek::Day DayOfWeek::GetDay() const
{
    date dCurr(m_year, m_month, m_day);
    auto dowCurr = dCurr.day_of_week();

    switch (dowCurr)
    {
        case boost::date_time::weekdays::Sunday:
            return Sunday;
        case boost::date_time::weekdays::Monday:
            return Monday;
        case boost::date_time::weekdays::Tuesday:
            return Tuesday;
        case boost::date_time::weekdays::Wednesday:
            return Wednesday;
        case boost::date_time::weekdays::Thursday:
            return Thursday;
        case boost::date_time::weekdays::Friday:
            return Friday;
        case boost::date_time::weekdays::Saturday:
            return Saturday;
    }
    throw EnjoLib::ExceptLogicError("DayOfWeek::GetDay(): Invalid day provided");
}

int DayOfWeek::GetWeekNumber() const
{
    date dCurr(m_year, m_month, m_day);
    return dCurr.week_number();
}

int DayOfWeek::GetDayInt() const
{
    auto dowCurr = GetDay();
    switch (dowCurr)
    {
    case Sunday:
        return 0;
    case Monday:
        return 1;
    case Tuesday:
        return 2;
    case Wednesday:
        return 3;
    case Thursday:
        return 4;
    case Friday:
        return 5;
    case Saturday:
        return 6;
    }
    return -1;
}


#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
#include <boost/timer.hpp>

using namespace std;

//CNRGbasisarray CutStates(CNRGarray* Ain, int Ncutoff){

void CutStates(CNRGarray &Ain, CNRGbasisarray &Acut, int Ncutoff){

  //
  // Function that reduces the number of states in Ain
  //

  // No longer like this
  //CNRGbasisarray Acut(*Ain);
  Acut.ClearAll();
  Acut.SyncNRGarray(Ain);
  Acut.SetSCFfromdEn();
  Acut.BlockBegEndBC=Ain.BlockBegEnd;
  Acut.SetBlockBegEndEigVec();

  if (Ncutoff>=Ain.Nstates()) return; // Does nothing

  // Get Ecutoff

  double Ecutoff=Ain.Ecut(Ncutoff);  // Equivalent to (*Ain).Ecut(Ncutoff)

  cout << "Ecutoff = " << Ecutoff << endl;

  Acut.SetKept(Ncutoff);
  //cout << "No blocks before cut: " << Acut.NumBlocks() << endl;
  //
  // Identify which blocks are eliminated
  //

  cout << "Removing blocks..." << endl;

  for (int ibl=0;ibl<Acut.NumBlocks();ibl++){
    double Eblmin=Acut.GetBlockEmin(ibl);
    if (Eblmin>Ecutoff){cout << "ibl = " << ibl << " : Eblmin>Ecut" <<endl;}
    if (Eblmin>Ecutoff){ // not enough. Make sure you're not slicing a block
    //if ( dGTPrec(Eblmin,Ecutoff,0.01/fabs(Ecutoff)) ){
      cout << " Removing block ibl= " << ibl
	   << " Eblmin = " << Eblmin
	   << endl;
      Acut.RemoveBlock(ibl);
      ibl--; // Needs this!!
    }
  }

  cout << "...done removing blocks." << endl;

  //
  // Remove all remaining states
  //

//   Acut.PrintEn();

  // Check time
  boost::timer t;

  t.restart();

  cout << "Removing states..." << endl;

  for (int ibl=0;ibl<Acut.NumBlocks();ibl++){
    double Eblmax=Acut.GetBlockEmax(ibl);
    double Eblmin=Acut.GetBlockEmin(ibl);

    if (Eblmax>Ecutoff){ // Make sure you're not slicing a block
    //if ( dGTPrec(Eblmax,Ecutoff,0.01/fabs(Ecutoff)) ){

//       cout << " Block " << ibl << " of " << Acut.NumBlocks() <<
// 	" has Emax = " << Eblmax << endl;
//       cout << " and Emin = " << Eblmin << " <= Ecut = " << Ecutoff << endl;
//       cout << " Block begins at " << Acut.GetBlockLimit(ibl,0) <<
// 	" ends at " << Acut.GetBlockLimit(ibl,1) << endl;
//       cout << " First energy: " << Acut.dEn[Acut.GetBlockLimit(ibl,0)] << endl;

      int ipos=Acut.GetBlockEcutPos(ibl,Ecutoff);
      // Assumes states are energy-ordered within block
	Acut.RemoveStatesFromBlock(ibl,ipos,Acut.GetBlockLimit(ibl,1));
    }
    //End if Eblmax > Ecutoff

  }
  // end loop in remaining blocks

  cout << "...done removing states. Total time =" << t.elapsed() << endl;

}

/////////////////////////////////
/////////////////////////////////
/////     OLD version       /////
/////////////////////////////////
/////////////////////////////////

CNRGbasisarray CutStates(CNRGarray* pAin, int Ncutoff){

  //
  // Function that reduces the number of states in Ain
  //

  // No longer like this
  CNRGbasisarray Acut(*pAin);
  Acut.SetSCFfromdEn();
  Acut.BlockBegEndBC=pAin->BlockBegEnd;
  Acut.SetBlockBegEndEigVec();

  if (Ncutoff>=pAin->Nstates()) return(Acut); // Does nothing

  // Get Ecutoff

  double Ecutoff=pAin->Ecut(Ncutoff);  // Equivalent to (*Ain).Ecut(Ncutoff)

  cout << "Ecutoff = " << Ecutoff << endl;
  //cout << "No blocks before cut: " << Acut.NumBlocks() << endl;
  //
  // Identify which blocks are eliminated
  //

  cout << "Removing blocks..." << endl;

  for (int ibl=0;ibl<Acut.NumBlocks();ibl++)
    {
      double Eblmin=Acut.GetBlockEmin(ibl);
      if (Eblmin>Ecutoff)
	{
	 Acut.RemoveBlock(ibl);
	 ibl--; // Needs this!!
	}
    }

  cout << "...done removing blocks." << endl;

  //
  // Remove all remaining states
  //

//   Acut.PrintEn();

  // Check time
  boost::timer t;

  t.restart();

  cout << "Removing states..." << endl;

  for (int ibl=0;ibl<Acut.NumBlocks();ibl++)
    {
      double Eblmax=Acut.GetBlockEmax(ibl);
      double Eblmin=Acut.GetBlockEmin(ibl);

//       cout << " Block " << ibl << " of " << Acut.NumBlocks() <<
// 	" has Emax = " << Eblmax << endl;
//       cout << " and Emin = " << Eblmin << " <= Ecut = " << Ecutoff << endl;
//       cout << " Block begins at " << Acut.GetBlockLimit(ibl,0) <<
// 	" ends at " << Acut.GetBlockLimit(ibl,1) << endl;
//       cout << " First energy: " << Acut.dEn[Acut.GetBlockLimit(ibl,0)] << endl;

      if (Eblmax>Ecutoff)
	{

	  int ipos=Acut.GetBlockEcutPos(ibl,Ecutoff);
	  // Assumes states are energy-ordered within block
	  Acut.RemoveStatesFromBlock(ibl,ipos,Acut.GetBlockLimit(ibl,1));

//  	  for (int ist=Acut.GetBlockLimit(ibl,0);ist<=Acut.GetBlockLimit(ibl,1);ist++)
//  	    {
//  	      if (Acut.dEn[ist]>Ecutoff)
//  		{
//  		  cout << " Should remove ist= " << ist << " > ipos = " << ipos << " E = " << Acut.dEn[ist] <<endl;
//  		}
// 	      // end Ei>Ecutoff
//  	    }
// 	  // for inside block

	}
      //End if Eblmax > Ecutoff

    }

    cout << "...done removing states. Total time =" << t.elapsed() << endl;

  // Updates StCameFrom

  return(Acut);

}

/////////////////////////////////


//==================================================================================================
/*!
  @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_RSHL_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_RSHL_HPP_INCLUDED

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

 /*!

    @ingroup group-bitwise
    Function object implementing rshl capabilities

    Returns the first entry shifted left or right
    by the absolute value of the second entry,
    according to its sign.

    @par semantic:
    For any given value @c x of type @c T, @c n of type @c I:

    @code
    T r = rshl(x, n);
    @endcode

    @see  rshr, shr, shl

  **/
  Value rshl(Value const & x, IntegerValue const &n);
} }
#endif

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

#endif


#include <boost/units/systems/si/impedance.hpp>


/*
 * GridTools
 *
 * Copyright (c) 2014-2019, ETH Zurich
 * All rights reserved.
 *
 * Please, refer to the LICENSE file in the root directory.
 * SPDX-License-Identifier: BSD-3-Clause
 */
#pragma once

#include "../../common/array.hpp"
#include "../../common/boollist.hpp"
#include "../GCL.hpp"
#include <boost/algorithm/string.hpp>
#include <boost/type_traits/integral_constant.hpp>
#include <cmath>
#include <iostream>
#include <string>

// This file needs to be changed

namespace gridtools {

#ifdef GCL_MPI
    /** \class MPI_3D_process_grid_t
     * Class that provides a representation of a 3D process grid given an MPI CART
     * It requires the MPI CART to be defined before the grid is created
     * \tparam CYCLIC is a template argument matching \ref boollist_concept to specify periodicities
     * \n
     * This is a process grid matching the \ref proc_grid_concept concept
     */
    template <int Ndims>
    struct MPI_3D_process_grid_t {

        /** number of dimensions
         */
        static const int ndims = Ndims;

        typedef boost::true_type has_communicator;
        typedef gridtools::boollist<ndims> period_type;

      private:
        MPI_Comm m_communicator; // Communicator that is associated with the MPI CART!
        period_type m_cyclic;
        int m_nprocs;
        gridtools::array<int, ndims> m_dimensions;
        gridtools::array<int, ndims> m_coordinates;

      public:
        MPI_3D_process_grid_t(MPI_3D_process_grid_t const &other) : m_cyclic(other.cyclic()), m_nprocs(other.m_nprocs) {

            MPI_Comm_dup(other.m_communicator, &m_communicator);

            for (uint_t i = 0; i < ndims; ++i) {
                m_dimensions[i] = other.m_dimensions[i];
                m_coordinates[i] = other.m_coordinates[i];
            }
        }

        /** Constructor that takes an MPI CART communicator, already configured, and use it to set up the process grid.
            \param c Object containing information about periodicities as defined in \ref boollist_concept
            \param comm MPI Communicator describing the MPI 3D computing grid
        */
        MPI_3D_process_grid_t(period_type const &c, MPI_Comm const &comm)
            : m_cyclic(c), m_nprocs(0), m_dimensions(), m_coordinates() {

            MPI_Comm_dup(comm, &m_communicator);

            int period[ndims];
            MPI_Cart_get(comm, ndims, &m_dimensions[0], period, &m_coordinates[0]);
            MPI_Comm_size(comm, &m_nprocs);
        }

        /** Constructor that takes an MPI CART communicator, already configured, and use it to set up the process grid.
            \param c Object containing information about periodicities as defined in \ref boollist_concept
            \param comm MPI Communicator describing the MPI 3D computing grid
            \param dims Array of dimensions of the processor grid
        */
        template <typename Array>
        MPI_3D_process_grid_t(period_type const &c, MPI_Comm const &comm, Array const &dims)
            : m_communicator(), m_cyclic(c), m_nprocs(0), m_dimensions(dims), m_coordinates() {
            MPI_Comm_size(comm, &m_nprocs);
            MPI_Dims_create(m_nprocs, dims.size(), &m_dimensions[0]);
            int period[3] = {1, 1, 1};
            MPI_Cart_create(comm, 3, &m_dimensions[0], period, false, &m_communicator);
            MPI_Cart_get(m_communicator, ndims, &m_dimensions[0], period /*does not really care*/, &m_coordinates[0]);
        }

        ~MPI_3D_process_grid_t() { MPI_Comm_free(&m_communicator); }

        /**
           Returns communicator
        */
        MPI_Comm communicator() const { return m_communicator; }

        /** Returns in t_R and t_C the lenght of the dimensions of the process grid AS PRESCRIBED BY THE CONCEPT
            \param[out] t_R Number of elements in first dimension
            \param[out] t_C Number of elements in second dimension
            \param[out] t_S Number of elements in third dimension
        */
        void dims(int &t_R, int &t_C, int &t_S) const {
            GT_STATIC_ASSERT(ndims == 3, "this interface supposes ndims=3");
            t_R = m_dimensions[0];
            t_C = m_dimensions[1];
            t_S = m_dimensions[2];
        }

        /** Returns the dimensions in an array of dimensions (at least of size 3)
            \tparam The array type
            \param array The array where to put the values
        */
        template <class Array>
        void fill_dims(Array &array) const {
            GT_STATIC_ASSERT(ndims == 3, "this interface supposes ndims=3");
            array[0] = m_dimensions[0];
            array[1] = m_dimensions[1];
            array[2] = m_dimensions[2];
        }

        void dims(int &t_R, int &t_C) const {
            GT_STATIC_ASSERT(ndims == 2, "this interface supposes ndims=2");
            t_R = m_dimensions[0];
            t_C = m_dimensions[1];
        }

        /** Returns the number of processors of the processor grid

            \return Number of processors
        */
        uint_t size() const {
            uint_t ret = m_dimensions[0];
            for (uint_t i = 1; i < ndims; ++i)
                ret *= m_dimensions[i];
            return ret;
        }

        /** Returns in t_R and t_C the coordinates ot the caller process in the grid AS PRESCRIBED BY THE CONCEPT
            \param[out] t_R Coordinate in first dimension
            \param[out] t_C Coordinate in second dimension
            \param[out] t_S Coordinate in third dimension
        */
        void coords(int &t_R, int &t_C, int &t_S) const {
            GT_STATIC_ASSERT(ndims == 3, "this interface supposes ndims=3");
            t_R = m_coordinates[0];
            t_C = m_coordinates[1];
            t_S = m_coordinates[2];
        }

        void coords(int &t_R, int &t_C) const {
            GT_STATIC_ASSERT(ndims == 2, "this interface supposes ndims=2");
            t_R = m_coordinates[0];
            t_C = m_coordinates[1];
        }

        /** Returns the process ID of the process with relative coordinates (I,J) with respect to the caller process AS
           PRESCRIBED BY THE CONCEPT
            \tparam I Relative coordinate in the first dimension
            \tparam J Relative coordinate in the second dimension
            \tparam K Relative coordinate in the third dimension
            \return The process ID of the required process
        */
        template <int I, int J, int K>
        int proc() const {
            // int coords[3]={I,J,K};
            return proc(I, J, K);
        }

        int pid() const {
            int rank;
            MPI_Comm_rank(m_communicator, &rank);
            return rank;
        }

        /** Returns the process ID of the process with relative coordinates (I,J) with respect to the caller process AS
           PRESCRIBED BY THE CONCEPT
            \param[in] I Relative coordinate in the first dimension
            \param[in] J Relative coordinate in the second dimension
            \param[in] K Relative coordinate in the third dimension
            \return The process ID of the required process
        */
        int proc(int I, int J, int K) const {
            int _coords[3];

            if (m_cyclic.value(0))
                _coords[0] = (m_coordinates[0] + I) % m_dimensions[0];
            else {
                _coords[0] = m_coordinates[0] + I;
                if (_coords[0] < 0 || _coords[0] >= m_dimensions[0])
                    return -1;
            }

            if (m_cyclic.value(1))
                _coords[1] = (m_coordinates[1] + J) % m_dimensions[1];
            else {
                _coords[1] = m_coordinates[1] + J;
                if (_coords[1] < 0 || _coords[1] >= m_dimensions[1])
                    return -1;
            }

            if (m_cyclic.value(2))
                _coords[2] = (m_coordinates[2] + K) % m_dimensions[2];
            else {
                _coords[2] = m_coordinates[2] + K;
                if (_coords[2] < 0 || _coords[2] >= m_dimensions[2])
                    return -1;
            }

            int pid = 0;
            MPI_Comm_rank(MPI_COMM_WORLD, &pid);
            int res;
            MPI_Cart_rank(m_communicator, _coords, &res);
            return res;
        }

        GT_FUNCTION
        gridtools::array<int, ndims> const &coordinates() const { return m_coordinates; }

        GT_FUNCTION
        gridtools::array<int, ndims> const &dimensions() const { return m_dimensions; }

        /** Returns the process ID of the process with absolute coordinates specified by the input gridtools::array of
           coordinates
            \param[in] crds gridtools::aray of coordinates of the processor of which the ID is needed

            \return The process ID of the required process
        */
        int abs_proc(gridtools::array<int, ndims> const &crds) const {
            return proc(crds[0] - m_coordinates[0], crds[1] - m_coordinates[1], crds[2] - m_coordinates[2]);
        }

        int ntasks() { return m_nprocs; }

        bool periodic(int index) const {
            assert(index < ndims);
            return m_cyclic.value(index);
        }

        array<bool, ndims> periodic() const {
            GT_STATIC_ASSERT(period_type::m_size == ndims, "Dimensions not matching");
            return m_cyclic.value();
        }

        period_type const &cyclic() const { return m_cyclic; }

        int const &coordinates(uint_t const &i) const { return m_coordinates[i]; }
        int const &dimensions(uint_t const &i) const { return m_dimensions[i]; }
    };

#endif

} // namespace gridtools


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy_n at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/algorithm/copy_n.hpp>
#include <numeric>
#include <vector>
#include <simd_test.hpp>

using namespace boost::simd;
using namespace boost::alignment;

STF_CASE_TPL( "Check unary simd::copy_n", STF_NUMERIC_TYPES )
{
  static const int N = pack<T>::static_size;

  std::vector<T> values(2*N+1), ref(2*N+3), out(2*N+3);
  std::iota(values.begin(), values.end(),T(1));

  std::copy(values.begin(), values.begin()+N, ref.begin());

  // verify we stopped where we should
  STF_EQUAL ( (boost::simd::copy_n(values.data(), N, out.data()))
            , out.data()+N
            );

  // verify values
  STF_EQUAL( out, ref );
}


#include "framework/builder/framework_builder.h"

#include <deal.II/base/conditional_ostream.h>
#include <deal.II/base/mpi.h>
#include <sstream>

#include "utility/reporter/mpi.h"
#include "utility/reporter/colors.h"

// Convergence classes
#include "convergence/final_checker_or_n.h"
#include "convergence/moments/single_moment_checker_l1_norm.h"
#include "convergence/parameters/single_parameter_checker.h"
#include "convergence/reporter/mpi.h"

// Domain classes
#include "domain/definition.h"
#include "domain/finite_element/finite_element_gaussian.h"
#include "domain/mesh/mesh_cartesian.h"

// Formulation classes
#include "formulation/angular/self_adjoint_angular_flux.h"
#include "formulation/scalar/diffusion.h"
#include "formulation/stamper.h"
#include "formulation/updater/saaf_updater.h"
#include "formulation/updater/diffusion_updater.h"

// Framework class
#include "framework/framework.h"

// KEffective Updater Classes
#include "calculator/cell/total_aggregated_fission_source.h"
#include "calculator/cell/integrated_fission_source.h"
#include "eigenvalue/k_effective/updater_via_fission_source.h"

// Material classes
#include "material/material_protobuf.h"

// Solver classes
#include "solver/group/single_group_solver.h"
#include "solver/gmres.h"

// Iteration classes
#include "iteration/initializer/initialize_fixed_terms_once.h"
#include "iteration/group/group_source_iteration.h"
#include "iteration/outer/outer_power_iteration.h"

// Quadrature classes & factories
#include "quadrature/quadrature_generator_i.h"
#include "quadrature/factory/quadrature_factories.h"
#include "quadrature/utility/quadrature_utilities.h"

// Results class
#include "results/output_dealii_vtu.h"

// System classes
#include "system/system.h"
#include "system/solution/mpi_group_angular_solution.h"
#include "system/system_functions.h"

namespace bart {

namespace framework {

namespace builder {

template<int dim>
auto FrameworkBuilder<dim>::BuildFramework(std::string name,
                                           ParametersType& prm)
-> std::unique_ptr<FrameworkType> {
  // Framework parameters
  int n_angles = 1; // Set to default value of 1 for scalar solve
  const int n_groups = prm.NEnergyGroups();

  *reporter_ptr_ << "Building Framework: " << Color::Green << name <<
                 Color::Reset << "\n";

  auto finite_element_ptr = Shared(BuildFiniteElement(prm));
  auto cross_sections_ptr = Shared(BuildCrossSections(prm));

  auto domain_ptr = Shared(BuildDomain(prm, finite_element_ptr,
                                       ReadMappingFile(prm.MaterialMapFilename())));
  *reporter_ptr_ << "\tSetting up domain\n";
  domain_ptr->SetUpMesh().SetUpDOF();

  std::shared_ptr<QuadratureSetType> quadrature_set_ptr = nullptr;
  UpdaterPointers updater_pointers;
  std::unique_ptr<MomentCalculatorType> moment_calculator_ptr = nullptr;

  if (prm.TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {
    quadrature_set_ptr = BuildQuadratureSet(prm);
    n_angles = quadrature_set_ptr->size();
    auto stamper_ptr = BuildStamper(domain_ptr);
    auto saaf_formulation_ptr = BuildSAAFFormulation(finite_element_ptr,
                                                     cross_sections_ptr,
                                                     quadrature_set_ptr);
    saaf_formulation_ptr->Initialize(domain_ptr->Cells().at(0));
    updater_pointers = BuildUpdaterPointers(
        std::move(saaf_formulation_ptr),
        std::move(stamper_ptr),
        quadrature_set_ptr);
    moment_calculator_ptr = std::move(BuildMomentCalculator(quadrature_set_ptr));

  } else if (prm.TransportModel() == problem::EquationType::kDiffusion) {
    auto diffusion_formulation_ptr = BuildDiffusionFormulation(
        finite_element_ptr,
        cross_sections_ptr);
    diffusion_formulation_ptr->Precalculate(domain_ptr->Cells().at(0));
    auto stamper_ptr = BuildStamper(domain_ptr);
    updater_pointers = BuildUpdaterPointers(
        std::move(diffusion_formulation_ptr),
        std::move(stamper_ptr));
    moment_calculator_ptr = std::move(BuildMomentCalculator());
  }

  auto initializer_ptr = BuildInitializer(
      updater_pointers.fixed_updater_ptr, n_groups, n_angles);
  auto convergence_reporter_ptr = Shared(BuildConvergenceReporter());
  auto group_solution_ptr = Shared(BuildGroupSolution(n_angles));
  system::SetUpMPIAngularSolution(*group_solution_ptr, *domain_ptr);

  auto iterative_group_solver_ptr = BuildGroupSolveIteration(
      BuildSingleGroupSolver(),
      BuildMomentConvergenceChecker(1e-10, 100),
      std::move(moment_calculator_ptr),
      group_solution_ptr,
      updater_pointers.scattering_source_updater_ptr,
      convergence_reporter_ptr);

  auto k_effective_updater = BuildKEffectiveUpdater(finite_element_ptr,
                                                    cross_sections_ptr,
                                                    domain_ptr);

  auto outer_iteration_ptr = BuildOuterIteration(
      std::move(iterative_group_solver_ptr),
      BuildParameterConvergenceChecker(1e-6, 100),
      std::move(k_effective_updater),
      updater_pointers.fission_source_updater_ptr,
      convergence_reporter_ptr);

  auto system_ptr = BuildSystem(n_groups, n_angles, *domain_ptr,
                                group_solution_ptr->solutions().at(0).size());

  auto results_output_ptr =
      std::make_unique<results::OutputDealiiVtu<dim>>(domain_ptr);

  Validate();

  return std::make_unique<framework::Framework>(
      std::move(system_ptr),
      std::move(initializer_ptr),
      std::move(outer_iteration_ptr),
      std::move(results_output_ptr));
}

template<int dim>
auto FrameworkBuilder<dim>::BuildConvergenceReporter()
-> std::unique_ptr<ReporterType> {
  ReportBuildingComponant("ConvergenceReporter");
  using Reporter = bart::convergence::reporter::MpiNoisy;

  std::unique_ptr<ReporterType> return_ptr = nullptr;

  int this_process = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
  auto pout_ptr = std::make_unique<dealii::ConditionalOStream>(std::cout, this_process == 0);
  return_ptr = std::make_unique<Reporter>(std::move(pout_ptr));
  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildCrossSections(
    const problem::ParametersI& problem_parameters)
    -> std::unique_ptr<CrossSectionType> {
  ReportBuildingComponant("Cross-sections");
  std::unique_ptr<CrossSectionType> return_ptr = nullptr;
  // Default implementation using protocol buffers
  try {
    MaterialProtobuf materials(problem_parameters.MaterialFilenames(),
                               problem_parameters.IsEigenvalueProblem(),
                               problem_parameters.DoNDA(),
                               problem_parameters.NEnergyGroups(),
                               problem_parameters.NumberOfMaterials());
    return_ptr = std::move(std::make_unique<CrossSectionType>(materials));
    ReportBuildSuccess("(default) Cross-sections using protobuf");
  } catch (...) {
    ReportBuildError("(default) Cross-sections using protobuf");
    throw;
  }
  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildDiffusionFormulation(
    const std::shared_ptr<FiniteElementType>& finite_element_ptr,
    const std::shared_ptr<data::CrossSections>& cross_sections_ptr,
    const formulation::DiffusionFormulationImpl implementation)
-> std::unique_ptr<DiffusionFormulationType> {
  ReportBuildingComponant("Diffusion formulation");
  std::unique_ptr<DiffusionFormulationType> return_ptr = nullptr;

  if (implementation == formulation::DiffusionFormulationImpl::kDefault) {
    using ReturnType = formulation::scalar::Diffusion<dim>;
    return_ptr = std::move(std::make_unique<ReturnType>(
        finite_element_ptr, cross_sections_ptr));
  }
  ReportBuildSuccess(return_ptr->description());

  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildDomain(
    ParametersType problem_parameters,
    const std::shared_ptr<FiniteElementType>& finite_element_ptr,
    std::string material_mapping)
-> std::unique_ptr<DomainType>{
  std::unique_ptr<DomainType> return_ptr = nullptr;

  ReportBuildingComponant("Mesh");
  auto mesh_ptr = std::make_unique<domain::mesh::MeshCartesian<dim>>(
      problem_parameters.SpatialMax(),
      problem_parameters.NCells(),
      material_mapping);
  ReportBuildSuccess(mesh_ptr->description());

  ReportBuildingComponant("Domain");
  return_ptr = std::move(std::make_unique<domain::Definition<dim>>(
      std::move(mesh_ptr),
      finite_element_ptr));
  ReportBuildSuccess(return_ptr->description());
  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildFiniteElement(ParametersType problem_parameters)
-> std::unique_ptr<FiniteElementType>{
  std::unique_ptr<FiniteElementType> return_ptr = nullptr;

  using FiniteElementGaussianType = domain::finite_element::FiniteElementGaussian<dim>;

  ReportBuildingComponant("Cell finite element basis");

  try {
    return_ptr = std::move(std::make_unique<FiniteElementGaussianType>(
        problem::DiscretizationType::kContinuousFEM,
        problem_parameters.FEPolynomialDegree()));

    ReportBuildSuccess(return_ptr->description());
  } catch (...) {
    ReportBuildError();
    throw;
  }
  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildUpdaterPointers(
    std::unique_ptr<DiffusionFormulationType> formulation_ptr,
    std::unique_ptr<StamperType> stamper_ptr)
-> UpdaterPointers {
  ReportBuildingComponant("Building Diffusion Formulation updater");
  UpdaterPointers return_struct;

  using ReturnType = formulation::updater::DiffusionUpdater<dim>;

  auto diffusion_updater_ptr = std::make_shared<ReturnType>(
      std::move(formulation_ptr),
      std::move(stamper_ptr));
  return_struct.fixed_updater_ptr = diffusion_updater_ptr;
  return_struct.scattering_source_updater_ptr = diffusion_updater_ptr;
  return_struct.fission_source_updater_ptr = diffusion_updater_ptr;

  return return_struct;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildUpdaterPointers(
    std::unique_ptr<SAAFFormulationType> formulation_ptr,
    std::unique_ptr<StamperType> stamper_ptr,
    const std::shared_ptr<QuadratureSetType>& quadrature_set_ptr)
-> UpdaterPointers {
  ReportBuildingComponant("Building SAAF Formulation updater");
  UpdaterPointers return_struct;

  using ReturnType = formulation::updater::SAAFUpdater<dim>;
  auto saaf_updater_ptr = std::make_shared<ReturnType>(
      std::move(formulation_ptr),
      std::move(stamper_ptr),
      quadrature_set_ptr);
  return_struct.fixed_updater_ptr = saaf_updater_ptr;
  return_struct.scattering_source_updater_ptr = saaf_updater_ptr;
  return_struct.fission_source_updater_ptr = saaf_updater_ptr;

  return return_struct;
}

template <int dim>
auto FrameworkBuilder<dim>::BuildGroupSolveIteration(
    std::unique_ptr<SingleGroupSolverType> single_group_solver_ptr,
    std::unique_ptr<MomentConvergenceCheckerType> moment_convergence_checker_ptr,
    std::unique_ptr<MomentCalculatorType> moment_calculator_ptr,
    const std::shared_ptr<GroupSolutionType>& group_solution_ptr,
    const std::shared_ptr<ScatteringSourceUpdaterType>& scattering_source_updater_ptr,
    const std::shared_ptr<ReporterType>& convergence_report_ptr)
    -> std::unique_ptr<GroupSolveIterationType> {
  std::unique_ptr<GroupSolveIterationType> return_ptr = nullptr;

  ReportBuildingComponant("Iterative group solver");

  return_ptr = std::move(
      std::make_unique<iteration::group::GroupSourceIteration<dim>>(
          std::move(single_group_solver_ptr),
          std::move(moment_convergence_checker_ptr),
          std::move(moment_calculator_ptr),
          group_solution_ptr,
          scattering_source_updater_ptr,
          convergence_report_ptr)
      );
  has_scattering_source_update_ = true;
  ReportBuildSuccess(return_ptr->description());
  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildGroupSolution(const int n_angles)
-> std::unique_ptr<GroupSolutionType> {
  std::unique_ptr<GroupSolutionType> return_ptr = nullptr;
  ReportBuildingComponant("Group solution");

  return_ptr = std::move(
      std::make_unique<system::solution::MPIGroupAngularSolution>(n_angles));
  ReportBuildSuccess(return_ptr->description());
  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildInitializer(
    const std::shared_ptr<formulation::updater::FixedUpdaterI>& updater_ptr,
    const int total_groups,
    const int total_angles) -> std::unique_ptr<InitializerType> {
  ReportBuildingComponant("Initializer");

  std::unique_ptr<InitializerType> return_ptr = nullptr;

  using InitializeOnceType = iteration::initializer::InitializeFixedTermsOnce;

  return_ptr = std::move(std::make_unique<InitializeOnceType>(
      updater_ptr, total_groups, total_angles));
  ReportBuildSuccess(return_ptr->description());
  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildKEffectiveUpdater(
    const std::shared_ptr<FiniteElementType>& finite_element_ptr,
    const std::shared_ptr<CrossSectionType>& cross_sections_ptr,
    const std::shared_ptr<DomainType>& domain_ptr)
-> std::unique_ptr<KEffectiveUpdaterType> {
  using AggregatedFissionSource = calculator::cell::TotalAggregatedFissionSource<dim>;
  using IntegratedFissionSource = calculator::cell::IntegratedFissionSource<dim>;
  using ReturnType = eigenvalue::k_effective::UpdaterViaFissionSource;

  ReportBuildingComponant("K_Effective updater");
  std::unique_ptr<KEffectiveUpdaterType> return_ptr = nullptr;

  return_ptr = std::move(
      std::make_unique<ReturnType>(
          std::make_unique<AggregatedFissionSource>(
              std::make_unique<IntegratedFissionSource>(finite_element_ptr,
                                                        cross_sections_ptr),
              domain_ptr),
          2.0,
          10));

  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildMomentCalculator(
    MomentCalculatorImpl implementation)
-> std::unique_ptr<MomentCalculatorType> {
  return BuildMomentCalculator(nullptr, implementation);
}

template<int dim>
auto FrameworkBuilder<dim>::BuildMomentCalculator(
    std::shared_ptr<QuadratureSetType> quadrature_set_ptr,
    FrameworkBuilder::MomentCalculatorImpl implementation)
-> std::unique_ptr<MomentCalculatorType> {
  ReportBuildingComponant("Moment calculator");
  std::unique_ptr<MomentCalculatorType> return_ptr = nullptr;

  try {
    return_ptr = std::move(quadrature::factory::MakeMomentCalculator<dim>(
        implementation, quadrature_set_ptr));

    if (implementation == MomentCalculatorImpl::kScalarMoment) {
      ReportBuildSuccess("(default) calculator for scalar solve");
    } else if (implementation == MomentCalculatorImpl::kZerothMomentOnly) {
      ReportBuildSuccess("(default) calculator for 0th moment only");
    } else {
      AssertThrow(false,
                  dealii::ExcMessage("Unsupported implementation of moment "
                                     "calculator specified in call to "
                                     "BuildMomentCalculator"))
    }
  } catch (...) {
    ReportBuildError();
    throw;
  }

  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildMomentConvergenceChecker(
    double max_delta, int max_iterations)
-> std::unique_ptr<MomentConvergenceCheckerType>{
  //TODO(Josh): Add option for using other than L1Norm
  ReportBuildingComponant("Moment convergence checker");

  using CheckerType = convergence::moments::SingleMomentCheckerL1Norm;
  using FinalCheckerType = convergence::FinalCheckerOrN<
      system::moments::MomentVector,
      convergence::moments::SingleMomentCheckerI>;

  auto single_checker_ptr = std::make_unique<CheckerType>(max_delta);
  auto return_ptr = std::make_unique<FinalCheckerType>(
      std::move(single_checker_ptr));
  return_ptr->SetMaxIterations(max_iterations);
  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildOuterIteration(
    std::unique_ptr<GroupSolveIterationType> group_solve_iteration_ptr,
    std::unique_ptr<ParameterConvergenceCheckerType> parameter_convergence_checker_ptr,
    std::unique_ptr<KEffectiveUpdaterType> k_effective_updater_ptr,
    const std::shared_ptr<FissionSourceUpdaterType>& fission_source_updater_ptr,
    const std::shared_ptr<ReporterType>& convergence_reporter_ptr)
-> std::unique_ptr<OuterIterationType> {
  std::unique_ptr<OuterIterationType> return_ptr = nullptr;
  ReportBuildingComponant("Outer Iteration");

  using DefaultOuterPowerIteration = iteration::outer::OuterPowerIteration;

  return_ptr = std::move(
      std::make_unique<DefaultOuterPowerIteration>(
          std::move(group_solve_iteration_ptr),
          std::move(parameter_convergence_checker_ptr),
          std::move(k_effective_updater_ptr),
          fission_source_updater_ptr,
          convergence_reporter_ptr));

  has_fission_source_update_ = true;

  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildParameterConvergenceChecker(
    double max_delta, int max_iterations)
-> std::unique_ptr<ParameterConvergenceCheckerType>{
  ReportBuildingComponant("Parameter (double) convergence checker");
  using CheckerType = convergence::parameters::SingleParameterChecker;
  using FinalCheckerType = convergence::FinalCheckerOrN<double, CheckerType>;

  auto single_checker_ptr = std::make_unique<CheckerType>(max_delta);
  auto return_ptr = std::make_unique<FinalCheckerType>(
      std::move(single_checker_ptr));
  return_ptr->SetMaxIterations(max_iterations);

  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildQuadratureSet(ParametersType problem_parameters)
-> std::shared_ptr<QuadratureSetType> {
  ReportBuildingComponant("quadrature set");
  using QuadratureGeneratorType = quadrature::QuadratureGeneratorI<dim>;

  std::shared_ptr<QuadratureSetType> return_ptr = nullptr;
  std::shared_ptr<QuadratureGeneratorType > quadrature_generator_ptr = nullptr;

  const int order_value = problem_parameters.AngularQuadOrder();
  switch (problem_parameters.AngularQuad()) {
    default: {
      if (dim == 3) {
        quadrature_generator_ptr =
            quadrature::factory::MakeAngularQuadratureGeneratorPtr<dim>(
                quadrature::Order(order_value),
                quadrature::AngularQuadratureSetType::kLevelSymmetricGaussian);
      } else {
        AssertThrow(false,
                    dealii::ExcMessage("No supported quadratures for this dimension "
                                       "and transport model"))
      }
    }
  }

  return_ptr = quadrature::factory::MakeQuadratureSetPtr<dim>();

  auto quadrature_points = quadrature::utility::GenerateAllPositiveX<dim>(
      quadrature_generator_ptr->GenerateSet());

  quadrature::factory::FillQuadratureSet<dim>(return_ptr.get(),
                                              quadrature_points);

  return return_ptr;
}

template <int dim>
auto FrameworkBuilder<dim>::BuildSAAFFormulation(
    const std::shared_ptr<FiniteElementType>& finite_element_ptr,
    const std::shared_ptr<data::CrossSections>& cross_sections_ptr,
    const std::shared_ptr<QuadratureSetType>& quadrature_set_ptr,
    const formulation::SAAFFormulationImpl implementation)
-> std::unique_ptr<SAAFFormulationType> {
  reporter_ptr_->Report("\tBuilding SAAF Formulation\n");
  std::unique_ptr<SAAFFormulationType> return_ptr;

  if (implementation == formulation::SAAFFormulationImpl::kDefault) {
    using ReturnType = formulation::angular::SelfAdjointAngularFlux<dim>;

    return_ptr = std::move(std::make_unique<ReturnType>(finite_element_ptr,
                                                        cross_sections_ptr,
                                                        quadrature_set_ptr));
  }

  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildSingleGroupSolver(
    const int max_iterations,
    const double convergence_tolerance)
-> std::unique_ptr<SingleGroupSolverType> {
  ReportBuildingComponant("Single group solver");
  std::unique_ptr<SingleGroupSolverType> return_ptr = nullptr;

  auto linear_solver_ptr = std::make_unique<solver::GMRES>(max_iterations,
                                                           convergence_tolerance);
  ReportBuildSuccess("GMRES: tol = " + std::to_string(convergence_tolerance)
                            + "iter_max = " + std::to_string(max_iterations));
  return_ptr = std::move(std::make_unique<solver::group::SingleGroupSolver>(
          std::move(linear_solver_ptr)));

  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildSystem(
    const int total_groups,
    const int total_angles,
    const DomainType& domain,
    const std::size_t solution_size,
    bool is_eigenvalue_problem) -> std::unique_ptr<SystemType> {
  std::unique_ptr<SystemType> return_ptr;

  ReportBuildingComponant("system");
  try {
    return_ptr = std::move(std::make_unique<SystemType>());
    system::InitializeSystem(*return_ptr, total_groups, total_angles,
                             is_eigenvalue_problem);
    system::SetUpSystemTerms(*return_ptr, domain);
    system::SetUpSystemMoments(*return_ptr, solution_size);
  } catch (...) {
    ReportBuildError("system initialization error.");
    throw;
  }

  return return_ptr;
}

template<int dim>
auto FrameworkBuilder<dim>::BuildStamper(
    const std::shared_ptr<DomainType>& domain_ptr)
-> std::unique_ptr<StamperType> {
  ReportBuildingComponant("Stamper");
  std::unique_ptr<StamperType> return_ptr = nullptr;

  return_ptr = std::move(
      std::make_unique<formulation::Stamper<dim>>(domain_ptr));
  ReportBuildSuccess(return_ptr->description());
  return return_ptr;
}
template<int dim>
std::string FrameworkBuilder<dim>::ReadMappingFile(std::string filename) {
  reporter_ptr_->Report("\tReading mapping file: ");

  std::ifstream mapping_file(filename);
  if (mapping_file.is_open()) {
    reporter_ptr_->Report(filename + '\n', Color::Green);

    return std::string(
        (std::istreambuf_iterator<char>(mapping_file)),
        std::istreambuf_iterator<char>());
  } else {
    reporter_ptr_->Report("Error reading " + filename + "\n", Color::Red);
    AssertThrow(false,
                dealii::ExcMessage("Failed to open material mapping file"))
  }
}

template<int dim>
void FrameworkBuilder<dim>::Validate() const {
  bool issue = false;
  Report("\nValidating framework\n");
  reporter_ptr_->Report("\tHas scattering source update: ");
  if (has_scattering_source_update_) {
    reporter_ptr_->Report("True\n", Color::Green);
  } else {
    reporter_ptr_->Report("False\n", Color::Red);
    issue = true;
  }
  reporter_ptr_->Report("\tHas fission source update: ");
  if (has_fission_source_update_) {
    reporter_ptr_->Report("True\n", Color::Green);
  } else {
    reporter_ptr_->Report("False\n", Color::Red);
    issue = true;
  }
  if (issue) {
    reporter_ptr_->Report("Warning: one or more issues identified during "
                          "framework validation\n", Color::Yellow);
  }
}

template class FrameworkBuilder<1>;
template class FrameworkBuilder<2>;
template class FrameworkBuilder<3>;

} // namespace builder

} // namespace framework

} // namespace bart


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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

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

//#define MBILOG_ENABLE_DEBUG

#include "QmitkQBallReconstructionView.h"

// qt includes
#include <QMessageBox>

// itk includes
#include "itkTimeProbe.h"

// mitk includes
#include "mitkProgressBar.h"
#include "mitkStatusBar.h"

#include "mitkNodePredicateDataType.h"
#include "QmitkDataStorageComboBox.h"

#include "itkDiffusionQballReconstructionImageFilter.h"
#include "itkAnalyticalDiffusionQballReconstructionImageFilter.h"
#include "itkDiffusionMultiShellQballReconstructionImageFilter.h"
#include "itkVectorContainer.h"
#include "itkB0ImageExtractionImageFilter.h"
#include <itkBinaryThresholdImageFilter.h>

#include "mitkOdfImage.h"
#include "mitkProperties.h"
#include "mitkVtkResliceInterpolationProperty.h"
#include "mitkLookupTable.h"
#include "mitkLookupTableProperty.h"
#include "mitkTransferFunction.h"
#include "mitkTransferFunctionProperty.h"
#include "mitkDataNodeObject.h"
#include "mitkOdfNormalizationMethodProperty.h"
#include "mitkOdfScaleByProperty.h"
#include <mitkImageCast.h>
#include "mitkDiffusionImagingConfigure.h"
#include <mitkNodePredicateIsDWI.h>

#include "berryIStructuredSelection.h"
#include "berryIWorkbenchWindow.h"
#include "berryISelectionService.h"
#include <mitkShImage.h>
#include <boost/version.hpp>
#include <itkShToOdfImageFilter.h>
#include <mitkImageCast.h>

const std::string QmitkQBallReconstructionView::VIEW_ID = "org.mitk.views.qballreconstruction";

typedef float TTensorPixelType;

const int QmitkQBallReconstructionView::nrconvkernels = 252;

struct QbrShellSelection
{
  typedef mitk::DiffusionPropertyHelper::GradientDirectionType            GradientDirectionType;
  typedef mitk::DiffusionPropertyHelper::GradientDirectionsContainerType  GradientDirectionContainerType;
  typedef mitk::DiffusionPropertyHelper::BValueMapType                    BValueMapType;
  typedef itk::VectorImage< DiffusionPixelType, 3 >                       ITKDiffusionImageType;

  QmitkQBallReconstructionView* m_View;
  mitk::DataNode * m_Node;
  std::string m_NodeName;

  std::vector<QCheckBox *> m_CheckBoxes;
  QLabel * m_Label;

  mitk::Image * m_Image;

  QbrShellSelection(QmitkQBallReconstructionView* view, mitk::DataNode * node)
    : m_View(view),
      m_Node(node),
      m_NodeName(node->GetName())
  {
    m_Image = dynamic_cast<mitk::Image * > (node->GetData());

    if(!m_Image)
    {
      MITK_ERROR << "QmitkQBallReconstructionView::QbrShellSelection : no image selected";
      return;
    }
    bool isDiffusionImage( mitk::DiffusionPropertyHelper::IsDiffusionWeightedImage( dynamic_cast<mitk::Image *>(m_Node->GetData())) );

    if( !isDiffusionImage )
    {
      MITK_ERROR <<
                    "QmitkQBallReconstructionView::QbrShellSelection : selected image contains no diffusion information";
      return;
    }

    GenerateCheckboxes();

  }

  void GenerateCheckboxes()
  {
    BValueMapType origMap = mitk::DiffusionPropertyHelper::GetBValueMap(m_Image);
    BValueMapType::iterator itStart = origMap.begin();
    itStart++;
    BValueMapType::iterator itEnd = origMap.end();

    m_Label = new QLabel(m_NodeName.c_str());
    m_Label->setVisible(true);
    m_View->m_Controls->m_QBallSelectionBox->layout()->addWidget(m_Label);

    for(BValueMapType::iterator it = itStart ; it!= itEnd; it++)
    {
      QCheckBox * box  = new QCheckBox(QString::number(it->first));
      m_View->m_Controls->m_QBallSelectionBox->layout()->addWidget(box);

      box->setChecked(true);
      box->setCheckable(true);
      // box->setVisible(true);
      m_CheckBoxes.push_back(box);
    }
  }

  void SetVisible(bool vis)
  {
    foreach(QCheckBox * box, m_CheckBoxes)
    {
      box->setVisible(vis);
    }
  }

  BValueMapType GetBValueSelctionMap()
  {
    BValueMapType inputMap = mitk::DiffusionPropertyHelper::GetBValueMap(m_Image);
    BValueMapType outputMap;

    unsigned int val = 0;

    if(inputMap.find(0) == inputMap.end()){
      return outputMap;
    }else{
      outputMap[val] = inputMap[val];
    }

    foreach(QCheckBox * box, m_CheckBoxes)
    {
      if(box->isChecked()){
        val = box->text().toUInt();
        outputMap[val] = inputMap[val];
        MITK_INFO << val;
      }
    }

    return outputMap;
  }

  ~QbrShellSelection()
  {
    m_View->m_Controls->m_QBallSelectionBox->layout()->removeWidget(m_Label);
    delete m_Label;

    for(std::vector<QCheckBox *>::iterator it = m_CheckBoxes.begin() ; it!= m_CheckBoxes.end(); it++)
    {
      m_View->m_Controls->m_QBallSelectionBox->layout()->removeWidget((*it));
      delete (*it);
    }
    m_CheckBoxes.clear();
  }
};

QmitkQBallReconstructionView::QmitkQBallReconstructionView()
  : QmitkAbstractView(),
    m_Controls(nullptr)
{
}

QmitkQBallReconstructionView::~QmitkQBallReconstructionView()
{

}

void QmitkQBallReconstructionView::CreateQtPartControl(QWidget *parent)
{
  if (!m_Controls)
  {
    // create GUI widgets
    m_Controls = new Ui::QmitkQBallReconstructionViewControls;
    m_Controls->setupUi(parent);
    this->CreateConnections();

    QStringList items;
    items << "2" << "4" << "6" << "8" << "10" << "12";
    m_Controls->m_QBallReconstructionMaxLLevelComboBox->addItems(items);
    m_Controls->m_QBallReconstructionMaxLLevelComboBox->setCurrentIndex(1);
    MethodChoosen(m_Controls->m_QBallReconstructionMethodComboBox->currentIndex());
  }
}

void QmitkQBallReconstructionView::SetFocus()
{
}

void QmitkQBallReconstructionView::CreateConnections()
{
  if ( m_Controls )
  {
    connect( static_cast<QObject*>(m_Controls->m_ButtonStandard), SIGNAL(clicked()), this, SLOT(ReconstructStandard()) );
    connect( static_cast<QObject*>(m_Controls->m_QBallReconstructionMethodComboBox), SIGNAL(currentIndexChanged(int)), this, SLOT(MethodChoosen(int)) );
    connect( static_cast<QObject*>(m_Controls->m_QBallReconstructionThreasholdEdit), SIGNAL(valueChanged(int)), this, SLOT(PreviewThreshold(int)) );
    connect( static_cast<QObject*>(m_Controls->m_ConvertButton), SIGNAL(clicked()), this, SLOT(ConvertShImage()) );

    m_Controls->m_ImageBox->SetDataStorage(this->GetDataStorage());
    mitk::NodePredicateIsDWI::Pointer isDwi = mitk::NodePredicateIsDWI::New();
    m_Controls->m_ImageBox->SetPredicate( isDwi );

    m_Controls->m_ShImageBox->SetDataStorage(this->GetDataStorage());
    mitk::TNodePredicateDataType<mitk::ShImage>::Pointer isSh = mitk::TNodePredicateDataType<mitk::ShImage>::New();
    m_Controls->m_ShImageBox->SetPredicate( isSh );

    connect( static_cast<QObject*>(m_Controls->m_ImageBox), SIGNAL(currentIndexChanged(int)), this, SLOT(UpdateGui()));
    connect( static_cast<QObject*>(m_Controls->m_ShImageBox), SIGNAL(currentIndexChanged(int)), this, SLOT(UpdateGui()));

    UpdateGui();
  }
}

template<int ShOrder>
void QmitkQBallReconstructionView::TemplatedConvertShImage(mitk::ShImage::Pointer mitkImage)
{
  typedef itk::ShToOdfImageFilter< float, ShOrder > ShConverterType;

  typename ShConverterType::InputImageType::Pointer itkvol = ShConverterType::InputImageType::New();
  mitk::CastToItkImage(mitkImage, itkvol);

  typename ShConverterType::Pointer converter = ShConverterType::New();
  converter->SetInput(itkvol);
  converter->Update();

  mitk::OdfImage::Pointer image = mitk::OdfImage::New();
  image->InitializeByItk( converter->GetOutput() );
  image->SetVolume( converter->GetOutput()->GetBufferPointer() );
  mitk::DataNode::Pointer node=mitk::DataNode::New();
  node->SetData( image );
  node->SetName(m_Controls->m_ShImageBox->GetSelectedNode()->GetName());

  GetDataStorage()->Add(node, m_Controls->m_ShImageBox->GetSelectedNode());
}

void QmitkQBallReconstructionView::ConvertShImage()
{
  if (m_Controls->m_ShImageBox->GetSelectedNode().IsNotNull())
  {
    mitk::ShImage::Pointer mitkImg = dynamic_cast<mitk::ShImage*>(m_Controls->m_ShImageBox->GetSelectedNode()->GetData());
    switch (mitkImg->ShOrder())
    {
    case 2:
      TemplatedConvertShImage<2>(mitkImg);
      break;
    case 4:
      TemplatedConvertShImage<4>(mitkImg);
      break;
    case 6:
      TemplatedConvertShImage<6>(mitkImg);
      break;
    case 8:
      TemplatedConvertShImage<8>(mitkImg);
      break;
    case 10:
      TemplatedConvertShImage<10>(mitkImg);
      break;
    case 12:
      TemplatedConvertShImage<12>(mitkImg);
      break;
    default :
      QMessageBox::warning(nullptr, "Error", "Only spherical harmonics orders 2-12 are supported.", QMessageBox::Ok);
    }
  }
}

void QmitkQBallReconstructionView::UpdateGui()
{
  m_Controls->m_ButtonStandard->setEnabled(false);
  if (m_Controls->m_ImageBox->GetSelectedNode().IsNotNull())
  {
    m_Controls->m_ButtonStandard->setEnabled(true);
    GenerateShellSelectionUI(m_Controls->m_ImageBox->GetSelectedNode());
  }

  m_Controls->m_ConvertButton->setEnabled(m_Controls->m_ShImageBox->GetSelectedNode().IsNotNull());
}

void QmitkQBallReconstructionView::OnSelectionChanged(berry::IWorkbenchPart::Pointer /*part*/, const QList<mitk::DataNode::Pointer>& /*nodes*/)
{
  UpdateGui();
}

void QmitkQBallReconstructionView::Activated()
{
}

void QmitkQBallReconstructionView::Deactivated()
{
}

void QmitkQBallReconstructionView::Visible()
{
}

void QmitkQBallReconstructionView::Hidden()
{
}

void QmitkQBallReconstructionView::ReconstructStandard()
{
  int index = m_Controls->m_QBallReconstructionMethodComboBox->currentIndex();

  switch(index)
  {
  case 0:
  {
    // Numerical
    Reconstruct(0,0);
    break;
  }
  case 1:
  {
    // Standard
    Reconstruct(1,0);
    break;
  }
  case 2:
  {
    // Solid Angle
    Reconstruct(1,6);
    break;
  }
  case 3:
  {
    // ADC
    Reconstruct(1,4);
    break;
  }
  case 4:
  {
    // Raw Signal
    Reconstruct(1,5);
    break;
  }
  case 5:
  {
    // Q-Ball reconstruction
    Reconstruct(2,0);
    break;
  }
  }
}

void QmitkQBallReconstructionView::MethodChoosen(int method)
{
  m_Controls->m_QBallSelectionBox->setHidden(true);
  m_Controls->m_OutputCoeffsImage->setHidden(true);

  if (method==0)
    m_Controls->m_ShFrame->setVisible(false);
  else
    m_Controls->m_ShFrame->setVisible(true);

  switch(method)
  {
  case 0:
    m_Controls->m_Description->setText("Numerical recon. (Tuch 2004)");
    break;
  case 1:
    m_Controls->m_Description->setText("Spherical harmonics recon. (Descoteaux 2007)");
    m_Controls->m_OutputCoeffsImage->setHidden(false);
    break;
  case 2:
    m_Controls->m_Description->setText("SH recon. with solid angle consideration (Aganj 2009)");
    m_Controls->m_OutputCoeffsImage->setHidden(false);
    break;
  case 3:
    m_Controls->m_Description->setText("SH solid angle with non-neg. constraint (Goh 2009)");
    m_Controls->m_OutputCoeffsImage->setHidden(false);
    break;
  case 4:
    m_Controls->m_Description->setText("SH recon. of the plain ADC-profiles");
    m_Controls->m_OutputCoeffsImage->setHidden(false);
    break;
  case 5:
    m_Controls->m_Description->setText("SH recon. of the raw diffusion signal");
    m_Controls->m_OutputCoeffsImage->setHidden(false);
    break;
  case 6:
    m_Controls->m_Description->setText("SH recon. of the multi shell diffusion signal (Aganj 2010)");
    m_Controls->m_QBallSelectionBox->setHidden(false);
    m_Controls->m_OutputCoeffsImage->setHidden(false);
    break;
  }
}

void QmitkQBallReconstructionView::Reconstruct(int method, int normalization)
{
  if (m_Controls->m_ImageBox->GetSelectedNode().IsNotNull())
  {
    if(method == 0)
    {
      NumericalQBallReconstruction(m_Controls->m_ImageBox->GetSelectedNode(), normalization);
    }
    else if(method == 1)
    {
      AnalyticalQBallReconstruction(m_Controls->m_ImageBox->GetSelectedNode(), normalization);
    }
    else if(method == 2)
    {
      MultiQBallReconstruction(m_Controls->m_ImageBox->GetSelectedNode());
    }
  }
}

void QmitkQBallReconstructionView::NumericalQBallReconstruction(mitk::DataNode::Pointer node, int normalization)
{
  try
  {
    mitk::Image* vols = static_cast<mitk::Image*>(node->GetData());

    std::string nodename = node->GetName();

    typedef itk::DiffusionQballReconstructionImageFilter
        <DiffusionPixelType, DiffusionPixelType, TTensorPixelType, ODF_SAMPLING_SIZE>
        QballReconstructionImageFilterType;

    ITKDiffusionImageType::Pointer itkVectorImagePointer = ITKDiffusionImageType::New();
    mitk::CastToItkImage(vols, itkVectorImagePointer);

    QballReconstructionImageFilterType::Pointer filter = QballReconstructionImageFilterType::New();
    filter->SetBValue(mitk::DiffusionPropertyHelper::GetReferenceBValue(vols));
    filter->SetGradientImage(mitk::DiffusionPropertyHelper::GetGradientContainer(vols), itkVectorImagePointer);
    filter->SetThreshold( m_Controls->m_QBallReconstructionThreasholdEdit->value() );

    std::string nodePostfix;
    switch(normalization)
    {
    case 0:
    {
      filter->SetNormalizationMethod(QballReconstructionImageFilterType::QBR_STANDARD);
      nodePostfix = "_Numerical_Qball";
      break;
    }
    case 1:
    {
      filter->SetNormalizationMethod(QballReconstructionImageFilterType::QBR_B_ZERO_B_VALUE);
      nodePostfix = "_Numerical_ZeroBvalueNormalization_Qball";
      break;
    }
    case 2:
    {
      filter->SetNormalizationMethod(QballReconstructionImageFilterType::QBR_B_ZERO);
      nodePostfix = "_NumericalQball_ZeroNormalization_Qball";
      break;
    }
    case 3:
    {
      filter->SetNormalizationMethod(QballReconstructionImageFilterType::QBR_NONE);
      nodePostfix = "_NumericalQball_NoNormalization_Qball";
      break;
    }
    default:
    {
      filter->SetNormalizationMethod(QballReconstructionImageFilterType::QBR_STANDARD);
      nodePostfix = "_NumericalQball_Qball";
    }
    }

    filter->Update();

    // ODFs TO DATATREE
    mitk::OdfImage::Pointer image = mitk::OdfImage::New();
    image->InitializeByItk( filter->GetOutput() );
    image->SetVolume( filter->GetOutput()->GetBufferPointer() );
    mitk::DataNode::Pointer new_node = mitk::DataNode::New();
    new_node->SetData( image );
    new_node->SetName(nodename+nodePostfix);
    mitk::ProgressBar::GetInstance()->Progress();

    GetDataStorage()->Add(new_node, node);

    this->GetRenderWindowPart()->RequestUpdate();
  }
  catch (itk::ExceptionObject &ex)
  {
    MITK_INFO << ex ;
    QMessageBox::information(0, "Reconstruction not possible:", ex.GetDescription());
    return ;
  }
}

void QmitkQBallReconstructionView::AnalyticalQBallReconstruction( mitk::DataNode::Pointer node, int normalization)
{
  try
  {
    float lambda = m_Controls->m_QBallReconstructionLambdaLineEdit->value();
    switch(m_Controls->m_QBallReconstructionMaxLLevelComboBox->currentIndex())
    {
    case 0:
    {
      TemplatedAnalyticalQBallReconstruction<2>(node, lambda, normalization);
      break;
    }
    case 1:
    {
      TemplatedAnalyticalQBallReconstruction<4>(node, lambda, normalization);
      break;
    }
    case 2:
    {
      TemplatedAnalyticalQBallReconstruction<6>(node, lambda, normalization);
      break;
    }
    case 3:
    {
      TemplatedAnalyticalQBallReconstruction<8>(node, lambda, normalization);
      break;
    }
    case 4:
    {
      TemplatedAnalyticalQBallReconstruction<10>(node, lambda, normalization);
      break;
    }
    case 5:
    {
      TemplatedAnalyticalQBallReconstruction<12>(node, lambda, normalization);
      break;
    }
    }

    this->GetRenderWindowPart()->RequestUpdate();
  }
  catch (itk::ExceptionObject &ex)
  {
    MITK_INFO << ex;
    QMessageBox::information(0, "Reconstruction not possible:", ex.GetDescription());
    return;
  }
}

template<int L>
void QmitkQBallReconstructionView::TemplatedAnalyticalQBallReconstruction(mitk::DataNode* dataNodePointer, float lambda, int normalization)
{
  typedef itk::AnalyticalDiffusionQballReconstructionImageFilter
      <DiffusionPixelType,DiffusionPixelType,TTensorPixelType,L,ODF_SAMPLING_SIZE> FilterType;
  typename FilterType::Pointer filter = FilterType::New();

  mitk::Image* vols = dynamic_cast<mitk::Image*>(dataNodePointer->GetData());

  ITKDiffusionImageType::Pointer itkVectorImagePointer = ITKDiffusionImageType::New();
  mitk::CastToItkImage(vols, itkVectorImagePointer);

  filter->SetBValue(mitk::DiffusionPropertyHelper::GetReferenceBValue(vols));
  filter->SetGradientImage(mitk::DiffusionPropertyHelper::GetGradientContainer(vols), itkVectorImagePointer);
  filter->SetThreshold( m_Controls->m_QBallReconstructionThreasholdEdit->value() );
  filter->SetLambda(lambda);

  std::string nodePostfix;
  switch(normalization)
  {
  case 0:
  {
    filter->SetNormalizationMethod(FilterType::QBAR_STANDARD);
    nodePostfix = "_SH_Qball";
    break;
  }
  case 1:
  {
    filter->SetNormalizationMethod(FilterType::QBAR_B_ZERO_B_VALUE);
    nodePostfix = "_SH_1_Qball";
    break;
  }
  case 2:
  {
    filter->SetNormalizationMethod(FilterType::QBAR_B_ZERO);
    nodePostfix = "_SH_2_Qball";
    break;
  }
  case 3:
  {
    filter->SetNormalizationMethod(FilterType::QBAR_NONE);
    nodePostfix = "_SH_3_Qball";
    break;
  }
  case 4:
  {
    filter->SetNormalizationMethod(FilterType::QBAR_ADC_ONLY);
    nodePostfix = "_AdcProfile";
    break;
  }
  case 5:
  {
    filter->SetNormalizationMethod(FilterType::QBAR_RAW_SIGNAL);
    nodePostfix = "_RawSignal";
    break;
  }
  case 6:
  {
    filter->SetNormalizationMethod(FilterType::QBAR_SOLID_ANGLE);
    nodePostfix = "_SH_CSA_Qball";
    break;
  }
  case 7:
  {
    filter->SetNormalizationMethod(FilterType::QBAR_NONNEG_SOLID_ANGLE);
    nodePostfix = "_SH_NonNegCSA_Qball";
    break;
  }
  default:
  {
    filter->SetNormalizationMethod(FilterType::QBAR_STANDARD);
  }
  }

  filter->Update();

  // ODFs TO DATATREE
  mitk::OdfImage::Pointer image = mitk::OdfImage::New();
  image->InitializeByItk( filter->GetOutput() );
  image->SetVolume( filter->GetOutput()->GetBufferPointer() );
  mitk::DataNode::Pointer node=mitk::DataNode::New();
  node->SetData( image );
  node->SetName(dataNodePointer->GetName()+nodePostfix);

  GetDataStorage()->Add(node, dataNodePointer);

  if(m_Controls->m_OutputCoeffsImage->isChecked())
  {
    mitk::Image::Pointer coeffsImage = dynamic_cast<mitk::Image*>(mitk::ShImage::New().GetPointer());
    coeffsImage->InitializeByItk( filter->GetCoefficientImage().GetPointer() );
    coeffsImage->SetVolume( filter->GetCoefficientImage()->GetBufferPointer() );

    mitk::DataNode::Pointer coeffsNode=mitk::DataNode::New();
    coeffsNode->SetData( coeffsImage );
    coeffsNode->SetProperty( "name", mitk::StringProperty::New(dataNodePointer->GetName()+"_SH-Coeffs") );
    GetDataStorage()->Add(coeffsNode, node);
  }
}

void QmitkQBallReconstructionView::MultiQBallReconstruction(mitk::DataNode::Pointer node)
{
  try
  {
    float lambda  = m_Controls->m_QBallReconstructionLambdaLineEdit->value();
    switch(m_Controls->m_QBallReconstructionMaxLLevelComboBox->currentIndex())
    {
    case 0:
    {
      TemplatedMultiQBallReconstruction<2>(lambda, node);
      break;
    }
    case 1:
    {
      TemplatedMultiQBallReconstruction<4>(lambda, node);
      break;
    }
    case 2:
    {
      TemplatedMultiQBallReconstruction<6>(lambda, node);
      break;
    }
    case 3:
    {
      TemplatedMultiQBallReconstruction<8>(lambda, node);
      break;
    }
    case 4:
    {
      TemplatedMultiQBallReconstruction<10>(lambda, node);
      break;
    }
    case 5:
    {
      TemplatedMultiQBallReconstruction<12>(lambda, node);
      break;
    }
    }
  }
  catch (itk::ExceptionObject &ex)
  {
    MITK_INFO << ex ;
    QMessageBox::information(0, "Reconstruction not possible:", ex.GetDescription());
    return ;
  }
}

template<int L>
void QmitkQBallReconstructionView::TemplatedMultiQBallReconstruction(float lambda, mitk::DataNode* dataNodePointer)
{
  typedef itk::DiffusionMultiShellQballReconstructionImageFilter
      <DiffusionPixelType,DiffusionPixelType,TTensorPixelType,L,ODF_SAMPLING_SIZE> FilterType;
  typename FilterType::Pointer filter = FilterType::New();

  std::string nodename;
  dataNodePointer->GetStringProperty("name",nodename);

  mitk::Image* dwi = dynamic_cast<mitk::Image*>(dataNodePointer->GetData());
  BValueMapType currSelectionMap = m_ShellSelectorMap[dataNodePointer]->GetBValueSelctionMap();

  if(currSelectionMap.size() != 4)// || currSelectionMap.find(0) != currSelectionMap.end())
  {
    QMessageBox::information(0, "Reconstruction not possible:" ,QString("Only three equidistant shells are supported. (ImageName: " + QString(nodename.c_str()) + ")"));
    return;
  }

  BValueMapType::reverse_iterator it1 = currSelectionMap.rbegin();
  BValueMapType::reverse_iterator it2 = currSelectionMap.rbegin();
  ++it2;

  // Get average distance
  int avdistance = 0;
  for(; it2 != currSelectionMap.rend(); ++it1,++it2)
    avdistance += (int)it1->first - (int)it2->first;
  avdistance /= currSelectionMap.size()-1;

  // Check if all shells are using the same averae distance
  it1 = currSelectionMap.rbegin();
  it2 = currSelectionMap.rbegin();
  ++it2;
  for(; it2 != currSelectionMap.rend(); ++it1,++it2)
  {
    if(avdistance != (int)it1->first - (int)it2->first)
    {
      QMessageBox::information(0, "Reconstruction not possible:" ,QString("Selected Shells are not in a equidistant configuration. (ImageName: " + QString(nodename.c_str()) + ")"));
      return;
    }
  }
  ITKDiffusionImageType::Pointer itkVectorImagePointer = ITKDiffusionImageType::New();
  mitk::CastToItkImage(dwi, itkVectorImagePointer);

  filter->SetBValueMap(m_ShellSelectorMap[dataNodePointer]->GetBValueSelctionMap());
  filter->SetGradientImage(mitk::DiffusionPropertyHelper::GetGradientContainer(dwi), itkVectorImagePointer, mitk::DiffusionPropertyHelper::GetReferenceBValue(dwi));
  filter->SetThreshold( m_Controls->m_QBallReconstructionThreasholdEdit->value() );
  filter->SetLambda(lambda);
  filter->Update();

  // ODFs TO DATATREE
  mitk::OdfImage::Pointer image = mitk::OdfImage::New();
  image->InitializeByItk( filter->GetOutput() );
  image->SetVolume( filter->GetOutput()->GetBufferPointer() );
  mitk::DataNode::Pointer node=mitk::DataNode::New();
  node->SetData( image );
  node->SetName(nodename+"_SH_MultiShell_Qball");

  GetDataStorage()->Add(node, dataNodePointer);

  if(m_Controls->m_OutputCoeffsImage->isChecked())
  {
    mitk::Image::Pointer coeffsImage = dynamic_cast<mitk::Image*>(mitk::ShImage::New().GetPointer());
    coeffsImage->InitializeByItk( filter->GetCoefficientImage().GetPointer() );
    coeffsImage->SetVolume( filter->GetCoefficientImage()->GetBufferPointer() );

    mitk::DataNode::Pointer coeffsNode=mitk::DataNode::New();
    coeffsNode->SetData( coeffsImage );
    coeffsNode->SetProperty( "name", mitk::StringProperty::New(dataNodePointer->GetName()+"_SH-Coeffs") );
    GetDataStorage()->Add(coeffsNode, node);
  }
}

void QmitkQBallReconstructionView::GenerateShellSelectionUI(mitk::DataNode::Pointer node)
{
  std::map<const mitk::DataNode * , QbrShellSelection * > tempMap;

  if(m_ShellSelectorMap.find(  node.GetPointer() ) != m_ShellSelectorMap.end())
  {
    tempMap[node.GetPointer()] = m_ShellSelectorMap[node.GetPointer()];
    m_ShellSelectorMap.erase(node.GetPointer());
  }
  else
  {
    tempMap[node.GetPointer()] = new QbrShellSelection(this, node );
    tempMap[node.GetPointer()]->SetVisible(true);
  }

  for(std::map<const mitk::DataNode * , QbrShellSelection * >::iterator it = m_ShellSelectorMap.begin(); it != m_ShellSelectorMap.end();it ++)
  {
    delete it->second;
  }
  m_ShellSelectorMap.clear();
  m_ShellSelectorMap = tempMap;
}

void QmitkQBallReconstructionView::PreviewThreshold(int threshold)
{
  if (m_Controls->m_ImageBox->GetSelectedNode().IsNotNull())
  {
    mitk::Image* vols = static_cast<mitk::Image*>(m_Controls->m_ImageBox->GetSelectedNode()->GetData());

    // Extract b0 image
    ITKDiffusionImageType::Pointer itkVectorImagePointer = ITKDiffusionImageType::New();
    mitk::CastToItkImage(vols, itkVectorImagePointer);

    typedef itk::B0ImageExtractionImageFilter<short, short> FilterType;
    FilterType::Pointer filterB0 = FilterType::New();
    filterB0->SetInput( itkVectorImagePointer );
    filterB0->SetDirections(mitk::DiffusionPropertyHelper::GetGradientContainer(vols));
    filterB0->Update();

    mitk::Image::Pointer mitkImage = mitk::Image::New();

    typedef itk::Image<short, 3> ImageType;
    typedef itk::Image<short, 3> SegmentationType;
    typedef itk::BinaryThresholdImageFilter<ImageType, SegmentationType> ThresholdFilterType;
    // apply threshold
    ThresholdFilterType::Pointer filterThreshold = ThresholdFilterType::New();
    filterThreshold->SetInput(filterB0->GetOutput());
    filterThreshold->SetLowerThreshold(threshold);
    filterThreshold->SetInsideValue(0);
    filterThreshold->SetOutsideValue(1); // mark cut off values red
    filterThreshold->Update();

    mitkImage->InitializeByItk( filterThreshold->GetOutput() );
    mitkImage->SetVolume( filterThreshold->GetOutput()->GetBufferPointer() );
    mitk::DataNode::Pointer node;
    if (this->GetDataStorage()->GetNamedDerivedNode("ThresholdOverlay", m_Controls->m_ImageBox->GetSelectedNode()))
    {
      node = this->GetDataStorage()->GetNamedDerivedNode("ThresholdOverlay", m_Controls->m_ImageBox->GetSelectedNode());
    }
    else
    {
      // create a new node, to show thresholded values
      node = mitk::DataNode::New();
      GetDataStorage()->Add( node, m_Controls->m_ImageBox->GetSelectedNode() );
      node->SetProperty( "name", mitk::StringProperty::New("ThresholdOverlay"));
      node->SetBoolProperty("helper object", true);
    }
    node->SetData( mitkImage );
    mitk::RenderingManager::GetInstance()->RequestUpdateAll();
  }
}


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

#include <cmath>
#include <cstdio>
#include <iostream>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>
#include <boost/lexical_cast.hpp>
#include <rl/kin/Kinematics.h>
#include <rl/kin/Puma.h>
#include <rl/math/Rotation.h>
#include <rl/math/Unit.h>

int
main(int argc, char** argv)
{
	if (argc < 8)
	{
		std::cout << "Usage: rlPumaDemo PUMAFILE Q1 Q2 Q3 Q4 Q5 Q6" << std::endl;
		return EXIT_FAILURE;
	}

	try
	{
		std::shared_ptr<rl::kin::Kinematics> kinematics(rl::kin::Kinematics::create(argv[1]));

		if (rl::kin::Puma* puma = dynamic_cast<rl::kin::Puma*>(kinematics.get()))
		{
			rl::math::Vector q(puma->getDof());

			for (std::ptrdiff_t i = 0; i < q.size(); ++i)
			{
				q(i) = boost::lexical_cast<rl::math::Real>(argv[i + 2]) * rl::math::DEG2RAD;
			}

			rl::kin::Puma::Arm arm;
			rl::kin::Puma::Elbow elbow;
			rl::kin::Puma::Wrist wrist;

			puma->parameters(q, arm, elbow, wrist);

			puma->setArm(arm);
			puma->setElbow(elbow);
			puma->setWrist(wrist);

			std::cout
				<< "Arm: " << (arm == rl::kin::Puma::ARM_LEFT ? "LEFT" : "RIGHT")
				<< ", Elbow: " << (elbow == rl::kin::Puma::ELBOW_ABOVE ? "ABOVE" : "BELOW")
				<< ", Wrist: " << (wrist == rl::kin::Puma::WRIST_FLIP ? "FLIP" : "NONFLIP")
				<< std::endl;

			std::cout << "q=" << std::endl << q.transpose() * rl::math::RAD2DEG << std::endl;

			puma->setPosition(q);
			puma->updateFrames();

			const rl::math::Transform::ConstTranslationPart& position = puma->forwardPosition().translation();

			rl::math::Vector3 orientation = puma->forwardPosition().rotation().eulerAngles(2, 1, 0).reverse();

			std::cout << "x=" << std::endl << "x: " << position.x() << " m, y: " << position.y() << " m, z: " << position.z() << " m, a: " << orientation.x() * rl::math::RAD2DEG << " deg, b: " << orientation.y() * rl::math::RAD2DEG << " deg, c: " << orientation.z() * rl::math::RAD2DEG << " deg" << std::endl;

			rl::math::Vector q2(puma->getDof());
			q2.setConstant(1.0 * rl::math::DEG2RAD);

			rl::math::Transform x = puma->forwardPosition();

			if (!puma->inversePosition(x, q2))
			{
				std::cout << "out of reach" << std::endl;
				return EXIT_FAILURE;
			}

			std::cout << "q=" << std::endl << q2.transpose() * rl::math::RAD2DEG << std::endl;

			puma->setPosition(q2);
			puma->updateFrames();

			orientation = puma->forwardPosition().rotation().eulerAngles(2, 1, 0).reverse();

			std::cout << "x=" << std::endl << "x: " << position.x() << " m, y: " << position.y() << " m, z: " << position.z() << " m, a: " << orientation.x() * rl::math::RAD2DEG << " deg, b: " << orientation.y() * rl::math::RAD2DEG << " deg, c: " << orientation.z() * rl::math::RAD2DEG << " deg" << std::endl;
		}
		else
		{
			return EXIT_FAILURE;
		}
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}