/*
 * gtest_problem.cpp
 *
 *  Created on: Nov 23, 2016
 *      Author: jsola
 */

#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"

#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
#include "core/sensor/sensor_odom_2D.h"
#include "core/sensor/sensor_odom_3D.h"
#include "core/processor/processor_odom_3D.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"

#include <iostream>

using namespace wolf;
using namespace Eigen;


// Register in the ProcessorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
} // namespace wolf


WOLF_PTR_TYPEDEFS(DummySolverManager);

class DummySolverManager : public SolverManager
{
public:
  DummySolverManager(const ProblemPtr& _problem)
    : SolverManager(_problem)
  {
    //
  }
  virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
  virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
  virtual bool hasConverged(){return true;};
  virtual SizeStd iterations(){return 0;};
  virtual Scalar initialCost(){return 0;};
  virtual Scalar finalCost(){return 0;};
  virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
  virtual void addFactor(const FactorBasePtr& fac_ptr){};
  virtual void removeFactor(const FactorBasePtr& fac_ptr){};
  virtual void addStateBlock(const StateBlockPtr& state_ptr){};
  virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
  virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
  virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
  virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
  virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
  virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
  virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
};

TEST(Problem, create)
{
    ProblemPtr P = Problem::create("POV", 3);

    // check double pointers to branches
    ASSERT_EQ(P, P->getHardware()->getProblem());
    ASSERT_EQ(P, P->getTrajectory()->getProblem());
    ASSERT_EQ(P, P->getMap()->getProblem());

    // check frame structure through the state size
    ASSERT_EQ(P->getFrameStructureSize(), 10);
}

TEST(Problem, Sensors)
{
    ProblemPtr P = Problem::create("POV", 3);

    // add a dummy sensor
    // SensorBasePtr S = std::make_shared<SensorBase>("Dummy", nullptr, nullptr, nullptr, 2, false);
    // P->addSensor(S);
    auto S = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);

    // check pointers
    ASSERT_EQ(P, S->getProblem());
    ASSERT_EQ(P->getHardware(), S->getHardware());

}

TEST(Problem, Processor)
{
    ProblemPtr P = Problem::create("PO", 3);

    // check motion processor is null
    ASSERT_FALSE(P->getProcessorMotion());

    // add a motion sensor and processor
    // SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics
    // P->addSensor(Sm);
    auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D());

    // add motion processor
    // ProcessorMotionPtr Pm = std::make_shared<ProcessorOdom3D>(std::make_shared<ProcessorParamsOdom3D>());
    // Sm->addProcessor(Pm);
    auto Pm = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>()));

    // check motion processor IS NOT by emplace
    ASSERT_EQ(P->getProcessorMotion(), Pm);
}

TEST(Problem, Installers)
{
    std::string wolf_root = _WOLF_ROOT_DIR;
    ProblemPtr P = Problem::create("PO", 3);
    Eigen::Vector7s xs;

    SensorBasePtr    S = P->installSensor   ("ODOM 3D", "odometer",        xs,         wolf_root + "/test/yaml/sensor_odom_3D.yaml");

    // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
    auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer");

    // check motion processor IS NOT set
    ASSERT_FALSE(P->getProcessorMotion());

    // install processor motion
    ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");

    // check motion processor is set
    ASSERT_TRUE(P->getProcessorMotion());

    // check motion processor is correct
    ASSERT_EQ(P->getProcessorMotion(), pm);
}

TEST(Problem, SetOrigin_PO_2D)
{
    ProblemPtr P = Problem::create("PO", 2);
    TimeStamp       t0(0);
    Eigen::VectorXs x0(3); x0 << 1,2,3;
    Eigen::MatrixXs P0(3,3); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id

    P->setPrior(x0, P0, t0, 1.0);

    // check that no sensor has been added
    ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);

    // check that the state is correct
    ASSERT_MATRIX_APPROX(x0, P->getCurrentState(), Constants::EPS_SMALL );

    // check that we have one frame, one capture, one feature, one factor
    TrajectoryBasePtr T = P->getTrajectory();
    ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
    FrameBasePtr F = P->getLastFrame();
    ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
    CaptureBasePtr C = F->getCaptureList().front();
    ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
    FeatureBasePtr f = C->getFeatureList().front();
    ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);

    // check that the factor is absolute (no pointers to other F, f, or L)
    FactorBasePtr c = f->getFactorList().front();
    ASSERT_FALSE(c->getFrameOther());
    ASSERT_FALSE(c->getLandmarkOther());

    // check that the Feature measurement and covariance are the ones provided
    ASSERT_MATRIX_APPROX(x0, f->getMeasurement(), Constants::EPS_SMALL );
    ASSERT_MATRIX_APPROX(P0, f->getMeasurementCovariance(), Constants::EPS_SMALL );

    //    P->print(4,1,1,1);
}

