#include <string>
#include <array>
#include <vector>
#include <set>
#include <map>
#include <queue>
#include <stack>
#include <sstream>
#include <cstdint>
#include <ostream>
#include <bitset>
#include <algorithm>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "Action.hpp"
#include "UnionFind.hpp"
#include "DirectedGraph.hpp"
#include "Partition.hpp"

#ifndef STRATEGY_N3M5_HPP
#define STRATEGY_N3M5_HPP

class StrategyN3M5;

typedef std::bitset<5> HistoM5;

class StateN3M5 {
 public:
  StateN3M5(const HistoM5 &a_histo, const HistoM5 &b_histo, const HistoM5 &c_histo) :
      ha(a_histo), hb(b_histo), hc(c_histo) {};
  StateN3M5(const std::string &str) { // format "ccccc_dddcc_cdccd" (a0,a1,a2,...c3,c4) last action comes first
    assert(str.size() == 17 && str[5] == '_' && str[11] == '_');
    std::string sa = str.substr(0, 5), sb = str.substr(6, 5), sc = str.substr(12, 5);
    auto s_to_binary = [](std::string s)->HistoM5 {
      std::reverse(s.begin(), s.end());
      std::replace(s.begin(), s.end(), 'c', '0');
      std::replace(s.begin(), s.end(), 'd', '1');
      return HistoM5(s);
    };
    ha = s_to_binary(sa); hb = s_to_binary(sb); hc = s_to_binary(sc);
  }
  StateN3M5(uint64_t i) { // (c4,c3,c2,....,a1,a0) the lowest bit is a0
    uint64_t mask = 31ull;
    ha = HistoM5(i & mask); hb = HistoM5((i>>5ul)&mask); hc = HistoM5((i>>10)&mask);
  }
  HistoM5 ha, hb, hc;

  bool operator==(const StateN3M5 &rhs) const {
    return (ha == rhs.ha && hb == rhs.hb && hc == rhs.hc);
  }
  friend std::ostream &operator<<(std::ostream &os, const StateN3M5 &s) {
    auto to_s = [](const HistoM5 &h)->std::string {
      std::string s = h.to_string();
      std::reverse(s.begin(), s.end());
      std::replace(s.begin(), s.end(), '0', 'c');
      std::replace(s.begin(), s.end(), '1', 'd');
      return s;
    };
    os << to_s(s.ha) << '_' << to_s(s.hb) << '_' << to_s(s.hc);
    return os;
  };

  StateN3M5 NextState(Action act_a, Action act_b, Action act_c) const {
    HistoM5 nha = ha << 1;
    HistoM5 nhb = hb << 1;
    HistoM5 nhc = hc << 1;
    if (act_a == D) { nha.set(0); }
    if (act_b == D) { nhb.set(0); }
    if (act_c == D) { nhc.set(0); }
    return StateN3M5(nha, nhb, nhc);
  };

  std::vector<StateN3M5> PossiblePrevStates() const {
    std::vector<StateN3M5> ans;
    for (size_t i = 0; i < 2; i++) {
      for (size_t j = 0; j < 2; j++) {
        for (size_t k = 0; k < 2; k++) {
          HistoM5 lha = ha >> 1;
          HistoM5 lhb = hb >> 1;
          HistoM5 lhc = hc >> 1;
          if (i == 1) { lha.set(4); }
          if (j == 1) { lhb.set(4); }
          if (k == 1) { lhc.set(4); }
          ans.emplace_back(lha, lhb, lhc);
        }
      }
    }
    return std::move(ans);
  }

  int RelativePayoff(bool against_B = true) const {
    bool a0 = ha[0];
    bool b0 = against_B ? hb[0] : hc[0];
    if (a0 == false && b0 == true) { return -1; } // C,D
    else if (a0 == true && b0 == false) { return 1; } // D,C
    else if (a0 == b0) { return 0; }  // C,C or D,D
    else {
      assert(false);
      return -10000;
    }
  }

  StateN3M5 StateFromB() const { return StateN3M5(hb, hc, ha); } // state from B's viewpoint
  StateN3M5 StateFromC() const { return StateN3M5(hc, ha, hb); } // state from C's viewpoint

  std::array<StateN3M5, 3> NoisedStates() const {
    HistoM5 ha_n = ha, hb_n = hb, hc_n = hc;
    ha_n ^= HistoM5(1ull);
    hb_n ^= HistoM5(1ull);
    hc_n ^= HistoM5(1ull);
    std::array<StateN3M5, 3> ans = {StateN3M5(ha_n, hb, hc), StateN3M5(ha, hb_n, hc), StateN3M5(ha, hb, hc_n)};
    return ans;
  }

