//
//  LiDAR_main.cpp
//  Gravity
//
//  Created by Hassan Hijazi on 3 April 2020.
//
//
#include <stdio.h>
#include <gravity/solver.h>
#ifdef USE_OPT_PARSER
#include <optionParser.hpp>
#endif
#include <gravity/rapidcsv.h>
#ifdef USE_MATPLOT
#include <gravity/matplotlibcpp.h>
#endif
#include <queue>
#include "DataSet.h"
#include "lasreader.hpp"
#include "laswriter.hpp"
#include <algorithm>
#include <iostream>
#include <vector>
#include <string>
#include <map>
#include <set>
#include <future>
#include <thread>
#include "Lidar_utils.h"
#include <gravity/solver.h>
#include "ARMOConfig.h"
#include "BB.h"
#include "Lower_Bound.h"

#ifdef USE_EIGEN3
#include <Eigen/Dense>
#endif
#include <time.h>

#ifdef USE_GJK
extern "C" {
#include "openGJK.h"
}
#endif
#include <iostream>
#include <algorithm>
using namespace std;
using namespace gravity;

/* Plot two point clouds */
void plot(const vector<vector<double>>& point_cloud1, const vector<vector<double>>& point_cloud2, double point_thick = 0.1);
void plot(const vector<vector<double>>& point_cloud1, const vector<vector<double>>& point_cloud2,const vector<vector<double>>& point_cloud3, double point_thick = 0.1);
void plot(const vector<vector<double>>& ext_model, const vector<vector<double>>& ext_data, const vector<vector<double>>& ext_data1,const vector<vector<double>>& ext_data2, double point_thick=0.1);