TEST(Problem, SetOrigin_PO_3D)
{
    ProblemPtr P = Problem::create("PO", 3);
    TimeStamp       t0(0);
    Eigen::VectorXs x0(7); x0 << 1,2,3,4,5,6,7;
    Eigen::MatrixXs P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id

    P->setPrior(x0, P0, t0, 1.0);

    // check that no sensor has been added
    ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);

    // check that the state is correct
    ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));

    // check that we have one frame, one capture, one feature, one factor
    TrajectoryBasePtr T = P->getTrajectory();
    ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
    FrameBasePtr F = P->getLastFrame();
    ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
    CaptureBasePtr C = F->getCaptureList().front();
    ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
    FeatureBasePtr f = C->getFeatureList().front();
    ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);

    // check that the factor is absolute (no pointers to other F, f, or L)
    FactorBasePtr c = f->getFactorList().front();
    ASSERT_FALSE(c->getFrameOther());
    ASSERT_FALSE(c->getLandmarkOther());

    // check that the Feature measurement and covariance are the ones provided
    ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
    ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL));

    //    P->print(4,1,1,1);
}

TEST(Problem, emplaceFrame_factory)
{
    ProblemPtr P = Problem::create("PO", 2);

    FrameBasePtr f0 = P->emplaceFrame("PO", 2,    KEY, VectorXs(3),  TimeStamp(0.0));
    FrameBasePtr f1 = P->emplaceFrame("PO", 3,    KEY, VectorXs(7),  TimeStamp(1.0));
    FrameBasePtr f2 = P->emplaceFrame("POV", 3,   KEY, VectorXs(10), TimeStamp(2.0));

    //    std::cout << "f0: type PO 2D?    "  << f0->getType() << std::endl;
    //    std::cout << "f1: type PO 3D?    "  << f1->getType() << std::endl;
    //    std::cout << "f2: type POV 3D?   "  << f2->getType() << std::endl;

    ASSERT_EQ(f0->getType().compare("PO 2D"), 0);
    ASSERT_EQ(f1->getType().compare("PO 3D"), 0);
    ASSERT_EQ(f2->getType().compare("POV 3D"), 0);

    // check that all frames are effectively in the trajectory
    ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (SizeStd) 3);

    // check that all frames are linked to Problem
    ASSERT_EQ(f0->getProblem(), P);
    ASSERT_EQ(f1->getProblem(), P);
    ASSERT_EQ(f2->getProblem(), P);
}

TEST(Problem, StateBlocks)
{
    std::string wolf_root = _WOLF_ROOT_DIR;

    ProblemPtr P = Problem::create("PO", 3);
    Eigen::Vector7s xs3d;
    Eigen::Vector3s xs2d;

    // 2 state blocks, fixed
    SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);

    // 2 state blocks, fixed
    SensorBasePtr    St = P->installSensor   ("ODOM 2D", "other odometer", xs2d, "");
    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2));

    auto pt = P->installProcessor("TRACKER FEATURE DUMMY",  "dummy",            "odometer");
    auto pm = P->installProcessor("ODOM 3D",                "odom integrator",  "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");

    // 2 state blocks, estimated
    auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2 + 2));

    // Notifications
    Notification notif;
    ASSERT_TRUE(P->getStateBlockNotification(KF->getP(), notif));
    ASSERT_EQ(notif, ADD);
    ASSERT_TRUE(P->getStateBlockNotification(KF->getO(), notif));
    ASSERT_EQ(notif, ADD);
    //    P->print(4,1,1,1);

    // change some SB properties
    St->unfixExtrinsics();
    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE

    //    P->print(4,1,1,1);

    // consume notifications
    DummySolverManagerPtr SM = std::make_shared<DummySolverManager>(P);
    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2 + 2));
    SM->update(); // calls P->consumeStateBlockNotificationMap();
    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consume empties the notification map

    // remove frame
    auto SB_P = KF->getP();
    auto SB_O = KF->getO();
    KF->remove();
    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2));
    ASSERT_TRUE(P->getStateBlockNotification(SB_P, notif));
    ASSERT_EQ(notif, REMOVE);
    ASSERT_TRUE(P->getStateBlockNotification(SB_O, notif));
    ASSERT_EQ(notif, REMOVE);

}

TEST(Problem, Covariances)
{
    std::string wolf_root = _WOLF_ROOT_DIR;

    ProblemPtr P = Problem::create("PO", 3);
    Eigen::Vector7s xs3d;
    Eigen::Vector3s xs2d;

    SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
    SensorBasePtr    St = P->installSensor   ("ODOM 2D", "other odometer", xs2d, "");

    ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
    params->time_tolerance            = 0.1;
    params->max_new_features          = 5;
    params->min_features_for_keyframe = 10;

    auto pt = P->installProcessor("TRACKER FEATURE DUMMY",  "dummy",            "odometer");
    auto pm = P->installProcessor("ODOM 3D",                "odom integrator",  "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");

    // 4 state blocks, estimated
    St->unfixExtrinsics();
    FrameBasePtr F = P->emplaceFrame("PO", 3, KEY, xs3d, 0);

    // set covariance (they are not computed without a solver)
    P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
    P->addCovarianceBlock(F->getO(), Eigen::Matrix3s::Identity());
    P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix3s::Zero());

    // get covariance
    Eigen::MatrixXs Cov;
    ASSERT_TRUE(P->getFrameCovariance(F, Cov));

    ASSERT_EQ(Cov.cols() , 6);
    ASSERT_EQ(Cov.rows() , 6);
    ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix6s::Identity(), 1e-12);

}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}