  int NumDiffInT1(const StateN3M5 &other) const {
    const auto b1 = ToBits();
    const auto b2 = other.ToBits();

    std::bitset<15> mask("111101111011110");
    if( (b1 & mask) != (b2 & mask) ) { // inconsistent bit is found
      return -1;
    } else {
      return ((b1 & ~mask) ^ (b2 & ~mask)).count(); // number of different bits
    }
  }

  std::string ToString() const {
    std::ostringstream os;
    os << *this;
    return os.str();
  }

  std::bitset<15> ToBits() const {
    std::bitset<15> bits(0ull);
    bits ^= ha.to_ullong();
    bits ^= (hb.to_ullong() << 5);
    bits ^= (hc.to_ullong() <<10);
    return bits;
  }

  uint64_t ID() const {
    return ToBits().to_ullong();
  }

  bool operator<(const StateN3M5 &rhs) const {
    return (ID() < rhs.ID());
  }
};

class StrategyN3M5 {
 public:
  static const size_t N = 1ull << 15ull; // == 32768
  StrategyN3M5(const std::bitset<N> &actions); // construct a strategy from a list of actions. 0=>c,1=>d
  StrategyN3M5 &operator=(const StrategyN3M5 & rhs) = default;

  std::string ToString() const;
  friend std::ostream &operator<<(std::ostream &os, const StrategyN3M5 &strategy);
  bool operator==(const StrategyN3M5 &rhs) const {
    for (size_t i = 0; i < 64; i++) { if (actions[i] != rhs.actions[i]) return false; }
    return true;
  }

  Action ActionAt(const StateN3M5 &s) const { return actions[s.ID()] ? D : C; }
  bool IsDefensible() const;  // check defensibility. Not computationally feasible.
  bool IsDefensibleDFA() const; // check defensibility using DFA minimization
  // get stationary state. When coplayer is nullptr, it is set to self
  std::array<double, N> StationaryState(double e = 0.0001, const StrategyN3M5 *B = nullptr, const StrategyN3M5 *C = nullptr) const;
  // std::array<double, N> StationaryState2(double e = 0.0001, const StrategyN3M5 *B = nullptr, const StrategyN3M5 *C = nullptr) const;
  // check efficiency. all actions must be fixed
  bool IsEfficient(double e = 0.00001, double th = 0.95) const { return (StationaryState(e)[0] > th); }
  bool IsEfficientTopo() const; // check efficiency using ITG
  bool IsDistinguishable(double e = 0.00001, double th = 0.95) const {
    const StrategyN3M5 allc(std::bitset<N>(0ull));
    return (StationaryState(e, &allc, &allc)[0] < th);
    // return (StationaryState(e, this, &allc)[0] < th);
  };  // check distinguishability against AllC
  bool IsDistinguishableTopo() const; // check distinguishability using the transition graph
  DirectedGraph ITG() const;  // construct g(S,S).
  std::array<uint64_t , StrategyN3M5::N> DestsOfITG() const; // Trace g(S,S) from node i. Destination is stored in i'th element.
  uint64_t NextITGState(const StateN3M5 &s) const; // Trace the intra-transition graph by one step
  std::vector<uint64_t> TraceStates(uint64_t start, const StrategyN3M5 *B = nullptr, const StrategyN3M5 *C = nullptr) const;
  UnionFind MinimizeDFA(bool noisy = false) const; // DFA minimization using brute-force method. Computationally demanding
  Partition MinimizeDFAHopcroft(bool noisy = false) const;

  static StrategyN3M5 AllC();
  static StrategyN3M5 AllD();
  static StrategyN3M5 TFT();
  static StrategyN3M5 WSLS();
  static StrategyN3M5 AON(size_t m);
  static StrategyN3M5 FUSS_m3();
  static StrategyN3M5 CAPRI3();
  static StrategyN3M5 sCAPRI3();
 private:
  std::bitset<N> actions;
  std::vector<StateN3M5> NextPossibleStates(StateN3M5 current) const;
  bool _Equivalent(size_t i, size_t j, UnionFind &uf_0, bool noisy) const;
  typedef std::pair<size_t, int> splitter_t;
  std::array<std::set<size_t>,2> _SplitBySplitter(const Partition &partition, size_t org, const std::set<size_t> &Q, int b, bool noisy) const;
};