int main (int argc, char * argv[])
{
    DebugOn("\n********************************************************************************\n");
    DebugOn("********************************  ARMO  ****************************************\n");
    DebugOn("********************************************************************************\n");
    DebugOn("This program uses Gravity and LasLib and is released as open-source code under\n");
    DebugOn("the BSD-3 License. For more information visit https://github.com/lanl-ansi/ARMO\n");
    DebugOn("********************************************************************************\n");
    DebugOn("********************************************************************************\n");
    DebugOn("********************************************************************************\n\n");
#ifdef USE_MPI
    auto err_init = MPI_Init(nullptr,nullptr);
    int worker_id, nb_workers;
    auto err_rank = MPI_Comm_rank(MPI_COMM_WORLD, &worker_id);
    auto err_size = MPI_Comm_size(MPI_COMM_WORLD, &nb_workers);
#endif
    string file_u= string(armo_dir)+"/datasets/Truck.adc.laz";
    /*Scanner offset*/
    double scanner_x=0.0, scanner_y=0.160999998450279, scanner_z=0.016;
    /*"hidden" calibration applied by LiDAR viewer given in .json file*/
    /*Truck set*/
    double hr=-0.0004815624270122, hp=0.000897555320989341, hy=0.00249693566001952;
    /*to select overlapping regions of the object*/
    /*Truck set*/
    double xm=0, ym=0,zm=1262.5,xd=0,yd=0,zd=1261.1;
    /*to downsample points*/
    /*Truck set*/
    int mskip =1, dskip =2;
    double max_time = 100;
    double bore_roll=0, bore_pitch=0, bore_yaw=0;/*Calibration angles in degrees*/
    string algo="aGS";
    /*Algorithm Choices
     "aGS"(default) to run the heuristic upper bound
     "nsBB" to run the spatial branch and bound algorithm
     "gurobi" to run gurobi on the problem
     else if 3 angles given, the data-set is calibrated with these
     */
    if(argc>=2){
        file_u = argv[1];
    }
    if(argc>=3){
        algo = argv[2];
    }
    if(argc==4){
        max_time = std::stod(argv[3]);
    }
    if(argc>=5){
        algo="apply_angles";
        bore_roll= std::stod(argv[2]);
        bore_pitch= std::stod(argv[3]);
        bore_yaw= std::stod(argv[4]);
    }
    if(file_u.find(string(armo_dir)+"/datasets/Truck.adc.laz")!=std::string::npos){
        DebugOn("Truck data set selected"<<endl);
    }
    else if(file_u.find(string(armo_dir)+"/datasets/Car.adc.laz")!=std::string::npos){
        hr=0;hp=0;hy=0;
        xm=0, ym=0,zm=2122.0,xd=385276,yd=0,zd=2121.4;
        mskip=1,dskip=4;
        DebugOn("Car data set selected"<<endl);
    }
    else if(file_u.find(string(armo_dir)+"/datasets/Tent.adc.laz")!=std::string::npos){
        hr=0;hp=0;hy=0;
        xm=0; ym=0;zm=124;xd=0;yd=0;zd=124.2;
        mskip=2;dskip=3;
        DebugOn("Tent data set selected"<<endl);
    }
    else{
        DebugOn("WARNING: New data set!!!"<<endl<<"Before continuing "<<endl);
        DebugOn("Check if values of hidden calibration values are updated"<<endl);
        DebugOn("Check if values of xm,ym,zm,xd,yd,zd are updated"<<endl);
        DebugOn("Check if values of mskip and dskip are updated"<<endl);
    }
    string error_type="L2";
    vector<double> best_rot(9,0.0);
    bool data_opt=true;/*If true, Working with data set \hat{P} union \bar{P}, else data set \hat{D} union \bar{D}  */
    double best_ub=1e5,L2init, L1init;

    vector<vector<double>> full_point_cloud_model, full_point_cloud_data, full_uav_model, full_uav_data;
    vector<vector<double>> point_cloud_model, point_cloud_data,point_cloud_model1, point_cloud_data1;
    vector<vector<double>> uav_model, uav_data,uav_model1, uav_data1,cloud1, cloud2, uav1, uav2, rpy1, rpy2;
    vector<vector<double>> full_rpy_model, full_rpy_data, rpy_model, rpy_data,rpy_model1, rpy_data1;
    vector<vector<double>> lidar_point_cloud, roll_pitch_yaw, em;
    /*Reads input laz*/
    auto uav_cloud_u=::read_laz(file_u, lidar_point_cloud, roll_pitch_yaw);

    if(data_opt){/*Working with data sets P*/
        int mid_i=0;
        /*Separating data in parallel flight lines 1 and 2*/
        for(auto i=1;i<uav_cloud_u.size();i++)
        {
            auto x=uav_cloud_u.at(i)[0];
            auto y=uav_cloud_u.at(i)[1];
            auto z=uav_cloud_u.at(i)[2];
            auto x_prev=uav_cloud_u.at(i-1)[0];
            auto y_prev=uav_cloud_u.at(i-1)[1];
            auto z_prev=uav_cloud_u.at(i-1)[2];
            if(abs(x-x_prev)>=1 || abs(y-y_prev)>=1 || abs(z-z_prev)>=1){
                DebugOn("Two flight lines are detected"<<endl);
                mid_i=i;
            }
        }
        /*If two flight lines identified2*/
        if(mid_i==0){
            invalid_argument("Two flight lines are not detected!");
        }
        for(auto i=0;i<mid_i;i++){
            cloud1.push_back(lidar_point_cloud.at(i));
            uav1.push_back(uav_cloud_u.at(i));
            rpy1.push_back(roll_pitch_yaw.at(i));
        }
        for(auto i=mid_i;i<lidar_point_cloud.size();i++){
            cloud2.push_back(lidar_point_cloud.at(i));
            uav2.push_back(uav_cloud_u.at(i));
            rpy2.push_back(roll_pitch_yaw.at(i));
        }
        DebugOff("cloud1.size() "<<cloud1.size()<<endl);
        DebugOff("cloud2.size() "<<cloud2.size()<<endl);
        if(cloud1.size()>=cloud2.size()){
            full_point_cloud_model=cloud1;
            full_point_cloud_data=cloud2;
            full_uav_model=uav1;
            full_uav_data=uav2;
            full_rpy_model=rpy1;
            full_rpy_data=rpy2;
        }
        else{
            full_point_cloud_model=cloud2;
            full_point_cloud_data=cloud1;
            full_uav_model=uav2;
            full_uav_data=uav1;
            full_rpy_model=rpy2;
            full_rpy_data=rpy1;
        }
#ifdef USE_MATPLOT
        plot(full_point_cloud_model, full_point_cloud_data);
#endif
        vector<vector<double>> e;
        e.push_back(full_point_cloud_model[0]);
#ifdef USE_MATPLOT
        plot(full_point_cloud_model,e);
#endif
        e.clear();
        e.push_back(full_point_cloud_data[0]);
#ifdef USE_MATPLOT
        plot(full_point_cloud_data,e);
#endif
        for(auto i=0;i<full_point_cloud_model.size();i+=1){
            auto x=full_point_cloud_model.at(i)[0];
            auto y=full_point_cloud_model.at(i)[1];
            auto z=full_point_cloud_model.at(i)[2];
            if(x>=xm && y>=ym && z>=zm){
                point_cloud_model1.push_back(full_point_cloud_model.at(i));
                uav_model1.push_back(full_uav_model.at(i));
                rpy_model1.push_back(full_rpy_model.at(i));
            }
        }
        for(auto i=0;i<full_point_cloud_data.size();i++){
            auto x=full_point_cloud_data.at(i)[0];
            auto y=full_point_cloud_data.at(i)[1];
            auto z=full_point_cloud_data.at(i)[2];
            if(x>=xd && y>=yd && z>=zd){
                point_cloud_data1.push_back(full_point_cloud_data.at(i));
                uav_data1.push_back(full_uav_data.at(i));
                rpy_data1.push_back(full_rpy_data.at(i));
            }
        }
        DebugOff(point_cloud_model1.size()<<endl);
        DebugOff(point_cloud_data1.size()<<endl);
#ifdef USE_MATPLOT
        plot(point_cloud_model1, point_cloud_data1);
        plot(uav_model1,uav_data1);
#endif

        for(auto i=0;i<point_cloud_model1.size();i+=mskip){
            point_cloud_model.push_back(point_cloud_model1.at(i));
            uav_model.push_back(uav_model1.at(i));
            rpy_model.push_back(rpy_model1.at(i));

        }

        for(auto i=0;i<point_cloud_data1.size();i+=dskip){
            point_cloud_data.push_back(point_cloud_data1.at(i));
            uav_data.push_back(uav_data1.at(i));
            rpy_data.push_back(rpy_data1.at(i));

        }
        save_laz(file_u.substr(0,file_u.find(".laz"))+"_model.laz", point_cloud_model, em);
        save_laz(file_u.substr(0,file_u.find(".laz"))+"_data.laz", point_cloud_data, em);
        bool scale=true;
        if(scale){
            for(auto i=0;i<point_cloud_model.size();i++){
                point_cloud_model.at(i)[0]-=uav_cloud_u.at(0)[0];
                point_cloud_model.at(i)[1]-=uav_cloud_u.at(0)[1];
                point_cloud_model.at(i)[2]-=uav_cloud_u.at(0)[2];
            }
            for(auto i=0;i<point_cloud_data.size();i++){
                point_cloud_data.at(i)[0]-=uav_cloud_u.at(0)[0];
                point_cloud_data.at(i)[1]-=uav_cloud_u.at(0)[1];
                point_cloud_data.at(i)[2]-=uav_cloud_u.at(0)[2];
            }
            for(auto i=0;i<uav_model.size();i++){
                uav_model.at(i)[0]-=uav_cloud_u.at(0)[0];
                uav_model.at(i)[1]-=uav_cloud_u.at(0)[1];
                uav_model.at(i)[2]-=uav_cloud_u.at(0)[2];
            }
            for(auto i=0;i<uav_data.size();i++){
                uav_data.at(i)[0]-=uav_cloud_u.at(0)[0];
                uav_data.at(i)[1]-=uav_cloud_u.at(0)[1];
                uav_data.at(i)[2]-=uav_cloud_u.at(0)[2];
            }
        }
        DebugOn("Size of set bar P "<<point_cloud_model.size()<<endl);
        DebugOn("Size of set hat P "<<point_cloud_data.size()<<endl);

        double roll_min=-2*pi/180;
        double roll_max=2*pi/180;
        double pitch_min=-2*pi/180;
        double pitch_max=2*pi/180;
        double yaw_min=-2*pi/180;
        double yaw_max=2*pi/180;

        auto point_cloud_model_copy=point_cloud_model;
        auto point_cloud_data_copy=point_cloud_data;

        vector<int> matching(point_cloud_data.size());
        vector<double> err_per_point(point_cloud_data.size());

        L2init=computeL2error(point_cloud_model_copy,point_cloud_data_copy,matching,err_per_point);
        L1init=computeL1error(point_cloud_model_copy,point_cloud_data_copy,matching,err_per_point);
#ifdef USE_MPI
        if(worker_id==0){
#endif
            DebugOn("\n*************************\n");
            DebugOn("L2 Initial  "<<L2init<<endl);
            DebugOn("L1 Initial  "<<L1init<<endl);
            DebugOn("*************************\n");
#ifdef USE_MPI
        }
#endif

        if(error_type=="L2"){
            best_ub=L2init;
        }
        else{
            best_ub=L1init;
        }
        if(algo=="aGS"){
            auto rot= ub_heuristic_disc(point_cloud_model, point_cloud_data, uav_model, uav_data, rpy_model, rpy_data, best_rot, best_ub, error_type, scanner_x, scanner_y, scanner_z, hr, hp, hy, max_time);
            auto roll_rad_ub = rot[0];
            auto pitch_rad_ub = rot[1];
            auto yaw_rad_ub = rot[2];
            DebugOff("Angles radians "<<roll_rad_ub<<" "<<pitch_rad_ub<<" "<<yaw_rad_ub<<endl);
            DebugOn("Angles (degrees) "<<roll_rad_ub*180/pi<<" "<<pitch_rad_ub*180/pi<<" "<<yaw_rad_ub*180/pi<<endl);

        }
        else if(algo=="gurobi"){
            vector<vector<double>> input_data_cloud, input_model_cloud, input_data_offset, input_model_offset;
            generate_inputs(point_cloud_model, uav_model, rpy_model, scanner_x, scanner_y, scanner_z,hr,hp,hy, input_model_cloud, input_model_offset);
            generate_inputs(point_cloud_data, uav_data, rpy_data, scanner_x, scanner_y, scanner_z, hr,hp,hy,input_data_cloud, input_data_offset);
            auto rot_h= ub_heuristic_disc(point_cloud_model, point_cloud_data, uav_model, uav_data, rpy_model, rpy_data, best_rot, best_ub, error_type, scanner_x, scanner_y, scanner_z, hr, hp, hy);
            indices N1 = range(1,point_cloud_data.size());
            indices N2 = range(1,point_cloud_model.size());
            indices new_cells(N1,N2);
            param<double> dist_cells("dist_cells");
            auto model_i=Align_L2_model_rotation_trigonometric_scanner(input_model_cloud, input_data_cloud, uav_model, uav_data, rpy_model, rpy_data, input_model_offset, input_data_offset, roll_min, roll_max, pitch_min, pitch_max, yaw_min ,yaw_max, new_cells, dist_cells);
            int num_thread=thread::hardware_concurrency();
#ifdef USE_GUROBI
            solver<> S1(model_i,gurobi);
            S1.run(5,1e-6,"",9000000,10000000, best_ub, num_thread);
#endif

        }
        else if(algo=="nsBB"){/*Run the nsBB algorithm*/
            auto rot_h= ub_heuristic_disc(point_cloud_model, point_cloud_data, uav_model, uav_data, rpy_model, rpy_data, best_rot, best_ub, error_type, scanner_x, scanner_y, scanner_z, hr, hp, hy);
            vector<double> rot;
#ifdef USE_GJK
#ifdef USE_MPI
            rot=BranchBound_MPI(point_cloud_model, point_cloud_data, uav_model, uav_data, rpy_model, rpy_data, rot_h, best_ub, error_type, scanner_x, scanner_y, scanner_z, hr, hp, hy);
#else
            rot= BranchBound_Align(point_cloud_model, point_cloud_data, uav_model, uav_data, rpy_model, rpy_data, rot_h, best_ub, error_type, scanner_x, scanner_y, scanner_z, hr, hp, hy);
#endif

            auto roll_deg_bb = rot[0];
            auto pitch_deg_bb = rot[1];
            auto yaw_deg_bb = rot[2];

            apply_transform_new_order(roll_deg_bb*pi/180, pitch_deg_bb*pi/180, yaw_deg_bb*pi/180, point_cloud_model, uav_model, rpy_model, scanner_x,scanner_y,scanner_z,hr,hp,hy);
            apply_transform_new_order(roll_deg_bb*pi/180, pitch_deg_bb*pi/180, yaw_deg_bb*pi/180, point_cloud_data, uav_data, rpy_data, scanner_x,scanner_y,scanner_z,hr,hp,hy);
            save_laz(file_u.substr(0,file_u.find(".laz"))+to_string(roll_deg_bb)+"_"+to_string(pitch_deg_bb)+"_"+to_string(yaw_deg_bb)+"_hatp.laz", point_cloud_data, em);
            save_laz(file_u.substr(0,file_u.find(".laz"))+to_string(roll_deg_bb)+"_"+to_string(pitch_deg_bb)+"_"+to_string(yaw_deg_bb)+"_barp.laz", point_cloud_model, em);
            apply_transform_new_order(roll_deg_bb*pi/180, pitch_deg_bb*pi/180, yaw_deg_bb*pi/180, lidar_point_cloud, uav_cloud_u, roll_pitch_yaw, scanner_x,scanner_y,scanner_z,hr,hp,hy);
            save_laz(file_u.substr(0,file_u.find(".laz"))+"_"+to_string(roll_deg_bb)+"_"+to_string(pitch_deg_bb)+"_"+to_string(yaw_deg_bb)+".laz", lidar_point_cloud, em);

            auto L2=computeL2error(point_cloud_model,point_cloud_data,matching,err_per_point);
            auto L1=computeL1error(point_cloud_model,point_cloud_data,matching,err_per_point);

#ifdef USE_MPI
            if(worker_id==0){
#endif

                DebugOn("L2  "<<L2<<endl);
                DebugOn("L1  "<<L1<<endl);

                DebugOn("Percentage improved L2 "<<(L2init-L2)/L2init*100.0<<endl);
                DebugOn("Percentage improved L1 "<<(L1init-L1)/L1init*100.0<<endl);
#ifdef USE_MPI
            }
#endif
#endif
        }
        else{/*Apply the calibration values on sets \hat{P}, \bar{P} and \hat{P} union \bar{P} */
            apply_transform_new_order(bore_roll*pi/180, bore_pitch*pi/180, bore_yaw*pi/180, point_cloud_model, uav_model, rpy_model, scanner_x,scanner_y,scanner_z,hr,hp,hy);
            apply_transform_new_order(bore_roll*pi/180, bore_pitch*pi/180, bore_yaw*pi/180, point_cloud_data, uav_data, rpy_data, scanner_x,scanner_y,scanner_z,hr,hp,hy);
            apply_transform_new_order(bore_roll*pi/180, bore_pitch*pi/180, bore_yaw*pi/180, lidar_point_cloud, uav_cloud_u, roll_pitch_yaw, scanner_x, scanner_y, scanner_z, hr, hp, hy);
            save_laz(file_u.substr(0,file_u.find(".laz"))+to_string(bore_roll)+"_"+to_string(bore_pitch)+"_"+to_string(bore_yaw)+"_hatp.laz", point_cloud_data, em);
            save_laz(file_u.substr(0,file_u.find(".laz"))+to_string(bore_roll)+"_"+to_string(bore_pitch)+"_"+to_string(bore_yaw)+"_barp.laz", point_cloud_model, em);
            save_laz(file_u.substr(0,file_u.find(".laz"))+to_string(bore_roll)+"_"+to_string(bore_pitch)+"_"+to_string(bore_yaw)+".laz", lidar_point_cloud, em);
            auto L2=computeL2error(point_cloud_model,point_cloud_data,matching,err_per_point);
            auto L1=computeL1error(point_cloud_model,point_cloud_data,matching,err_per_point);
#ifdef USE_MPI
            if(worker_id==0){
#endif
                DebugOn("L2  "<<L2<<endl);
                DebugOn("L1  "<<L1<<endl);

                DebugOn("Percentage improved L2 "<<(L2init-L2)/L2init*100.0<<endl);
                DebugOn("Percentage improved L1 "<<(L1init-L1)/L1init*100.0<<endl);
#ifdef USE_MPI
            }
#endif
        }
    }
    else{/*Apply the calibration values on large data set D=\hat{D} union \bar{D}*/

        apply_transform_new_order(bore_roll*pi/180, bore_pitch*pi/180, bore_yaw*pi/180, lidar_point_cloud, uav_cloud_u, roll_pitch_yaw, scanner_x, scanner_y, scanner_z, hr, hp, hy);
        save_laz(file_u.substr(0,file_u.find(".laz"))+to_string(bore_roll)+"_"+to_string(bore_pitch)+"_"+to_string(bore_yaw)+".laz", lidar_point_cloud, em);
    }
#ifdef USE_MATPLOT
    plot(point_cloud_model, point_cloud_data);
#endif

}


