#pragma once

#if defined(SG_API_LIBRARY)
#  define SG_APISHARED_EXPORT __declspec(dllexport)
#else
#  define SG_APISHARED_EXPORT __declspec(dllimport)
#endif

#define OUTPUT_DEBUG	1
#define BAG_ALGO_USE_CORNER_FEATURE	1
#define RGN_HIST_SIZE	16 //目标颜色统计的数目

#include "SG_baseDataType.h"
#include <vector>

typedef struct
{
	SSG_bagParam bagParam;
	SSG_outlierFilterParam filterParam;
#if BAG_ALGO_USE_CORNER_FEATURE
	SSG_cornerParam cornerParam;
#else
	SSG_slopeParam slopeParam;
	SSG_VFeatureParam valleyPara;
#endif
	SSG_treeGrowParam growParam;
	//SSG_objSortParam sortParam;
	//SSG_polarScanParam polarScanParam;
}SG_bagPositionParam;

typedef struct
{
	double hueTh;
	double saturateTh;
	double FBVldPtRatioTh; //正反两面有效颜色点的比例门限
	bool frontVldPtGreater; //true：有效颜色比例高的点的是正面；false：有效颜色比例高的点的是反面
	bool front_upVldPtGreater;//true：有效颜色比例高的点的是正面朝上；false：有效颜色比例高的点的是正面朝下
	bool back_upVldPtGreater; //true：有效颜色比例高的点的是反面朝上；false：有效颜色比例高的点的是反面朝下
}SSG_hsvCmpParam;

typedef struct
{
	double jumpTh;  //和现场环境有关，为托盘的孔洞的Z跳变
	double baseHoleLen; //和现场有关，托盘孔洞的长度
	double baseHoleDist; //托盘两个孔洞的距离
}SSG_stackBaseParam;

//数据调平
SG_APISHARED_EXPORT void sg_lineDataR(
	SVzNL3DLaserLine* a_line,
	const double* camPoseR,
	double groundH);
SG_APISHARED_EXPORT void sg_lineDataR_RGBD(
	SVzNLXYZRGBDLaserLine* a_line,
	const double* camPoseR,
	double groundH);

SG_APISHARED_EXPORT void sg_bagPositioning_lineProc(
	SVzNL3DLaserLine* a_line, 
	int lineIdx, 
	int* errCode, 
	std::vector<SSG_lineFeature>& all_vLineFeatures, 
	std::vector<std::vector<int>>& noisePts, 
	const SG_bagPositionParam algoParam);

//获取编织袋中心抓取点姿态，从顶部抓取
SG_APISHARED_EXPORT void sg_getBagPosition(
	SVzNL3DLaserLine* laser3DPoints, 
	int lineNum, 
	//std::vector<SSG_lineFeature>& all_vLineFeatures, 
	//std::vector<std::vector<int>>& noisePts, 
	const SG_bagPositionParam algoParam, 
	const SSG_planeCalibPara poseCalibPara,
	std::vector<SSG_peakRgnInfo>& objOps);

SG_APISHARED_EXPORT void sg_getBagPositionAndOrientation(
	SVzNLXYZRGBDLaserLine* laser3DPoints,
	int lineNum,
	//std::vector<SSG_lineFeature>& all_vLineFeatures, 
	//std::vector<std::vector<int>>& noisePts, 
	const SG_bagPositionParam algoParam,
	const SSG_planeCalibPara poseCalibPara,
	const SSG_hsvCmpParam colorCmpParam,
	const RGB rgbColorPattern,  //用于区分袋子方向的颜色
	const double frontColorTemplate[RGN_HIST_SIZE],
	const double backColorTemplate[RGN_HIST_SIZE],
	std::vector<SSG_peakOrienRgnInfo>& objOps,
#if OUTPUT_DEBUG
	std::vector<std::vector< SVzNL3DPosition>>& bagPositionCloudPts,
#endif
	int* errCode);

SG_APISHARED_EXPORT void sg_sideBagPosition(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	const SG_bagPositionParam algoParam,
	std::vector<SSG_sideBagInfo>& objOps);

SG_APISHARED_EXPORT void sg_getSideBagStackBasePosition(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	const SSG_stackBaseParam stackBaseParam,
	const SSG_treeGrowParam growParam,
	SSG_6DOF* stackBasePosition  //垛的托盘的中心位置和角度
);

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getBagBaseCalibPara(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum);