/**
* This file is part of DSO.
* 
* Copyright 2016 Technical University of Munich and Intel.
* Developed by Jakob Engel <engelj at in dot tum dot de>,
* for more information see <http://vision.in.tum.de/dso>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* DSO is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* DSO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with DSO. If not, see <http://www.gnu.org/licenses/>.
*/


/*
 * KFBuffer.cpp
 *
 *  Created on: Jan 7, 2014
 *      Author: engelj
 */

#include "FullSystem/FullSystem.h"

#include "stdio.h"
#include "util/globalFuncs.h"
#include <Eigen/LU>
#include <algorithm>
#include "IOWrapper/ImageDisplay.h"
#include "util/globalCalib.h"

#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include <OptimizationBackend/EnergyFunctionalStructs.h>
#include "FullSystem/ImmaturePoint.h"
#include "math.h"

namespace dso {


  PointHessian *FullSystem::optimizeImmaturePoint(
      ImmaturePoint *point, int minObs,
      ImmaturePointTemporaryResidual *residuals) {
    int nres = 0;
    for (FrameHessian *fh : frameHessians) {
      if (fh != point->host) {
        residuals[nres].state_NewEnergy = residuals[nres].state_energy = 0;
        residuals[nres].state_NewState = ResState::OUTLIER;
        residuals[nres].state_state = ResState::IN;
        residuals[nres].target = fh;
        nres++;
      }
    }
    assert(nres == ((int) frameHessians.size()) - 1);

    bool print = false;//rand()%50==0;

    float lastEnergy = 0;
    float lastHdd = 0;
    float lastbd = 0;
    float currentIdepth = (point->idepth_max + point->idepth_min) * 0.5f;


    for (int i = 0; i < nres; i++) {
      lastEnergy += point->linearizeResidual(&Hcalib, 1000, residuals + i, lastHdd, lastbd, currentIdepth);
      residuals[i].state_state = residuals[i].state_NewState;
      residuals[i].state_energy = residuals[i].state_NewEnergy;
    }

    if (!std::isfinite(lastEnergy) || lastHdd < setting_minIdepthH_act) {
      if (print)
        printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",
               nres, lastHdd, lastEnergy);
      return 0;
    }

    if (print)
      printf("Activate point. %d residuals. H=%f. Initial Energy: %f. Initial Id=%f\n",
             nres, lastHdd, lastEnergy, currentIdepth);

    float lambda = 0.1;
    for (int iteration = 0; iteration < setting_GNItsOnPointActivation; iteration++) {
      float H = lastHdd;
      H *= 1 + lambda;
      float step = (1.0 / H) * lastbd;
      float newIdepth = currentIdepth - step;

      float newHdd = 0;
      float newbd = 0;
      float newEnergy = 0;
      for (int i = 0; i < nres; i++)
        newEnergy += point->linearizeResidual(&Hcalib, 1, residuals + i, newHdd, newbd, newIdepth);

      if (!std::isfinite(lastEnergy) || newHdd < setting_minIdepthH_act) {
        if (print)
          printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",
                 nres,
                 newHdd,
                 lastEnergy);
        return 0;
      }

      if (print)
        printf("%s %d (L %.2f) %s: %f -> %f (idepth %f)!\n",
               (true || newEnergy < lastEnergy) ? "ACCEPT" : "REJECT",
               iteration,
               log10(lambda),
               "",
               lastEnergy, newEnergy, newIdepth);

      if (newEnergy < lastEnergy) {
        currentIdepth = newIdepth;
        lastHdd = newHdd;
        lastbd = newbd;
        lastEnergy = newEnergy;
        for (int i = 0; i < nres; i++) {
          residuals[i].state_state = residuals[i].state_NewState;
          residuals[i].state_energy = residuals[i].state_NewEnergy;
        }

        lambda *= 0.5;
      }
      else {
        lambda *= 5;
      }