// Copyright (c) fortiss GmbH
//
// Authors: Patrick Hart, Julian Bernhard, Klemens Esterle, and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef BARK_ML_OBSERVERS_NEAREST_OBSERVER_HPP_
#define BARK_ML_OBSERVERS_NEAREST_OBSERVER_HPP_

#include <memory>
#include <vector>
#include <tuple>
#include <map>
#include <functional>
#include <Eigen/Dense>

#include "bark/commons/params/params.hpp"
#include "bark/world/world.hpp"
#include "bark/world/observed_world.hpp"
#include "bark/world/goal_definition/goal_definition_state_limits_frenet.hpp"
#include "bark/models/dynamic/dynamic_model.hpp"
#include "bark_ml/commons/spaces.hpp"
#include "bark_ml/commons/commons.hpp"
#include "bark_ml/observers/base_observer.hpp"

namespace bark_ml {
namespace observers {
using bark::commons::ParamsPtr;
using bark::world::Agent;
using spaces::Box;
using commons::Norm;
using spaces::Matrix_t;
using bark::world::AgentMap;
using bark::world::AgentPtr;
using bark::world::WorldPtr;
using bark::world::goal_definition::GoalDefinitionStateLimitsFrenet;
using bark::world::ObservedWorldPtr;
using bark::geometry::Point2d;
using bark::geometry::Line;
using bark::geometry::Distance;
using bark::geometry::Norm0To2PI;
using bark::models::dynamic::StateDefinition::X_POSITION;
using bark::models::dynamic::StateDefinition::Y_POSITION;
using bark::models::dynamic::StateDefinition::THETA_POSITION;
using bark::models::dynamic::StateDefinition::VEL_POSITION;
using ObservedState = Eigen::Matrix<float, 1, Eigen::Dynamic>;
using bark::commons::transformation::FrenetPosition;
using State = Eigen::Matrix<float, Eigen::Dynamic, 1>;

#define terminal_output_enabled false

class NearestObserver : public BaseObserver {
 public:
  explicit NearestObserver(const ParamsPtr& params) :
    BaseObserver(params),
    min_x_(0.), max_x_(100.),
    min_y_(0.), max_y_(100.),
    min_theta_(0), max_theta_(2*3.14159) {
      nearest_agent_num_ =
        params->GetInt(
          "ML::StateObserver::MaxNumAgents", "Nearest agents number", 4);
      min_vel_ = params->GetReal("ML::NearestObserver::MinVel", "", 0.0);
      max_vel_ = params->GetReal("ML::NearestObserver::MaxVel", "", 50.0);
      max_dist_ = params->GetReal("ML::NearestAgentsObserver::MaxOtherDistance", "", 75.0);
      state_size_ = params->GetInt("ML::NearestObserver::StateSize", "", 4);
      normalization_enabled = params->GetBool("ML::StateObserver::NormalizationEnabled", "", true);
      distance_method_ = params->GetInt("ML::NearestAgentsObserver::DistanceMethod", "", 2); //1=L1; 2=L2(default)
      observation_len_ = nearest_agent_num_ * state_size_;
  }

