#ifndef   BundleAdjustment_LM_H__
#define   BundleAdjustment_LM_H__

#include "Eye.h"

#include "BundleAdjustment.h"

#include "../math/methods.h"
#include "../math/Vector.h"
#include "../math/Matrix.h"
#include "../math/utility.h"

#include "../oo/ObjBase.h"

#include <algorithm>

namespace meow {

template<class Pixel>
class BundleAdjustment_LM: public BundleAdjustment<Pixel> {
private:
  struct MatchPair {
    size_t         from_i;
    Matrix<double> from_m;
    size_t         to_i;
    Matrix<double> to_m;
  };
  
  class NoOffsetController {
  private:
    std::vector<MatchPair               >* pairs_;
    std::vector<Rotation3D     <double> >* rot_;
    std::vector<PhotoProjection<double> >* pho_;
    BallProjection<double>                 bll_;
    bool flag;
  public:
    NoOffsetController(std::vector<MatchPair               >* pairs,
                       std::vector<Rotation3D<double>      >* rot,
                       std::vector<PhotoProjection<double> >* pho,
                       bool fg):
    pairs_(pairs), rot_(rot), pho_(pho), bll_(3.0, 1.0), flag(fg) {
    }
    Vector<double> init() {
      if (flag == false) {
        Vector<double> v(4, 0.0);
        int i = (*pho_).size() - 1, n = 0;
        v.entry(n++, (*pho_)[i].focal());
        v.entry(n++, (*rot_)[i].theta(0));
        v.entry(n++, (*rot_)[i].theta(1));
        v.entry(n++, (*rot_)[i].theta(2));
        return v;
      }
      Vector<double> v((*rot_).size() * 4, 0.0);
      for (size_t n = 0, i = 0, I = (*rot_).size(); i < I; ++i) {
        v.entry(n++, (*pho_)[i].focal());
        v.entry(n++, (*rot_)[i].theta(0));
        v.entry(n++, (*rot_)[i].theta(1));
        v.entry(n++, (*rot_)[i].theta(2));
      }
      return v;
    }
    Vector<double> residure(Vector<double> const& v) {
      for (size_t n = 0, i = 0, I = (*rot_).size(); i < I; ++i) {
        if (flag == false && i != I - 1) continue;
        (*pho_)[i].focal(v(n++));
        (*rot_)[i].theta(0, v(n++));
        (*rot_)[i].theta(1, v(n++));
        (*rot_)[i].theta(2, v(n++));
      }
      for (typename std::vector<MatchPair>::iterator
           it = (*pairs_).begin(), ed = (*pairs_).end(); it != ed; ++it) {
        (*it).from_m.entry(2, 0, -(*pho_)[(*it).from_i].focal());
        (*it).  to_m.entry(2, 0, -(*pho_)[(*it).  to_i].focal());
      }
      //
      Vector<double> ret((*pairs_).size() * 2, 0.0);
      size_t n = 0;
      for (typename std::vector<MatchPair>::iterator
           it = (*pairs_).begin(), ed = (*pairs_).end(); it != ed; ++it, ++n) {
        Matrix<double> tr(
          (*it).to_m
          -
          (*pho_)[(*it).to_i].transformate(
            (*rot_)[(*it).to_i].transformate(
              (*rot_)[(*it).from_i].transformateInv(
                bll_.transformate(
                  (*it).from_m
                )
              )
            )
          )
        );
        ret.entry(n * 2    , tr(0, 0));
        ret.entry(n * 2 + 1, tr(1, 0));
      }
      //*
      //for (size_t i = 0; i < ret.dimension(); i++) {
      //  printf("(%-9.1f ", ret(i));
      //  if (i % 8 == 7) printf("\n");
      //}
      //printf("\n");
      printf("re = %20.5f ?? %f\n", ret.length2(), 25.0 * (*pairs_).size());
      // */
      return ret;
    }
    Matrix<double> jacobian() const {
      Matrix<double> ret;
      if (flag == false) {
        ret.reset((*pairs_).size() * 2, 4, 0.0);
      }
      else {
        ret.reset((*pairs_).size() * 2, (*rot_).size() * 4, 0.0);
      }
      size_t n = 0;
      for (typename std::vector<MatchPair>::iterator
           it = (*pairs_).begin(), ed = (*pairs_).end(); it != ed; ++it, ++n) {
        Matrix<double> tr_A(  bll_               .transformate((*it).from_m));
        Matrix<double> tr_B((*rot_)[(*it).from_i].transformateInv(tr_A));
        Matrix<double> tr_C((*rot_)[(*it).  to_i].transformate   (tr_B));
        Matrix<double> ja_A((*pho_)[(*it).  to_i].jacobian   (tr_C));
        Matrix<double> ja_B((*rot_)[(*it).  to_i].jacobian   (tr_B));
        Matrix<double> ja_C((*rot_)[(*it).from_i].jacobianInv(tr_A));
        Matrix<double> m;
        int fr = (flag == false ? 0 : (*it).from_i);
        int to = (flag == false ? 0 : (*it).  to_i);
        if (flag != false || (*it).from_i == (*rot_).size() - 1) {
          m = ja_A * ja_B * ja_C * bll_.jacobian((*it).from_m).col(2);
          ret.entry(n * 2    , fr * 4, m(0, 0));
          ret.entry(n * 2 + 1, fr * 4, m(1, 0));
          for (size_t k = 0; k < 3; ++k) {
            m = ja_A * ja_B * (*rot_)[(*it).from_i].jacobianInv(tr_A, k);
            ret.entry(n * 2    , fr * 4 + 1 + k, m(0, 0));
            ret.entry(n * 2 + 1, fr * 4 + 1 + k, m(1, 0));
          }
        }
        if (flag != false || (*it).to_i == (*rot_).size() - 1) {
          m = (*pho_)[(*it).to_i].jacobian(tr_C, 0);
          ret.entry(n * 2    , to * 4, m(0, 0));
          ret.entry(n * 2 + 1, to * 4, m(1, 0));
          for (size_t k = 0; k < 3; ++k) {
            m = ja_A * (*rot_)[(*it).to_i].jacobian(tr_B, k);
            ret.entry(n * 2    , to * 4 + 1 + k, m(0, 0));
            ret.entry(n * 2 + 1, to * 4 + 1 + k, m(1, 0));
          }
        }
      }
      return ret;
    }
    Matrix<double> identity() const {
      if (flag == false) {
        Matrix<double> ret(4, 4, 0.0);
        ret.identitied();
        return ret;
      }
      Matrix<double> ret((*rot_).size() * 4, (*rot_).size() * 4, 0.0);
      ret.identitied();
      return ret;
    }
  };

