#include <QCoreApplication>

#include "lidar_obstacle_detector/obstacle_detector.hpp"

#include <iostream>

#include "modulecomm.h"

#include "ivstdcolorout.h"

#include "objectarray.pb.h"

using namespace lidar_obstacle_detector;

// Pointcloud Filtering Parameters
static bool USE_PCA_BOX;
static bool USE_TRACKING;
static float VOXEL_GRID_SIZE;
static Eigen::Vector4f ROI_MAX_POINT, ROI_MIN_POINT;
static float GROUND_THRESH;
static float CLUSTER_THRESH;
static int CLUSTER_MAX_SIZE, CLUSTER_MIN_SIZE;
static float DISPLACEMENT_THRESH, IOU_THRESH;

static size_t obstacle_id_;
static std::string bbox_target_frame_, bbox_source_frame_;
static std::vector<Box> prev_boxes_, curr_boxes_;

static std::shared_ptr<ObstacleDetector<pcl::PointXYZ>> obstacle_detector;

static void * gpa_cluster,*gpa_ground,* gpa_detect;


struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

void publishDetectedObjects(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>&& cloud_clusters)
{

    iv::lidar::objectarray  lidarobjvec;
  for (auto& cluster : cloud_clusters)
  {
    // Create Bounding Boxes
    Box box = USE_PCA_BOX?
      obstacle_detector->pcaBoundingBox(cluster, obstacle_id_) :
      obstacle_detector->axisAlignedBoundingBox(cluster, obstacle_id_);

    obstacle_id_ = (obstacle_id_ < SIZE_MAX)? ++obstacle_id_ : 0;
    curr_boxes_.emplace_back(box);
  }

  // Re-assign Box ids based on tracking result
  if (USE_TRACKING)
    obstacle_detector->obstacleTracking(prev_boxes_, curr_boxes_, DISPLACEMENT_THRESH, IOU_THRESH);


  ivstdcolorout("Print Objects:",iv::STDCOLOR_BOLDGREEN);

  // Transform boxes from lidar frame to base_link frame, and convert to jsk and autoware msg formats
  for (auto& box : curr_boxes_)
  {
      std::cout<<"     box x y z "<<box.position(0)<<" "<<box.position(1)<<" "<<box.position(2)<<std::endl;
//    geometry_msgs::Pose pose, pose_transformed;
//    pose.position.x = box.position(0);
//    pose.position.y = box.position(1);
//    pose.position.z = box.position(2);
//    pose.orientation.w = box.quaternion.w();
//    pose.orientation.x = box.quaternion.x();
//    pose.orientation.y = box.quaternion.y();
//    pose.orientation.z = box.quaternion.z();
//    tf2::doTransform(pose, pose_transformed, transform_stamped);

//    jsk_bboxes.boxes.emplace_back(transformJskBbox(box, bbox_header, pose_transformed));
//    autoware_objects.objects.emplace_back(transformAutowareObject(box, bbox_header, pose_transformed));

      iv::lidar::lidarobject * plidarobj = lidarobjvec.add_obj();

      iv::lidar::PointXYZ center;
      iv::lidar::Dimension xdim;
      center.set_x(box.position(0));
      center.set_y(box.position(1));
      center.set_z(box.position(2));
      xdim.set_x(box.dimension(0));
      xdim.set_y(box.dimension(1));
      xdim.set_z(box.dimension(2));

      EulerAngles xAng;
      Quaternion xQuat;
      xQuat.w = box.quaternion.w();
      xQuat.x = box.quaternion.x();
      xQuat.y = box.quaternion.y();
      xQuat.z = box.quaternion.z();
      xAng = ToEulerAngles(xQuat);

      plidarobj->set_id(box.id);
      plidarobj->set_tyaw(xAng.yaw);
      plidarobj->mutable_position()->CopyFrom(center);
      plidarobj->mutable_centroid()->CopyFrom(center);
      plidarobj->mutable_dimensions()->CopyFrom(xdim);


  }


  unsigned int ndatasize = lidarobjvec.ByteSize();
  std::shared_ptr<char> pstr_data = std::shared_ptr<char>(new char[ndatasize]);
  if(lidarobjvec.SerializeToArray(pstr_data.get(),static_cast<int>(ndatasize) ))
  {
      iv::modulecomm::ModuleSendMsg(gpa_detect,pstr_data.get(),ndatasize);
  }
  else
  {
      ivstdcolorout("Serialzie detect error.",iv::STDCOLOR_BOLDRED);
  }
//   char * strout = lidarobjtostr(lidarobjvec,ntlen);



  // Update previous bounding boxes
  prev_boxes_.swap(curr_boxes_);
  curr_boxes_.clear();
}