  float Norm(const float val, const float mi, const float ma) const {
    return (val - mi)/(ma - mi);
  }

  ObservedState FilterState(const State& state) const {
    ObservedState ret_state(1, state_size_);
    if (normalization_enabled == true){
      const float normalized_angle = Norm0To2PI(state(THETA_POSITION));
      ret_state << Norm(state(X_POSITION), min_x_, max_x_),
                  Norm(state(Y_POSITION), min_y_, max_y_),
                  Norm(normalized_angle, min_theta_, max_theta_),
                  Norm(state(VEL_POSITION), min_vel_, max_vel_);
      return ret_state;
    }
    else{
      ret_state <<
        state(X_POSITION),
        state(Y_POSITION),
        state(THETA_POSITION),
        state(VEL_POSITION);
      return ret_state;
    }
  }

  ObservedState Observe(const ObservedWorldPtr& observed_world) const {
    int row_idx = 0;
    ObservedState state(1, observation_len_);
    state.setZero();

    // find near agents (n)
    AgentMap nearest_agents = observed_world->GetNearestAgents(
      observed_world->CurrentEgoPosition(), nearest_agent_num_);

    // sort agents by distance and distance < max_dist_
    std::map<float, AgentPtr, std::less<float>> distance_agent_map;
    for (const auto& agent : nearest_agents) {
      const auto& agent_state = agent.second->GetCurrentPosition();
      float distance = 0; //init
      if (distance_method_ == 1){
        distance = L1_Distance(observed_world->CurrentEgoPosition(), agent_state);
      }
      else{
        distance = Distance(observed_world->CurrentEgoPosition(), agent_state);
      }
      if (distance < max_dist_)
        distance_agent_map[distance] = agent.second;
        #if terminal_output_enabled==true
          auto view_state = agent.second->GetCurrentState();
          std::cout<<"agent Id: "<< agent.first << std::endl;
          std::cout<<"distance to ego_agent: "<< distance << std::endl;
          std::cout<<"State : \n"<< view_state << std::endl;
        #endif
    }

    // add ego agent state
    ObservedState obs_ego_agent_state =
      FilterState(observed_world->CurrentEgoState());
    state.block(0, row_idx*state_size_, 1, state_size_) = obs_ego_agent_state;
    row_idx++;

    #if terminal_output_enabled==true
      std::shared_ptr<const Agent> ego_agent = observed_world->GetEgoAgent();
      std::cout<<"ego_id: " << ego_agent->GetAgentId() << std::endl;
      std::cout<<"ego_state_normalized: \n" << obs_ego_agent_state << std::endl;
      std::cout<<"state_vector: \n" << state << std::endl;
    #endif

    // add other states
    for (const auto& agent : distance_agent_map) {
      if (agent.second->GetAgentId() != observed_world->GetEgoAgentId()) {
        ObservedState other_agent_state =
          FilterState(agent.second->GetCurrentState());
        state.block(0, row_idx*state_size_, 1, state_size_) = other_agent_state;  // NOLINT
        row_idx++;

         #if terminal_output_enabled==true
          std::cout<<"agent_id: "<<agent.second->GetAgentId()<<std::endl;
          std::cout<<"agent_state_normalized: \n" << other_agent_state << std::endl;
          std::cout<<"state_vector: \n" << state << std::endl;
        #endif
      }
    }
    return state;
  }