      if (fabsf(step) < 0.0001 * currentIdepth)
        break;
    }

    if (!std::isfinite(currentIdepth)) {
      printf("MAJOR ERROR! point idepth is nan after initialization (%f).\n", currentIdepth);
      return (PointHessian *) ((long) (-1));    // yeah I'm like 99% sure this is OK on 32bit systems.
    }


    int numGoodRes = 0;
    for (int i = 0; i < nres; i++)
      if (residuals[i].state_state == ResState::IN) numGoodRes++;

    if (numGoodRes < minObs) {
      if (print) printf("OptPoint: OUTLIER!\n");
      return (PointHessian *) ((long) (-1));    // yeah I'm like 99% sure this is OK on 32bit systems.
    }


    PointHessian *ph = new PointHessian(point, &Hcalib);
    if (!std::isfinite(ph->energyTH)) {
      delete ph;
      return (PointHessian *) ((long) (-1));
    }

    ph->lastResiduals[0].first = 0;
    ph->lastResiduals[0].second = ResState::OOB;
    ph->lastResiduals[1].first = 0;
    ph->lastResiduals[1].second = ResState::OOB;
    ph->setIdepthZero(currentIdepth);
    ph->setIdepth(currentIdepth);
    ph->setPointStatus(PointHessian::ACTIVE);

    for (int i = 0; i < nres; i++)
      if (residuals[i].state_state == ResState::IN) {
        PointFrameResidual *r = new PointFrameResidual(ph, ph->host, residuals[i].target);
        r->state_NewEnergy = r->state_energy = 0;
        r->state_NewState = ResState::OUTLIER;
        r->setState(ResState::IN);
        ph->residuals.push_back(r);

        if (r->target == frameHessians.back()) {
          ph->lastResiduals[0].first = r;
          ph->lastResiduals[0].second = ResState::IN;
        }
        else if (r->target == (frameHessians.size() < 2 ? 0 : frameHessians[frameHessians.size() - 2])) {
          ph->lastResiduals[1].first = r;
          ph->lastResiduals[1].second = ResState::IN;
        }
      }

    //- Also add static stereo residual.
    //- Should check out first if the right Frame can see this point.
    //- And if previous do not add a residual, static stereo residual will not be added.
#if defined(STEREO_MODE)
//    if (!ph->residuals.empty()) {

    Mat33f K = Mat33f::Identity();
    K(0, 0) = Hcalib.fxl();
    K(1, 1) = Hcalib.fyl();
    K(0, 2) = Hcalib.cxl();
    K(1, 2) = Hcalib.cyl();

    point->u_stereo = point->u;
    point->v_stereo = point->v;
    point->idepth_min_stereo = 0;
    point->idepth_max_stereo = NAN;

    ImmaturePointStatus traceLeft2RightStatus = point->traceStereo(point->host->rightFrame, K, 1);
    if (traceLeft2RightStatus == ImmaturePointStatus::IPS_GOOD) {
      ImmaturePoint *ipRight = new ImmaturePoint(point->lastTraceUV(0), point->lastTraceUV(1),
                                                 point->host->rightFrame,
                                                 point->my_type, &Hcalib);

      ipRight->u_stereo = ipRight->u;
      ipRight->v_stereo = ipRight->v;
      ipRight->idepth_min_stereo = point->idepth_min = 0;
      ipRight->idepth_max_stereo = point->idepth_max = NAN;

      ImmaturePointStatus traceRight2LeftStatus = ipRight->traceStereo(point->host, K, 0);

      float u_stereo_delta = abs(point->u_stereo - ipRight->lastTraceUV(0));
      float depth = 1.0f / point->idepth_stereo;
      if (traceRight2LeftStatus == ImmaturePointStatus::IPS_GOOD && u_stereo_delta < 1 && depth > 0 &&
          depth < setting_acceptStaticDepthFactor * baseline) {
        PointFrameResidual *r = new PointFrameResidual(ph, ph->host, ph->host->rightFrame);
        r->staticStereo = true;
        r->state_NewEnergy = r->state_energy = 0;
        r->state_NewState = ResState::OUTLIER;
        r->setState(ResState::IN);
        ph->residuals.push_back(r);
      }
      delete ipRight;
    }
//    }
#endif

    if (print) printf("point activated!\n");

    statistics_numActivatedPoints++;
    return ph;
  }
}