  struct Myself {
    double t_;

    Myself(): t_(5.0) {
    }

    Myself(Myself const& m): t_(m.t_) {
    }

    ~Myself() {
    }
  };

  Self<Myself> const self;

public:
  BundleAdjustment_LM(): self() {
  }

  BundleAdjustment_LM(BundleAdjustment_LM const& b):
  self(b.self, Self<Myself>::COPY_FROM) {
  }

  ~BundleAdjustment_LM() {
  }

  BundleAdjustment_LM& copyFrom(BundleAdjustment_LM const& b) {
    self().copyFrom(b.self);
    return *this;
  }

  BundleAdjustment_LM& referenceFrom(BundleAdjustment_LM const& b) {
    self().referenceFrom(b.self);
    return *this;
  }

  double threshold() const {
    return self->t_;
  }

  double threshold(double t) {
    self()->t_ = t;
    return threshold();
  }

  bool adjustEye(std::vector<SceneInfo<Pixel> >* seq) const {
    // check
    size_t N = (*seq).size();
    for (size_t i = 0; i < N; ++i) {
      if (  (*seq)[i].flag & CAN_OFFSET ) return false;
      if (!((*seq)[i].flag & CAN_ROTATE)) return false;
      if (!((*seq)[i].flag & CAN_ZOOM  )) return false;
    }
    // get all pairs
    typedef typename Camera<Pixel>::FixedPoints2D::IdentityPointsMapIterK FPS_K;
    std::vector<FPS_K> beg(N), end(N);
    FPS_K it1, it2;
    for (size_t i = 0; i < N; ++i) {
      beg[i] = (*seq)[i].eye->camera().fixedPoints2D().identityPoints().begin();
      end[i] = (*seq)[i].eye->camera().fixedPoints2D().identityPoints().end  ();
    }
    std::vector<std::vector<std::vector<MatchPair> > > all_pairs(N);
    for (size_t i = 0; i < N; ++i) {
      all_pairs[i].resize(N);
      for (size_t j = 0; j < N; ++j) {
        if (i == j) continue;
        for (it1 = beg[i], it2 = beg[j]; it1 != end[i] && it2 != end[j]; ) {
          if     (it1->first < it2->first) ++it1;
          else if(it1->first > it2->first) ++it2;
          else {
            MatchPair tmp;
            tmp.from_i = i;
            tmp.from_m = it1->second.matrix();
            tmp.from_m.rows(3, -(*seq)[i].eye->camera().photo().focal());
            tmp.  to_i = j;
            tmp.  to_m = it2->second.matrix();
            tmp.  to_m.rows(3, -(*seq)[j].eye->camera().photo().focal());
            all_pairs[i][j].push_back(tmp);
            ++it1;
            ++it2;
          }
        }
      }
    }
    //
    std::vector<MatchPair               > pairs;
    std::vector<Rotation3D     <double> > rot;
    std::vector<PhotoProjection<double> > pho;
    std::vector<int                     > sum (N, 0);
    std::vector<int                     > big (N, 0);
    std::vector<int                     > real(N);
    std::vector<int                     > rev (N);
    // first!!
    int best = -1, best_ct = -1;
    for (size_t i = 0; i < N; ++i) {
      int d = 0;
      for (size_t j = 0; j < N; ++j) {
        d += all_pairs[i][j].size();
      }
      if (best_ct < d) {
        best_ct = d;
        best = i;
      }
      real[i] = i;
      big [i] = i;
    }
    for (size_t i = 0; i < N; ++i) {
      // update else
      real[i] = best;
      rev[best] = i;
      for (size_t j = 0; j < N; ++j) {
        if ((int)j == best) {
          sum[j] = -1;
          continue;
        }
        if (sum[j] < 0) continue;
        sum[j] += all_pairs[best][j].size();
        if (all_pairs[j][big[j]].size() < all_pairs[j][best].size())
          big[j] = best;
      }
      // add me
      printf("%d  same as %d\n", best, big[best]);
      if (big[best] == best) {
        rot.push_back((*seq)[big[best]].eye->camera()        .rotation());
        pho.push_back((*seq)[big[best]].eye->camera().photo().projection());
      }
      else {
        rot.push_back(rot[rev[big[best]]]);
        pho.push_back(pho[rev[big[best]]]);
      }
      for (size_t j = 0; j < N; ++j) {
        if (sum[j] >= 0) continue;
        for (size_t k = 0, K = all_pairs[best][j].size(); k < K; ++k) {
          all_pairs[best][j][k].from_i = rev[all_pairs[best][j][k].from_i];
          all_pairs[best][j][k].  to_i = rev[all_pairs[best][j][k].  to_i];
          pairs.push_back(all_pairs[best][j][k]);
        }
        for (size_t k = 0, K = all_pairs[j][best].size(); k < K; ++k) {
          all_pairs[j][best][k].from_i = rev[all_pairs[j][best][k].from_i];
          all_pairs[j][best][k].  to_i = rev[all_pairs[j][best][k].  to_i];
          pairs.push_back(all_pairs[j][best][k]);
        }
      }
      for (size_t j = 0; j < N; ++j) { printf("%4d ",  sum[j]); } printf("\n");
      for (size_t j = 0; j < N; ++j) { printf("%4d ",  big[j]); } printf("\n");
      for (size_t j = 0; j < N; ++j) { printf("%4d ", real[j]); } printf("\n");
      printf("\n");
      // bundle adjustment
      if (i > 0) {
        NoOffsetController ct1(&pairs, &rot, &pho, false);
        ct1.residure(levenbergMarquardtTraining(ct1, ct1.init(),
                                                7.3, 1.0,
                                                squ(threshold()) * pairs.size(),
                                                7, 1000));
        /*
        NoOffsetController ct2(&pairs, &rot, &pho, true);
        ct2.residure(levenbergMarquardtTraining(ct2, ct2.init(),
                                                0.1 * pairs.size(), 1.0,
                                                squ(threshold()) * pairs.size(),
                                                7, 1000));
        // */
      }
      // find next
      best_ct = 0;
      for (size_t j = 0; j < N; ++j) {
        if (best_ct < sum[j]) {
          best    = j;
          best_ct = sum[j];
        }
      }
      getchar();
      //if (i == N - 2) break;
    }
    // feedback
    for (size_t i = 0; i < N; ++i) {
      int id = real[i];
      (*seq)[id].eye->cameraGet()           .rotation  (rot[i]);
      (*seq)[id].eye->cameraGet().photoGet().projection(pho[i]);
      //if (i == N - 2) break;
    }
    return true;
  }

  bool adjustFixedPoint(std::vector<SceneInfo<Pixel> >* seq) const {
    return false;
  }

  bool write(FILE* f, bool bin, unsigned int fg) const {
    return false;
  }

  bool read(FILE* f, bool bin, unsigned int fg) const {
    return false;
  }

  ObjBase* create() const {
    return new BundleAdjustment_LM;
  }

  ObjBase* copyFrom(ObjBase const* o) {
    return &(copyFrom(*(BundleAdjustment_LM const*)o));
  }

  char const* ctype() const {
    return typeid(*this).name();
  }

  std::string type() const {
    return std::string(ctype());
  }
};

} // meow

#endif // BundleAdjustment_LM_H__