  float L1_Distance (const Point2d &p1, const Point2d &p2) const {
    float dx = boost::geometry::get<0>(p1) - boost::geometry::get<0>(p2);
    float dy = boost::geometry::get<1>(p1) - boost::geometry::get<1>(p2);
    return (abs(dx) + abs(dy));
  }

  WorldPtr Reset(const WorldPtr& world) {
    const auto& x_y = world->BoundingBox();
    Point2d bb0 = x_y.first;
    Point2d bb1 = x_y.second;
    min_x_ = bb0.get<0>();
    max_x_ = bb1.get<0>();
    min_y_ = bb0.get<1>();
    max_y_ = bb1.get<1>();
    return world;
  }

  Box<float> ObservationSpace() const {
    Matrix_t<float> low(1, observation_len_);
    low.setZero();
    Matrix_t<float> high(1, observation_len_);
    high.setOnes();
    std::tuple<int> shape{observation_len_};
    return Box<float>(low, high, shape);
  }

 private:
  int state_size_, nearest_agent_num_, observation_len_, distance_method_;
  float min_theta_, max_theta_, min_vel_, max_vel_, max_dist_,
         min_x_, max_x_, min_y_, max_y_;
  bool normalization_enabled;
};

}  // namespace observers
}  // namespace bark_ml

