#include <iostream>
#include "signal.h"
#include <gflags/gflags.h>

#include <glog/logging.h>

#include "system/syscontroller.h"
#include "input/sensor_subscriber.h"

#ifdef ENABLE_ROS
#include "input/ros_bridge/cloud_subscriber.h"
#include "input/ros_bridge/gnss_subscriber.h"
#include "input/ros_bridge/imu_subscriber.h"
#include "input/ros_bridge/wheel_subscriber.h"
#include "input/ros_bridge/imupose_subscriber.h"
#include "output/ros_publisher.h"
#else
#ifdef ENABLE_BAG
#include "input/rosbag/rosbag_reader.h"
#include "output/ros_publisher.h"
#else
#include "input/file_reader/cloud_receiver.h"
#include "input/file_reader/gnss_receiver.h"
#include "input/file_reader/imu_receiver.h"
#include "input/file_reader/wheel_receiver.h"
#include "input/file_reader/file_reader.h"
#include "output/netpublisher.h"
#endif
#endif

#include "dispatch/dispatch.h"
#include "serialize/blockserializer.h"
#include "locate_system/localize_utils/sys_utils.h"

DEFINE_string(base_dir, "", "base_dir");
DEFINE_string(bag_dir, "", "bag_dir");

using namespace std;
using namespace juefx;
using namespace boost::filesystem;

bool requestToEnd = false;
SensorSubscriber::Ptr imuSubscriber;
SensorSubscriber::Ptr gnssSubscriber;
SensorSubscriber::Ptr wheelSubscriber;
SensorSubscriber::Ptr cloudSubscriber;
SensorSubscriber::Ptr imuPoseSubscriber;

void failureHandle(const char* data, int size){
    string str = data;
    LOG(INFO) << "failureHandle: " << str;
}

void signalHandle(int sig){
    requestToEnd = true;
    LOG(INFO) << "requestToEnd";
}

Vector3d getCenter(const string &center_dir){
    Vector3d center;
    std::ifstream ifs_center(center_dir);
    if(!ifs_center) {
        LOG(WARNING) << "open centerPath_ file error: " << center_dir;
    }else{
        string line;
        getline(ifs_center, line);
        {
            double lat, lon, h;
            istringstream iss(line);
            string subline;
            getline(iss, subline, ',');
            istringstream(subline) >> lat;
            getline(iss, subline, ',');
            istringstream(subline) >> lon;
            getline(iss, subline, ',');
            istringstream(subline) >> h;
            center = {lat, lon, h};
            LOG(INFO) << setprecision(15) << "get center: " << center.transpose();
        }
        ifs_center.close();
    }
    return center;
}

