#ifndef BASIC_FUNCTIONS_H
#define BASIC_FUNCTIONS_H

#include "basic_types.h"
#include <chrono>
using namespace chrono;

int8_t checkTime(const double &time, const Time_Duration &timeDuration);

int8_t checkTime(const double &time, const vector<Time_Duration> &timeDurations);

vector<pair<uint32_t, uint32_t>> getIndexPeriod(const uint32_t size,
                                                const uint8_t threadCnt = std::thread::hardware_concurrency());

template <typename T>
Eigen::Matrix<T, 3, 1> RotationQuaternionToEulerVector(
    const Eigen::Quaternion<T>& quaternion) {
  Eigen::Quaternion<T> n_q = quaternion.normalized();
  // We choose the quaternion with positive 'w', i.e., the one with a smaller
  // angle that represents this orientation.
  if (n_q.w() < 0.) {
    // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560
    n_q.w() = -1. * n_q.w();
    n_q.x() = -1. * n_q.x();
    n_q.y() = -1. * n_q.y();
    n_q.z() = -1. * n_q.z();
  }
  // We convert the normalized_quaternion(n_q) into a vector along the rotation
  // axis with length of the rotation angle.
  const T roll = atan2(2 * (n_q.w() * n_q.x() + n_q.y() * n_q.z()),
                       1 - 2 * (n_q.x() * n_q.x() + n_q.y() * n_q.y()));
  const T pitch = asin(2 * (n_q.w() * n_q.y() - n_q.z() * n_q.x()));
  const T yaw = atan2(2 * (n_q.w() * n_q.z() + n_q.x() * n_q.y()),
                      1 - 2 * (n_q.y() * n_q.y() + n_q.z() * n_q.z()));
  return Eigen::Matrix<T, 3, 1>(roll, pitch, yaw);
}

template<typename ItemType>
void periodProcessThreadFunc(vector<ItemType> *Vector,
                             pair<uint32_t, uint32_t> period,
                             boost::function<void(ItemType &)> processItemFunc)
{
    vector<ItemType> tempOutputVec;
    uint32_t start_index = period.first;
    uint32_t index_cnt = period.second;
    uint32_t final_index = start_index + index_cnt;
    for(uint32_t index = start_index; index < final_index; index++){
        processItemFunc(Vector->at(index));
    }
}

template<typename ItemType>
void multiThreadProcess(vector<ItemType> &Vector,
                        boost::function<void(ItemType &)> processItemFunc)
{
    auto start = system_clock::now();
    vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(Vector.size());
    vector<ThreadPtr> threadVec;
    boost::mutex mutex;
    for(size_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
        threadVec.emplace_back(
                    ThreadPtr(new boost::thread(boost::bind(&periodProcessThreadFunc<ItemType>,
                                                            &Vector,
                                                            indexPeriod[thread_index],
                                                            processItemFunc))));
    }
    for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
        threadVec[thread_index]->join();
        threadVec[thread_index].reset();

    }
    auto end   = system_clock::now();
    auto duration = duration_cast<milliseconds>(end - start);
    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds,"
              << " output size: " << Vector.size();
}

template<typename ItemType>
void periodProcessThreadFunc(const vector<ItemType> *inputVector,
                             vector<ItemType> *outputVector,
//                             boost::mutex *mutex,
                             pair<uint32_t, uint32_t> period,
                             boost::function<bool(const ItemType &,
                                                  ItemType &)> processItemFunc)
{
    vector<ItemType> tempOutputVec;
    uint32_t start_index = period.first;
    uint32_t index_cnt = period.second;
    uint32_t final_index = start_index + index_cnt;
    for(uint32_t index = start_index; index < final_index; index++){
        ItemType outputItem;
        if(processItemFunc(inputVector->at(index), outputItem)){
            outputVector->push_back(outputItem);
        }
    }
    static int i = 0;
//    boost::unique_lock<boost::mutex> guard(*mutex);
//    outputVector->insert(outputVector->end(),
//                         tempOutputVec.begin(),
//                         tempOutputVec.end());
}

template<typename ItemType>
void multiThreadProcess(const vector<ItemType> &inputVector,
                        vector<ItemType> &outputVector,
                        boost::function<bool(const ItemType &,
                                             ItemType &)> processItemFunc)
{
    auto start = system_clock::now();
    vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(inputVector.size());
    vector<ThreadPtr> threadVec;
    vector<vector<ItemType>> outputVecs(indexPeriod.size());
    boost::mutex mutex;
    for(size_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
        threadVec.emplace_back(
                    ThreadPtr(new boost::thread(boost::bind(&periodProcessThreadFunc<ItemType>,
                                                            &inputVector,
                                                            &outputVecs[thread_index],
//                                                            &mutex,
                                                            indexPeriod[thread_index],
                                                            processItemFunc))));
    }
    for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
        threadVec[thread_index]->join();
        threadVec[thread_index].reset();
        outputVector.insert(outputVector.end(),
                            outputVecs[thread_index].begin(),
                            outputVecs[thread_index].end());
    }
    auto end   = system_clock::now();
    auto duration = duration_cast<milliseconds>(end - start);
    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds,"
              << " output size: " << outputVector.size();
}