#undef terminal_output_enabled
#endif  // BARK_ML_OBSERVERS_NEAREST_OBSERVER_HPP_


//
// Created by yalavrinenko on 22.05.2021.
//

#include <engines/greedy_engine.hpp>
#include <iostream>
#include <map/rogaine_map.hpp>
#include <pathfinders/Astar_searcher.hpp>
#include <solver.hpp>
#include <string>
#include <boost/format.hpp>

#include <pathfinders/boost_graph/full_linked_graph.hpp>
#include <pathfinders/boost_graph/boost_asearch.hpp>
#include <pathfinders/legacy/asearch.hpp>

int main(int argc, char** argv){
  //auto test_path = "../../demo/test.png";
  //auto test_path = "../../demo/16.jpg";
  auto test_path = "/home/yalavrinenko/Files/git/trash_rogaine_solver/demo/Serotonin Row 2021.png";

  trs::rogaine_map map(test_path);
  auto img = map.raw_image().clone();
  auto road = map.extract_roads();

  auto checks = map.checks();

  std::ranges::for_each(checks, [&img](auto const& check){
    auto position = cv::Point_<size_t>{check.second.position.first, check.second.position.second};
    cv::circle(img, position, 10, cv::Scalar{255}, 3);
    cv::putText(img, std::to_string(check.second.uid), position, cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar{128}, 3);
  });

  using namespace std::string_literals;
  auto raw_window = test_path + " raw"s;

  auto scale = [](auto const &img, auto fx, auto fy) {
    cv::Mat out;
    cv::resize(img, out, {}, fx, fy);
    return out;
  };

  cv::imshow(raw_window, scale(img, 0.5, 0.5));
  //cv::imshow(raw_window, img);

//  cv::Mat hsv;
//  cv::cvtColor(img, hsv, cv::COLOR_BGR2HSV);
//
//  auto mouse_cb = [](int event, int x, int y, int flags, void* userdata){
//    auto* img = static_cast<cv::Mat*>(userdata);
//    if (event == cv::EVENT_LBUTTONDOWN){
//      std::cout << x << " " << y << " ===> ";
//      auto color = img->at<cv::Vec3b>(y, x);
//      std::cout << static_cast<int>(color[0]) << " " << static_cast<int>(color[1]) << " " << static_cast<int>(color[2]) << std::endl;
//    }
//  };
//
//  cv::setMouseCallback(raw_window, mouse_cb, &hsv);

//  cv::imshow(raw_window + "road", scale(road, 0.5, 0.5));
  cv::waitKey();

//  using searcher_t = trs::Astar_searcher < trs::rogaine_map<>, trs::full_linked_grid,
//        trs::boost_astar_search<trs::full_linked_grid, trs::full_linked_grid::vertex_t,
//                                trs::full_linked_grid::visitor_t, trs::full_linked_grid::hash_t,
//                                trs::full_linked_grid::target_visitor::target_reached>>;

//  using searcher_t = trs::Astar_searcher < trs::rogaine_map<>, trs::boost_grid_graph,
//                                         trs::boost_astar_search<> >;

  using searcher_t = trs::Astar_searcher < trs::rogaine_map<>, trs::full_linked_grid,
                                         trs::astar_search<trs::full_linked_grid> >;

  [[maybe_unused]] trs::solver<trs::rogaine_map<>, searcher_t, trs::greedy_engine> solver(map);

  double ED = 10000.0;
  size_t CP;

  std::cin >> CP >> ED;
  std::cout << "Solving for CP " << CP << " and ED " << ED << std::endl;

  auto sol = solver.solve(ED, CP);

  std::cout << boost::format("Solution for CP %1% and estimate distance %2%\n") % CP % ED;
  std::cout << boost::format("\tScore %1% \t Distance %2% \t Checks %3%\n") % sol.score % sol.distance % (sol.trace.size() - 1);

  std::cout << "Visited checks: ";
  std::ranges::copy(sol.trace | std::views::take(sol.trace.size() - 1), std::ostream_iterator<size_t>(std::cout, "->"));
  std::cout << sol.trace.back() << std::endl;

  for (auto &track : sol.track) {
    for (auto &tp : track.second) { cv::circle(img, cv::Point_<int>{tp.second, tp.first}, 2, cv::Scalar{0, 0, 255}, 2); }
  }

  cv::imshow(raw_window, scale(img, 0.6, 0.6));
  cv::waitKey();
  return 0;
}


// Author Dmytro Bobkov, dmytro.bobkov@tum.de
// Chair of Media Technology, Technical University of Munich, 2018-2019
// Parts of these were done while being at research visit to group of Bernd Girod IVMS at Stanford University

#include "rendering_tools.h"

#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include <boost/filesystem.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>

namespace po = boost::program_options;
namespace fs = boost::filesystem;

#include "projection_helpers.h"
#include "io_helpers.h"