void Compress(const string& baseDir)
{
    LOG(INFO) << "baseDir: " << baseDir;
    auto whole_start = system_clock::now();

    string outputDir = baseDir + "/stream/";
    if(!is_directory(outputDir)){
        create_directory(outputDir);
    }else{
        return;
    }
    // string pcdDir = FLAGS_base_dir + "/pcd_cart/";
    string pcdDir = baseDir;
    if(!is_directory(pcdDir)){
        return;
    }
    string center_dir = baseDir + "/meshes.center";
    Vector3d center = getCenter(center_dir);
    directory_iterator pcd_iter(pcdDir);
    directory_iterator pcd_end_iter;
    map<string, vector<string>> meshPcdPathsMap;
    for(; pcd_iter != pcd_end_iter; pcd_iter++) {
        if(pcd_iter->path().extension().string() != ".pcd"){
            continue;
        }
        string pcd_path = pcd_iter->path().string();
        string pcd_name = pcd_iter->path().stem().string();
        string meshId;
        if(pcd_name.find("_") != string::npos){
            vector<string> pcd_name_parts;
            boost::split(pcd_name_parts, pcd_name, boost::is_any_of("_"), boost::token_compress_on);
            meshId = pcd_name_parts[0];
        }else{
            meshId = pcd_name;
        }
        auto iter = meshPcdPathsMap.find(meshId);
        if(iter == meshPcdPathsMap.end()){
            meshPcdPathsMap.insert(make_pair(meshId, vector<string>{pcd_path}));
        }else{
            iter->second.push_back(pcd_path);
        }
    }
    for(auto iter = meshPcdPathsMap.begin();
        iter != meshPcdPathsMap.end();
        iter++){
        // ulong meshId = stoi(iter->first);
        LOG(INFO) << iter->first;
        auto start = system_clock::now();
        vector<string> pcd_paths = iter->second;
        PointCloudExport::Ptr pointcloud(new PointCloudExport);
        for(string pcd_path : pcd_paths){
            LOG(INFO) << pcd_path;
            PointCloudExport pc;
            pcl::io::loadPCDFile(pcd_path, pc);
            *pointcloud += pc;
        }
        {
            rusage usage;
            getrusage(RUSAGE_SELF, &usage);
            LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
            print_mem(getpid());
        }
        boost::shared_ptr<Dispatch> dispatch(new Dispatch(pointcloud));
        // dispatch.setMeshId(meshId);
        dispatch->setBLH(center);
        dispatch->configOffset();
        dispatch->dispatch();
        LOG(INFO) << "dispatch done";
        boost::shared_ptr<GridLayer> meshLayer =
                dispatch->getMeshLayer();

        BlockSerializer blockSerializer(meshLayer);
        blockSerializer.setMeshId(iter->first);
        blockSerializer.setOffset(dispatch->getOffset());
        blockSerializer.setBLH(center);
        blockSerializer.setOutputPath(
                    outputDir + iter->first + ".stream");

        dispatch.reset();
        pointcloud.reset();

        {
            rusage usage;
            getrusage(RUSAGE_SELF, &usage);
            LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
            print_mem(getpid());
        }

        blockSerializer.configSerialization();

        {
            rusage usage;
            getrusage(RUSAGE_SELF, &usage);
            LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
            print_mem(getpid());
        }

        auto end = system_clock::now();
        auto duration = duration_cast<milliseconds>(end - start);
        LOG(INFO) << iter->first << " load take "
                  << double(duration.count()) / 1000 << " seconds";
    }
    auto whole_end = system_clock::now();
    auto whole_duration = duration_cast<milliseconds>(whole_end - whole_start);
    LOG(INFO) << "whole load take "
              << double(whole_duration.count()) / 1000 << " seconds";
    return;
}