template<typename ItemType>
void periodProcessThreadFuncNoOutput(const vector<ItemType> *inputVector_1,
                                     const vector<ItemType> *inputVector_2,
                                     pair<uint32_t, uint32_t> period,
                                     boost::function<void(const ItemType &,
                                                          const ItemType &)> processItemFunc)
{
    uint32_t start_index = period.first;
    uint32_t index_cnt = period.second;
    uint32_t final_index = start_index + index_cnt;
    for(uint32_t index = start_index; index < final_index; index++){
        processItemFunc(inputVector_1->at(index),
                        inputVector_2->at(index));
    }
    LOG(INFO) << start_index / index_cnt;
}

template<typename ItemType>
void multiThreadProcessNoOutput(const vector<ItemType> &inputVector_1,
                                const vector<ItemType> &inputVector_2,
                                boost::function<void(const ItemType &,
                                                     const ItemType &)> processItemFunc)
{
    auto start = system_clock::now();
    if(inputVector_1.size() != inputVector_2.size()){
        return;
    }
    vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(inputVector_1.size());
    vector<ThreadPtr> threadVec;
    for(size_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
        threadVec.emplace_back(
                    ThreadPtr(new boost::thread(boost::bind(&periodProcessThreadFuncNoOutput<ItemType>,
                                                            &inputVector_1,
                                                            &inputVector_2,
                                                            indexPeriod[thread_index],
                                                            processItemFunc))));
    }
    for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){
        threadVec[thread_index]->join();
        threadVec[thread_index].reset();
    }
    auto end   = system_clock::now();
    auto duration = duration_cast<milliseconds>(end - start);
    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds";
}

///////////////////////////////////////////////////////////////////////////////////////////////

template<typename ItemType>
void syncProcessThreadFunc(const vector<ItemType> *inputVector,
                           vector<ItemType> *outputVector,
                           int32_t *currentIndex,
//                           boost::mutex *mutex,
                           boost::function<bool(const ItemType &,
                                                ItemType &)> processItemFunc)
{
    uint32_t size = inputVector->size();
    int32_t index;
    while(true){
        {
//            boost::unique_lock<boost::mutex> guard(*mutex);
            index = ++(*currentIndex);
        }
        if((uint32_t)index >= size){
//            LOG(INFO) << "index: " << index;
            break;
        }
        ItemType outputItem;
        if(processItemFunc(inputVector->at((uint32_t)index), outputItem)){
            outputVector->push_back(outputItem);
        }
    }
}

template<typename ItemType>
void syncProcess(const vector<ItemType> &inputVector,
                 vector<ItemType> &outputVector,
                 boost::function<bool(const ItemType &,
                                      ItemType &)> processItemFunc)
{
    auto start = system_clock::now();
    int32_t currentIndex = -1;
    boost::mutex mutex;
    vector<ThreadPtr> threadVec;
    vector<vector<ItemType>> outputVecs(MAX_THREAD_CNT);
    for(size_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
        threadVec.emplace_back(
                    ThreadPtr(new boost::thread(boost::bind(&syncProcessThreadFunc<ItemType>,
                                                            &inputVector,
                                                            &outputVecs[thread_index],
                                                            &currentIndex,
//                                                            &mutex,
                                                            processItemFunc))));
    }
    for(uint8_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
        threadVec[thread_index]->join();
        threadVec[thread_index].reset();
        outputVector.insert(outputVector.end(),
                            outputVecs[thread_index].begin(),
                            outputVecs[thread_index].end());
    }
    auto end   = system_clock::now();
    auto duration = duration_cast<milliseconds>(end - start);
    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds,"
              << " output size: " << outputVector.size();
}

template<typename ItemType>
void syncProcessThreadFunc(const vector<ItemType> *inputVector,
                           int32_t *currentIndex,
//                           boost::mutex *mutex,
                           boost::function<void(const ItemType &)> processItemFunc)
{
    uint32_t size = inputVector->size();
    int32_t index;
    while(true){
        {
//            boost::unique_lock<boost::mutex> guard(*mutex);
            index = ++(*currentIndex);
        }
        if((uint32_t)index >= size){
            LOG(INFO) << "index: " << index;
            break;
        }
        processItemFunc(inputVector->at((uint32_t)index));
    }
}