void
generateRenderedImages( const Configuration& config_rendering,
                        const std::vector<std::string>& pano_filenames,
                        const std::map<std::string, Panorama>& pano_poses,
                        std::vector<RenderedImage>& rendered_images )
{
    int global_view_id = 0;
    for( size_t ind_pano_raw=0; ind_pano_raw<pano_filenames.size(); ind_pano_raw++ ) {
        std::string name_image = pano_filenames[ind_pano_raw];
        // get index from the filename correctly
        boost::filesystem::path p_i(name_image);
        std::string pano_name_file = p_i.stem().string(); // without extension
        std::string name_basename = p_i.filename().string();
        std::string name_prefix = pano_name_file + '_';

        // pano file to json pose (remove 3 symbols and add pose
        std::string name_json_pano_pose = pano_name_file.substr(0, pano_name_file.length() - 3) + "pose";

        if( pano_poses.count(name_json_pano_pose) == 0) {
            name_json_pano_pose = pano_name_file;
        }
        std::cout << "Pose filename " << name_json_pano_pose << " from " << pano_name_file << std::endl;

        Panorama pano = pano_poses.at(name_json_pano_pose);
        Pose pose = pano.pose;

        for (size_t ind_yaw = 0; ind_yaw < config_rendering.yaw_angles.size(); ind_yaw++) {
            for (size_t ind_pitch = 0; ind_pitch < config_rendering.pitch_angles.size(); ind_pitch++) {

                // based on pitch and roll, compute quaternion
                const double yaw_ref = config_rendering.yaw_angles[ind_yaw];
                double yaw_deg = constrainAngle(yaw_ref);
                const double pitch_deg = config_rendering.pitch_angles[ind_pitch];

                RenderedImage rendered_image = getRenderedImage( yaw_deg, pitch_deg,
                                                                 pano, name_prefix, name_basename );
                rendered_image.id = global_view_id;
                rendered_image.ind_yaw = ind_yaw;
                global_view_id++;

                rendered_images.push_back(rendered_image);
            }
        }

    }
}

void
parsePanoramaFilenames( const std::string& in_pano_dir,
                        std::vector<std::string>& pano_filenames )
{
    if( boost::filesystem::exists(in_pano_dir) ) {
        boost::filesystem::path p(in_pano_dir);
        for( auto& entry : boost::make_iterator_range(boost::filesystem::directory_iterator(p), {})) {
            std::string extension =  boost::filesystem::extension(entry);
            if( extension == ".png" ) {
                std::string name_file = entry.path().string();
                pano_filenames.push_back(name_file);
            }
            else {
                std::cout << "Not jpg" << entry.path().string() << std::endl;
            }
        }
    }
    else {
        std::cout << "Folder " << in_pano_dir << " does not exist" << std::endl;
        exit(-1);
    }
}

void
parseInLocPanoramaPoses( const std::string& folder_with_poses,
                         std::map<std::string, Panorama>& pano_infos )
{
    std::vector<std::string> files;
    //for (auto & p : fs::directory_iterator(folder_with_poses))
    for( const auto& p : boost::make_iterator_range(fs::directory_iterator(folder_with_poses), {}))
    {
        //std::cout << p << std::endl;
        files.push_back(p.path().string());
    }

    for( std::string& json_file : files ) {

        boost::filesystem::path p_i(json_file);

        std::string extension = boost::filesystem::extension(p_i);
        if( extension != ".json" ) {
            continue;
        }

        std::string basename_file = p_i.stem().string();

        std::ifstream json_input(json_file);
        nlohmann::json json_pose;
        json_input >> json_pose;

        nlohmann::json json_location = json_pose["camera_location"];
        Eigen::Vector3f t_correct;
        t_correct << json_location[0], json_location[1], json_location[2];

        std::vector<float> rot_vec = json_pose["final_camera_rotation"];

        Eigen::Quaternionf quat_rot(rot_vec.data());

        Panorama pano;
        pano.pose.location = t_correct;
        pano.pose.orientation = quat_rot;
        pano.room = json_pose["room"];

        std::cout << "Name: " << basename_file << std::endl;
        pano_infos.insert({basename_file, pano});
    }
    std::cout << "Finished parsing panorama poses, total " << pano_infos.size() << std::endl;

}

void
parsePanoramaPoses( const std::string& folder_with_poses,
                    std::map<std::string, Panorama>& pano_infos )
{
    std::vector<std::string> files;
    //for (auto & p : fs::directory_iterator(folder_with_poses))
    for( const auto& p : boost::make_iterator_range(fs::directory_iterator(folder_with_poses), {}))
    {
        //std::cout << p << std::endl;
        files.push_back(p.path().string());
    }

    for( std::string& json_file : files ) {

        boost::filesystem::path p_i(json_file);

        std::string extension = boost::filesystem::extension(p_i);
        if( extension != ".json" ) {
            continue;
        }

        std::string basename_file = p_i.stem().string();

        std::ifstream json_input(json_file);
        nlohmann::json json_pose;
        json_input >> json_pose;

        nlohmann::json json_location = json_pose["camera_location"];
        Eigen::Vector3f location;
        location << json_location[0], json_location[1], json_location[2];

        nlohmann::json rt_matrix = json_pose["camera_rt_matrix"];
        float rt00 = rt_matrix[0][0];
        Eigen::Matrix3f rot_pose;
        rot_pose << rt_matrix[0][0], rt_matrix[0][1], rt_matrix[0][2],
                rt_matrix[1][0], rt_matrix[1][1], rt_matrix[1][2],
                rt_matrix[2][0], rt_matrix[2][1], rt_matrix[2][2];
        Eigen::Vector3f t(rt_matrix[0][3], rt_matrix[1][3], rt_matrix[2][3]);
        Eigen::Vector3f t_correct = -rot_pose.transpose() * t;

        std::vector<float> rot_vec = json_pose["final_camera_rotation"];
        float yaw = rot_vec[0];
        float pitch = rot_vec[1];
        float roll = rot_vec[2];
        Eigen::Quaternionf q =  Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitZ()) *
                                Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
                                Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitX());
        Eigen::Quaternionf quat_rot(rot_pose);

        Panorama pano;
        pano.pose.location = t_correct;
        pano.pose.orientation = quat_rot;
        pano.room = json_pose["room"];

        std::cout << "Name: " << basename_file << std::endl;
        pano_infos.insert({basename_file, pano});
    }
    std::cout << "Finished parsing panorama poses, total " << pano_infos.size() << std::endl;
}