#endif //STRATEGY_N3M5_HPP


// Templated generic hybrid sorting

//          Copyright Steven J. Ross 2001 - 2009.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org/libs/sort/ for library home page.

/*
Some improvements suggested by:
Phil Endecott and Frank Gennari
float_mem_cast fix provided by:
Scott McMurray
*/

#ifndef BOOST_SORT_SPREADSORT_HPP
#define BOOST_SORT_SPREADSORT_HPP
#include <algorithm>
#include <vector>
#include <cstring>
#include <string>
#include <limits>
#include <boost/type_traits.hpp>
#include <boost/sort/spreadsort/integer_sort.hpp>
#include <boost/sort/spreadsort/float_sort.hpp>
#include <boost/sort/spreadsort/string_sort.hpp>

namespace boost {
namespace sort {

/*! Namespace for spreadsort sort variants for different data types.
\note Use hyperlinks (coloured) to get detailed information about functions.
*/
namespace spreadsort {

  /*!
    \brief Generic @c spreadsort variant detecting integer-type elements so call to @c integer_sort.
    \details If the data type provided is an integer, @c integer_sort is used.
    \note Sorting other data types requires picking between @c integer_sort, @c float_sort and @c string_sort directly,
    as @c spreadsort won't accept types that don't have the appropriate @c type_traits.
    \param[in] first Iterator pointer to first element.
    \param[in] last Iterator pointing to one beyond the end of data.

    \pre [@c first, @c last) is a valid range.
    \pre @c RandomAccessIter @c value_type is mutable.
    \pre @c RandomAccessIter @c value_type is LessThanComparable
    \pre @c RandomAccessIter @c value_type supports the @c operator>>,
    which returns an integer-type right-shifted a specified number of bits.
    \post The elements in the range [@c first, @c last) are sorted in ascending order.
  */

  template <class RandomAccessIter>
  inline typename boost::enable_if_c< std::numeric_limits<
    typename std::iterator_traits<RandomAccessIter>::value_type >::is_integer,
    void >::type
  spreadsort(RandomAccessIter first, RandomAccessIter last)
  {
    integer_sort(first, last);
  }

  /*!
    \brief Generic @c spreadsort variant detecting float element type so call to @c float_sort.
    \details If the data type provided is a float or castable-float, @c float_sort is used.
    \note Sorting other data types requires picking between @c integer_sort, @c float_sort and @c string_sort directly,
    as @c spreadsort won't accept types that don't have the appropriate @c type_traits.

    \param[in] first Iterator pointer to first element.
    \param[in] last Iterator pointing to one beyond the end of data.

    \pre [@c first, @c last) is a valid range.
    \pre @c RandomAccessIter @c value_type is mutable.
    \pre @c RandomAccessIter @c value_type is LessThanComparable
    \pre @c RandomAccessIter @c value_type supports the @c operator>>,
    which returns an integer-type right-shifted a specified number of bits.
    \post The elements in the range [@c first, @c last) are sorted in ascending order.
  */

  template <class RandomAccessIter>
  inline typename boost::enable_if_c< !std::numeric_limits<
    typename std::iterator_traits<RandomAccessIter>::value_type >::is_integer
    && std::numeric_limits<
    typename std::iterator_traits<RandomAccessIter>::value_type >::is_iec559,
    void >::type
  spreadsort(RandomAccessIter first, RandomAccessIter last)
  {
    float_sort(first, last);
  }

  /*!
    \brief  Generic @c spreadsort variant detecting string element type so call to @c string_sort for @c std::strings and @c std::wstrings.
    \details If the data type provided is a string or wstring, @c string_sort is used.
    \note Sorting other data types requires picking between @c integer_sort, @c float_sort and @c string_sort directly,
    as @c spreadsort won't accept types that don't have the appropriate @c type_traits.

    \param[in] first Iterator pointer to first element.
    \param[in] last Iterator pointing to one beyond the end of data.

    \pre [@c first, @c last) is a valid range.
    \pre @c RandomAccessIter @c value_type is mutable.
    \pre @c RandomAccessIter @c value_type is LessThanComparable
    \pre @c RandomAccessIter @c value_type supports the @c operator>>,
    which returns an integer-type right-shifted a specified number of bits.
    \post The elements in the range [@c first, @c last) are sorted in ascending order.
  */