template<typename ItemType>
void syncProcess(const vector<ItemType> &inputVector,
                 boost::function<void(const ItemType &)> processItemFunc)
{
    auto start = system_clock::now();
    int32_t currentIndex = -1;
    boost::mutex mutex;
    vector<ThreadPtr> threadVec;
    for(size_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
        threadVec.emplace_back(
                    ThreadPtr(new boost::thread(boost::bind(&syncProcessThreadFunc<ItemType>,
                                                            &inputVector,
                                                            &currentIndex,
//                                                            &mutex,
                                                            processItemFunc))));
    }
    for(uint8_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
        threadVec[thread_index]->join();
        threadVec[thread_index].reset();
    }
    auto end   = system_clock::now();
    auto duration = duration_cast<milliseconds>(end - start);
    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds";
}


template<typename ItemType>
void syncProcessThreadFuncNoOutput(const vector<ItemType> *inputVector_1,
                                   const vector<ItemType> *inputVector_2,
                                   int32_t *currentIndex,
                                   boost::function<void(const ItemType &,
                                                        const ItemType &)> processItemFunc)
{
    uint32_t size = inputVector_1->size();
    int32_t index;
    while(true){
        index = ++(*currentIndex);
        if((uint32_t)index >= size){
//            LOG(INFO) << "index: " << index;
            break;
        }
        processItemFunc(inputVector_1->at(index),
                        inputVector_2->at(index));
    }
}

template<typename ItemType>
void syncProcessNoOutput(const vector<ItemType> &inputVector_1,
                         const vector<ItemType> &inputVector_2,
                         boost::function<void(const ItemType &,
                                              const ItemType &)> processItemFunc)
{
    auto start = system_clock::now();
    if(inputVector_1.size() != inputVector_2.size()){
        return;
    }
    vector<ThreadPtr> threadVec;
    int32_t currentIndex = -1;
    for(size_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
        threadVec.emplace_back(
                    ThreadPtr(new boost::thread(boost::bind(&syncProcessThreadFuncNoOutput<ItemType>,
                                                            &inputVector_1,
                                                            &inputVector_2,
                                                            &currentIndex,
                                                            processItemFunc))));
    }
    for(uint8_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){
        threadVec[thread_index]->join();
        threadVec[thread_index].reset();
    }
    auto end   = system_clock::now();
    auto duration = duration_cast<milliseconds>(end - start);
    LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds";
}


template <typename PointType>
void addOffset(const Vector3d &offset,
               pcl::PointCloud<PointType> &pointcloud){
    for(auto &p : pointcloud){
        p.x += offset.x();
        p.y += offset.y();
        p.z += offset.z();
    }
    pointcloud.sensor_origin_ += Vector4f{offset.x(), offset.y(), offset.z(), 0};
}

#include <boost/filesystem.hpp>
#include <boost/optional.hpp>
#include <pcl/filters/voxel_grid.h>

using namespace boost::filesystem;
template<typename PointType>
void downSamplePcd(const string &path)
{
    using PointCloud = pcl::PointCloud<PointType>;
    if(!exists(path)){
        return;
    }
    directory_iterator file_iter(path);
    directory_iterator pcd_end_iter;
    for(; file_iter != pcd_end_iter; file_iter++) {
        string subffix = file_iter->path().extension().string();
        if(subffix == ".pcd"){
            string stem = file_iter->path().stem().string();
            auto it = stem.find("view");
            if(it != string::npos){
                continue;
            }
            string pcd_path = file_iter->path().string();
            PointCloud pointcloud;
            pcl::io::loadPCDFile(pcd_path, pointcloud);
            typename PointCloud::Ptr input, points_filtered;
            pcl::VoxelGrid<PointType> grid;

            points_filtered.reset(new PointCloud);
            input.reset(new PointCloud(pointcloud));

            grid.setLeafSize(0.4, 0.4, 0.4);
            grid.setInputCloud(input);
            grid.filter(*points_filtered);
            string output = path + "/view/";
            if(!exists(output)){
                create_directories(output);
            }
            pcl::io::savePCDFileBinary(output + stem +"_view.pcd", *points_filtered);
            LOG(INFO) << output + stem +"_view.pcd saved";
        }
    }

//    string output = "/data/lidar/2147/pcd_cart_new/";
//    if(!exists(output)){
//        create_directories(output);
//    }
//    for(; file_iter != pcd_end_iter; file_iter++) {
//        string stem = file_iter->path().stem().string();
//        string subffix = file_iter->path().extension().string();
//        if(subffix == ".pcd"){
//            string pcd_path = file_iter->path().string();
//            PointCloud pointcloud;
//            pcl::io::loadPCDFile(pcd_path, pointcloud);
//            Vector3d offset = Vector3d(-1, 0, 0);
//            addOffset<PointType>(offset, pointcloud);
//            pcl::io::savePCDFileBinary(output + stem +".pcd", pointcloud);
//            LOG(INFO) << output + stem +".pcd saved";
//        }
//    }
}

#endif // BASIC_FUNCTIONS_H