void sharepointcloud(void * pa,pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
{
    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];

    int * pHeadSize =  reinterpret_cast<int * >(strOut);
    *pHeadSize = static_cast<int >(4 + point_cloud->header.frame_id.size()+4+8) ;
    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
    uint32_t * pu32 =  reinterpret_cast<uint32_t *>(strOut+4+sizeof(point_cloud->header.frame_id.size()));
    *pu32 = point_cloud->header.seq;
    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
    pcl::PointXYZI * p;
    p = reinterpret_cast<pcl::PointXYZI *>(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
    std::cout<<"width: "<<point_cloud->width<<std::endl;

    iv::modulecomm::ModuleSendMsg(pa,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));

    delete [] strOut;
}

void sharepointxyz( pcl::PointCloud<pcl::PointXYZ>::Ptr &raw_cloud,void * pa)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());

    point_cloud->header.frame_id = "velodyne";
    point_cloud->header.seq = 1;
    point_cloud->header.stamp = 0;

    int nPCount = raw_cloud->points.size();
    std::cout<<" size: "<<nPCount<<std::endl;
    int i;
    for(i=0;i<nPCount;i++)
    {
        pcl::PointXYZI xp;
        xp.x = raw_cloud->points.at(i).x;
         xp.y = raw_cloud->points.at(i).y;
          xp.z = raw_cloud->points.at(i).z;
           xp.intensity = 100;
           point_cloud->push_back(xp);
    }

    sharepointcloud(pa,point_cloud);
}

void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZ>::Ptr &raw_cloud)
{
    // Downsampleing, ROI, and removing the car roof
    auto filtered_cloud = obstacle_detector->filterCloud(raw_cloud, VOXEL_GRID_SIZE, ROI_MIN_POINT, ROI_MAX_POINT);

    // Segment the groud plane and obstacles
    auto segmented_clouds = obstacle_detector->segmentPlane(filtered_cloud, 30, GROUND_THRESH);

    // Cluster objects
    auto cloud_clusters = obstacle_detector->clustering(segmented_clouds.first, CLUSTER_THRESH, CLUSTER_MIN_SIZE, CLUSTER_MAX_SIZE);

    publishDetectedObjects(std::move(cloud_clusters));

    sharepointxyz(segmented_clouds.first,gpa_cluster);
    sharepointxyz(segmented_clouds.second,gpa_ground);
}

void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize <=16)return;
    unsigned int * pHeadSize = (unsigned int *)strdata;
    if(*pHeadSize > nSize)
    {
        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
    }

    QTime xTime;
    xTime.start();

    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZ>());
    int nNameSize;
    nNameSize = *pHeadSize - 4-4-8;
    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
    point_cloud->header.frame_id = strName;
    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
    int i;
    pcl::PointXYZI * p;
    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
    for(i=0;i<nPCount;i++)
    {
        pcl::PointXYZ xp;
//        if((p->x<-30)||(p->x>30)||(p->y>50)||(p->y<-50))
//        {

//        }
//        else
//        {
//            memcpy(&xp,p,sizeof(pcl::PointXYZI));
//            xp.z = xp.z;
            xp.x = p->x;
            xp.y = p->y;
            xp.z = p->z;
            point_cloud->push_back(xp);

//        }
        p++;
//        xp.x = p->x;
//        xp.y = p->y;
//        xp.z = p->z;
//        xp.intensity = p->intensity;
//        point_cloud->push_back(xp);
//        p++;
    }

//    std::cout<<"pcl time is "<<xTime.elapsed()<<std::endl;

    DectectOnePCD(point_cloud);

    std::cout<<"time is "<<(QDateTime::currentMSecsSinceEpoch() % 1000)<<" use: "<<xTime.elapsed()<<std::endl;

}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    USE_PCA_BOX = false;
    USE_TRACKING = true;
    VOXEL_GRID_SIZE = 0.2;
    ROI_MAX_POINT = Eigen::Vector4f(70,30,1,1);
    ROI_MIN_POINT = Eigen::Vector4f(-30,-30,-2.5, 1);
    GROUND_THRESH = 0.3;
    CLUSTER_THRESH = 0.6;
    CLUSTER_MAX_SIZE = 5000;
    CLUSTER_MIN_SIZE = 10;
    DISPLACEMENT_THRESH = 1.0;
    IOU_THRESH = 1.0;


    obstacle_detector = std::make_shared<ObstacleDetector<pcl::PointXYZ>>();

    gpa_detect = iv::modulecomm::RegisterSend("lidar_pointpillar",10000000,1);

    gpa_cluster = iv::modulecomm::RegisterSend("lidarpc_cluster",10000000,1);
    gpa_ground  = iv::modulecomm::RegisterSend("lidarpc_ground",10000000,1);
    void * pa = iv::modulecomm::RegisterRecv("lidar_pc",ListenPointCloud);

    return a.exec();
}