/*
 * to_ewald_sphere_helpers.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <scitbx/vec3.h>
#include <scitbx/array_family/flex_types.h>
#include <dxtbx/model/beam.h>
#include <dxtbx/model/detector.h>
#include <dxtbx/model/goniometer.h>
#include <dxtbx/model/sequence.h>

namespace dxtbx { namespace boost_python {

  using namespace boost::python;

  using dxtbx::model::MonoBeam;
  using dxtbx::model::Detector;
  using dxtbx::model::Goniometer;
  using dxtbx::model::Scan;
  using scitbx::vec2;
  using scitbx::vec3;
  using scitbx::af::flex_grid;

  typedef scitbx::af::flex<vec3<double> >::type flex_vec3_double;

  class ImageToEwaldSphere {
  public:
    ImageToEwaldSphere(const MonoBeam &beam,
                       const Detector &detector,
                       const Goniometer &gonio,
                       const Scan &scan)
        : beam_(beam), detector_(detector), gonio_(gonio), scan_(scan) {}

    flex_vec3_double operator()(int frame, std::size_t panel) {
      // Check panel
      DXTBX_ASSERT(panel < detector_.size());

      // Get size and create array
      std::size_t slow_size = detector_[0].get_image_size()[1];
      std::size_t fast_size = detector_[0].get_image_size()[0];
      flex_vec3_double x(flex_grid<>(slow_size, fast_size));

      // Get rotation angle
      double phi = scan_.get_angle_from_array_index(frame - 0.5);

      // Get coordinate for each pixel
      for (std::size_t j = 0; j < slow_size; ++j) {
        for (std::size_t i = 0; i < fast_size; ++i) {
          vec3<double> s1 = detector_[panel].get_pixel_lab_coord(vec2<double>(i, j));
          x(j, i) =
            s1.normalize().unit_rotate_around_origin(gonio_.get_rotation_axis(), phi)
            / beam_.get_wavelength();
        }
      }

      // Return array
      return x;
    }

    MonoBeam beam_;
    Detector detector_;
    Goniometer gonio_;
    Scan scan_;
  };

  void export_to_ewald_sphere_helpers() {
    class_<ImageToEwaldSphere>("ImageToEwaldSphere", no_init)
      .def(init<const MonoBeam &, const Detector &, const Goniometer &, const Scan &>(
        (arg("beam"), arg("detector"), arg("goniometer"), arg("scan"))))
      .def("__call__", &ImageToEwaldSphere::operator());
  }

}}  // namespace dxtbx::boost_python


// Copyright (c) 2016, University of Minnesota
//
// lbfgssolver Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
// 2. 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 "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 UNIVERSITY OF MINNESOTA, DULUTH 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.

#ifndef ADMM_SYSTEM_H
#define ADMM_SYSTEM_H 1

#include "Force.hpp"
#include "ExplicitForce.hpp"
#include <Eigen/SparseCholesky>

namespace admm {

class System {
public:
	System() : elapsed_s(0.0), initialized(false) {}

	// Solver settings
	// Can be loaded from args: system.settings.parse_args(argc,argv)
	struct Settings {
		void parse_args( int argc, char **argv ); // parse from terminal args
		void help();		// -help	print details
		double timestep_s;	// -dt <flt>	timestep in seconds (don't change after initialize!)
		int verbose;		// -v <int>	terminal output level (higher=more)
		int admm_iters;		// -it <int>	number of admm-solver iterations
		Settings() : timestep_s(0.04), verbose(1), admm_iters(10) {}
	} settings ;

	double elapsed_s; // accumulated time in seconds

	// Per-node (x3) data (for x, y, and z)
	Eigen::VectorXd m_x; // node positions, scaled x3
	Eigen::VectorXd m_v; // node velocities, scaled x3
	Eigen::VectorXd m_masses; // node masses, scaled x3

	std::vector< std::shared_ptr<ExplicitForce> > explicit_forces; // forces applied explicitly
	std::vector< std::shared_ptr<Force> > forces; // minimized (implicit)

	// Adds nodes to the system.
	// Returns the current total number of nodes after insert.
	// Assumes x and m are scaled x3.
	int add_nodes( Eigen::VectorXd x, Eigen::VectorXd m );

	// Returns true on success.
	// Computes global matrices and should only be called once
	// after all nodes have been added to the system. Once called,
	// no more nodes can be added or removed.
	bool initialize();

	// Performs a system step without any capture/prints
	bool step();

	// You can change the weights at runtime.
	// To do so, adjust the weight value associated with whatever forces
	// you want to change. Then, call this function. However, this
	// means the system has to be recomputed, so do it sparingly.
	void recompute_weights();

	// Adds a callback function that is executed at the beginning of a step.
	// This is helpful for things like recording residuals, updating anchor control points, etc...
	std::vector< std::function<void ( admm::System* )> > pre_step_callbacks;

protected:

	// Settings
	bool initialized;

	// Global matrices
	Eigen::SparseMatrix<double> m_D; // "reduction" matrix
	Eigen::VectorXd m_W_diag; // diagonal of the weight matrix

	// Solver variables computed in initialize
	Eigen::SparseMatrix<double> solver_dt2_Dt_Wt_W;
	Eigen::SimplicialLDLT< Eigen::SparseMatrix<double> > solver;

	// These variables don't need to be class members, but
	// are stored as such to avoid reallocation. Otherwise it
	// becomes noticeably slower for large systems.
	Eigen::VectorXd solver_termB;
	Eigen::VectorXd Dx;
	Eigen::VectorXd curr_u; // admm dual
	Eigen::VectorXd curr_z; // admm primal

}; // end class system

} // end namespace admm

#endif