void Locate(const string& baseDir)
{
    string retPath = baseDir + "/CloudPredictPose.txt";
//    if(exists(retPath)){
//        LOG(INFO) << baseDir << " already done!";
//        return;
//    }
    std::ifstream ifs_ret(retPath);
    if(!ifs_ret) {
        LOG(INFO) << "open retPath file error: " << retPath;
    }else{
        string line;
        getline(ifs_ret, line);
        if(line.size() != 0){
            LOG(INFO) << baseDir << " already done!";
            return;
        }
        ifs_ret.close();
    }
#ifdef ENABLE_BAG
    string taskPath = baseDir;
    taskPath.erase(taskPath.end() - 8, taskPath.end());
    vector<string> line_vec;
    boost::split(line_vec, taskPath, boost::is_any_of("/"), boost::token_compress_on);
    if(0 == line_vec.size()){
        LOG(WARNING) << "0 == line_vec.size()!";
        return;
    }
    string bagPath = "";
    string prefix = line_vec.back();
    if(is_directory(FLAGS_bag_dir)){
        directory_iterator dir_iter(FLAGS_bag_dir);
        directory_iterator end_iter;
        for(; dir_iter != end_iter; dir_iter++) {
            if(!is_directory(dir_iter->path())){
                continue;
            }
            string taskDir = dir_iter->path().string();
            string taskId = dir_iter->path().stem().string();
            if(taskId == prefix){
                directory_iterator bag_iter(taskDir);
                directory_iterator bag_end_iter;
                string early = "";
                for(; bag_iter != bag_end_iter; bag_iter++) {
                    string oneBagPath = bag_iter->path().string();
                    if(0 == early.size()){
                        early = oneBagPath;
                    }else{
                        if(oneBagPath < early){
                            early = oneBagPath;
                        }
                    }
                }
                bagPath = early;
            }
        }
    }else{
        LOG(WARNING) << FLAGS_bag_dir << " FLAGS_bag_dir dont exist!!";
    }

    LOG(INFO) << "bagPath: " << bagPath;
    if(0 == bagPath.size()){
        LOG(WARNING) << bagPath << " cant find!";
        return;
    }
    if(!exists(bagPath)){
        LOG(WARNING) << bagPath << " dont exist!";
        return;
    }

//    string bagPath = baseDir + "/slam.bag";
//    if(!exists(bagPath)){
//        LOG(WARNING) << bagPath << " dont exist!";
//        return;
//    }

#endif
    Compress(baseDir);
    LOG(INFO) << baseDir << " start!";
    string offlineResultDir = baseDir + "/result";
    string pcdResultDir = offlineResultDir + "/pcd";
    string basePcdResultDir = offlineResultDir + "/basePcd";
    string guessPcdResultDir = offlineResultDir + "/guessPcd";
    string rawFrameResultDir = offlineResultDir + "/raw_frame";
    if(!exists(offlineResultDir)){
        create_directory(offlineResultDir);
    }
    int unused;
    unused = system(string("rm " + offlineResultDir + "/*").c_str());
    if(!exists(pcdResultDir)){
        create_directory(pcdResultDir);
    }else{
        unused = system(string("rm " + pcdResultDir + "/*").c_str());
    }
    if(!exists(basePcdResultDir)){
        create_directory(basePcdResultDir);
    }else{
        unused = system(string("rm " + basePcdResultDir + "/*").c_str());
    }
    if(!exists(guessPcdResultDir)){
        create_directory(guessPcdResultDir);
    }else{
        unused = system(string("rm " + guessPcdResultDir + "/*").c_str());
    }
    if(!exists(rawFrameResultDir)){
        create_directory(rawFrameResultDir);
    }else{
        unused = system(string("rm " + rawFrameResultDir + "/*").c_str());
    }
    string restoreParamDir = "/home/juefx/Documents/voxels/";
    if(!exists(baseDir + "/imu2base.calib")){
        unused = system(string("cp " + restoreParamDir + "/imu2base.calib " + baseDir).c_str());
    }
    if(!exists(baseDir + "/init_pose.txt")){
        unused = system(string("cp " + restoreParamDir + "/init_pose.txt " + baseDir).c_str());
    }
    if(!exists(baseDir + "/imu2base_lever_arm.txt")){
        unused = system(string("cp " + restoreParamDir + "/imu2base_lever_arm.txt " + baseDir).c_str());
    }
    if(!exists(baseDir + "/eskf_init_pose.txt")){
        unused = system(string("cp " + restoreParamDir + "/eskf_init_pose.txt " + baseDir).c_str());
    }
    if(!exists(baseDir + "/qrpconfig_auto.txt")){
        unused = system(string("cp " + restoreParamDir + "/qrpconfig_auto.txt " + baseDir).c_str());
    }

    Parameter::Instance()->init(baseDir);
    State::Instance()->AddState(IMU_CALIBING);
    if(PoseGuesser::Instance()->LoadSlamTraj(baseDir + "/traj.txt",
                                             baseDir + "/gps_traj.txt")){
        State::Instance()->AddState(TRAJ_HAS_GNSS);
    }
    SysController::Ptr sysController(new SysController());

//#ifdef ENABLE_ROS
//    string topic_imu = "/imu";
//    string topic_gnss = "/fix";
//    string topic_wheel = "/odom";
//    string topic_cloud = "/points2";
//    string topic_output_clouds = "/out_pointcloud";
//    string topic_output_odom = "/out_odom";
//    string topic_imu_pose = "/imu_pose";
//    ros::init(argc, argv, "localize");
//    ros::NodeHandle nh;
//    if(topic_imu.size()){
//        imuSubscriber.reset(new IMUSubscriber(sysController, &nh, topic_imu));
//    }
//    if(topic_wheel.size()){
//        wheelSubscriber.reset(new WheelSubscriber(sysController, &nh, topic_wheel));
//    }
//    if(topic_gnss.size()){
//        gnssSubscriber.reset(new GNSSSubscriber(sysController, &nh, topic_gnss));
//    }
//    if(topic_cloud.size()){
//        cloudSubscriber.reset(new CloudSubscriber(sysController, &nh, topic_cloud));
//    }
//    if(topic_cloud.size()){
//        imuPoseSubscriber.reset(new IMUPoseSubscriber(sysController, &nh, topic_imu_pose));
//    }
//    dynamic_cast<RosPublisher*>(RosPublisher::Instance().get())->SetRosHandle(&nh);
//    RosPublisher::Instance()->init(topic_output_odom, topic_output_clouds);
//    ros::AsyncSpinner spinner(4);
//    spinner.start();
#ifdef ENABLE_BAG
    string topic_output_clouds = "/out_pointcloud";
    string topic_output_odom = "/out_odom";
    ros::NodeHandle nh;
    dynamic_cast<RosPublisher*>(RosPublisher::Instance().get())->SetRosHandle(&nh);
    RosPublisher::Instance()->init(topic_output_odom, topic_output_clouds);
    RosBagReader bagReader(sysController, bagPath);
    bagReader.Play();
#else
//    DataReader::Instance()->Init(baseDir);
//    imuSubscriber.reset(new IMUReceiver(sysController));
//    wheelSubscriber.reset(new WheelReceiver(sysController));
//    gnssSubscriber.reset(new GNSSReceiver(sysController));
//    cloudSubscriber.reset(new CloudReceiver(sysController));
//    DataReader::Instance()->process();
#endif
    LOG(INFO) << baseDir << " Done!";
    sysController.reset();
    Parameter::Instance()->Reset();
    State::Instance()->Reset();
    PoseGuesser::Instance()->Reset();
    RosPublisher::Instance()->Reset();
}