  template <class RandomAccessIter>
  inline typename boost::enable_if_c<
    is_same<typename std::iterator_traits<RandomAccessIter>::value_type,
            typename std::string>::value ||
    is_same<typename std::iterator_traits<RandomAccessIter>::value_type,
            typename std::wstring>::value, void >::type
  spreadsort(RandomAccessIter first, RandomAccessIter last)
  {
    string_sort(first, last);
  }
} // namespace spreadsort
} // namespace sort
} // namespace boost

#endif


#include "globals/system_utils.h"
#include "core/depth_utils.h"
#include "core/nn_aligner.h"
#include "cloud_viewer.h"

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <iostream>
#include <Eigen/Core>

#include <limits>
#include <deque>
#include <queue>
#include <vector>
#include <fstream>
#include <qevent.h>
#include "qglviewer.h"
#include "qapplication.h"

#include <fstream>

using namespace std;
using namespace Eigen;
using namespace fps_mapper;
using namespace system_utils;

class AlignerViewer: public CloudViewer {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  AlignerViewer(){
  }

  void keyPressEvent(QKeyEvent *e) {
    if (e->key()==Qt::Key_X){
      align();
      return;
    }
    if (e->key()==Qt::Key_P){
      cerr << "transforms: " << endl;
      for (CloudIsometryMap::iterator it = _clouds.begin(); it!=_clouds.end(); it++) {
	cerr << it->first << " -> " << t2v(it->second).transpose() << endl;
      }
      return;
    }
    CloudViewer::keyPressEvent(e);
  }

  void align(){
    if (_selected_objects.size()!=2) {
      cerr << "the objects should be exactly two";
      return;
    }

    std::set<const Cloud*>:: iterator it = _selected_objects.begin();
    Cloud reference = *(*it);
    Eigen::Isometry3f& reference_transform = _clouds[*it];
    it++;
    Cloud current = *(*it);
    Cloud curr_cloud = *(*it);
    Eigen::Isometry3f& current_transform = _clouds[*it];

    voxelize(reference, 0.05);
    voxelize(current, 0.05);

    // determine the transform between reference and current
    Eigen::Isometry3f dt = reference_transform.inverse() * current_transform;

    aligner.setReferenceModel(&reference);
    aligner.setCurrentModel(&current);
    aligner.setIterations(10);
    aligner.align(dt);
    aligner.solver().setDamping(1e3);
    aligner.solver().setMaxError(0.01);
    current_transform = reference_transform*aligner.T();
    updateGL();

    curr_cloud.transformInPlace(current_transform);
    ofstream os("refined.cloud");
    curr_cloud.write(os);
    printErrorStats();
  }

  void printErrorStats() {
    const std::vector<float>& errors = aligner.solver().errors();
    int inliers = 0;
    double inliers_error_sum=0;
    double outliers_error_sum=0;
    int outliers = 0;
    for (size_t i = 0; i<errors.size(); i++){
      if (errors [i] <0 ){
	outliers++;
	outliers_error_sum -= errors[i];
	continue;
      } else {
	inliers++;
	inliers_error_sum += errors[i];
      }
    }
    cerr << "solver.max_error: " << aligner.solver().maxError() << endl;
    cerr << "inliers : " << inliers << endl;
    cerr << "outliers: " << outliers << endl;
    cerr << "error/inliers: " << inliers_error_sum/inliers << endl;
    cerr << "error/outliers: " << outliers_error_sum/outliers << endl;
  }

  NNAligner aligner;
};

const char* banner[]= {
  "fps_aligner_gui_app",
  "allows to align a set of clouds",
  "usage:",
  " fps_aligner_gui_app <clouds>",
  "",
  " once the giu has started",
  " shift-click selects a cloud",
  " M toggles the move cloud mode with cursor and page up keys (ctrl to rotate)",
  " X aligns two clouds",
  " P prints the relative transforms of the clouds",
  0
};

int main(int argc, char **argv) {
  std::list<Cloud*> clouds;
  if (argc<2 || ! strcmp(argv[1],"-h")){
    printBanner(banner);
    return 0;
  }

  int c = 1;
  while (c<argc){
    Cloud* cloud=new Cloud;
    ifstream is(argv[c]);
    cloud->read(is);
    clouds.push_back(cloud);
    cerr << "loaded cloud [" << argv[c] << "] with " << cloud->size() << "] points" << endl;
    c++;
  }
  QApplication app(argc, argv);
  AlignerViewer viewer;
  for (std::list<Cloud*>::iterator it=clouds.begin(); it!=clouds.end(); it++)
    viewer.addCloud(*it);
  viewer.show();
  app.exec();
}