//
// Created by zx on 2021/8/27.
//
#include "rotate_center_caliber.h"
#include "point_tool.h"
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include "tool/pathcreator.h"
#include "proto_tool.h"
#include <iostream>
#include <glog/logging.h>

//using google::protobuf::io::FileInputStream;
//using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;

GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
{
    time_t tt;
    time( &tt );
    tt = tt + 8*3600;  // transform the time zone
    tm* t= gmtime( &tt );
    char buf[255]={0};
    sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
            t->tm_year + 1900,
            t->tm_mon + 1,
            t->tm_mday,
            t->tm_hour,
            t->tm_min,
            t->tm_sec);

    FILE* tp_file=fopen(buf,"w");
    fprintf(tp_file,data,strlen(data));
    fclose(tp_file);

}

void init_glog()
{
    time_t tt = time(0);//时间cuo
    struct tm* t = localtime(&tt);

    char strYear[255]={0};
    char strMonth[255]={0};
    char strDay[255]={0};

    sprintf(strYear,"%04d", t->tm_year+1900);
    sprintf(strMonth,"%02d", t->tm_mon+1);
    sprintf(strDay,"%02d", t->tm_mday);

    char buf[255]={0};
    getcwd(buf,255);
    char strdir[255]={0};
    sprintf(strdir,"%s/caliber_log/%s/%s/%s", buf,strYear,strMonth,strDay);
    PathCreator creator;
    creator.Mkdir(strdir);

    char logPath[255] = { 0 };
    sprintf(logPath, "%s/", strdir);
    FLAGS_max_log_size = 100;
    FLAGS_logbufsecs = 0;
    google::InitGoogleLogging("shutter_verify");
    google::SetStderrLogging(google::INFO);
    google::SetLogDestination(0, logPath);
    google::SetLogFilenameExtension("zxlog");
    google::InstallFailureSignalHandler();
    google::InstallFailureWriter(&shut_down_logging);
    FLAGS_colorlogtostderr = true;        // Set log color
    FLAGS_logbufsecs = 0;                // Set log output speed(s)
    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
    FLAGS_stop_logging_if_full_disk = true;
}


pcl::visualization::PCLVisualizer viewer;

pcl::PointCloud<pcl::PointXYZ>::Ptr rotation_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
        float theta,float ox,float oy)
{

    //先平移点云，后旋转,再平移
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    transform_2.translation() << -ox, -oy, 0;
    pcl::transformPointCloud(*cloud, *transformed_cloud, transform_2);

    Eigen::Affine3f transform_r = Eigen::Affine3f::Identity();
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud1(new pcl::PointCloud<pcl::PointXYZ>());
    transform_r.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
    pcl::transformPointCloud(*transformed_cloud, *transformed_cloud1, transform_r);

    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud2(new pcl::PointCloud<pcl::PointXYZ>());
    Eigen::Affine3f transform_t = Eigen::Affine3f::Identity();
    transform_t.translation() << ox, oy, 0;
    pcl::transformPointCloud(*transformed_cloud1, *transformed_cloud2, transform_t);
    return transformed_cloud2;
}

#include "tick.hpp"
#include <thread>
#include <mutex>
std::mutex mut;

void run()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=ReadTxtCloud("./center_caliber_cloud_sample3.txt");
    LOG(INFO)<<"  cloud size:"<<cloud->size();

    mut.lock();
    viewer.addPointCloud(cloud,"cloud_src");
    mut.unlock();

    rotate_center_caliber caliber;
    caliber.set_first_frame(cloud);

    float rotation_x=6.8;
    float rotation_y=-3.5;

    float theta=0;
    for(theta=-7./180.*M_PI;theta<8./180.*M_PI;theta+=1./180.*M_PI)
    {

        pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud=rotation_cloud(cloud,theta,rotation_x,rotation_y);
        Tick tick;
        caliber.push_cloud(transform_cloud);
        std::cout<<"time : "<<tick.tic_relatively()<<std::endl;
        usleep(1000*500);
    }
}

int main()
{
    init_glog();
    viewer.addCoordinateSystem(1.0,0,0,0,"car_center");


    std::thread* thread=new std::thread(run);

    while(1)
    {
        mut.lock();
        viewer.spinOnce();
        mut.unlock();
        usleep(50*1000);
    }
    return 0;
}