int main(int argc, char *argv[])
{
    gflags::ParseCommandLineFlags(&argc, &argv, true);
    google::InitGoogleLogging(argv[0]);
    google::InstallFailureSignalHandler();
    google::InstallFailureWriter(&failureHandle);
    FLAGS_alsologtostderr = true;
    FLAGS_colorlogtostderr = true;
    FLAGS_stderrthreshold = 0;
    FLAGS_logbufsecs = 0;
    if(exists(FLAGS_base_dir)){
        FLAGS_log_dir = "/home/juefx/log";
        create_directory(FLAGS_log_dir);
    }
    ros::init(argc, argv, "localize");

    vector<string> taskBaseDirVec;
    if(is_directory(FLAGS_base_dir)){
        directory_iterator dir_iter(FLAGS_base_dir);
        directory_iterator end_iter;
        for(; dir_iter != end_iter; dir_iter++) {
            if (!is_directory(dir_iter->path())){
                continue;
            }
//            if(dir_iter->path().string() != "/home/juefx/Documents/data/chengdu/18_2"){
//                continue;
//            }
            string oneBasedir = dir_iter->path().string() + "/voxels/";
//            taskBaseDirVec.push_back(oneBasedir);
            Locate(oneBasedir);
        }
    }
//    boost::function<void(const string&)> locateFunc
//            = boost::bind(&Locate, _1);
//    syncProcess<string>(taskBaseDirVec, locateFunc);
    return 0;
}