#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include <vector>
#include <opencv2/opencv.hpp>

//在searchWin中搜索最高点(z最小)
void _findPeak(SVzNL3DLaserLine* scanLines, int lineNum, SVzNL2DPoint LTpos, SSG_localPkParam searchWin, SSG_2DValueI* pkPos)
{
    int line_end = LTpos.x + searchWin.seachW_lines;
    if (line_end >= lineNum)
        line_end = lineNum;
    int ptNum = scanLines->nPositionCnt;
    int pt_end = LTpos.y + searchWin.searchW_pts;
    if (pt_end >= ptNum)
        pt_end = ptNum;

    SSG_2DValueI peak = { -1, -1, 0, 0};
    double minZ = -1;
    for (int i = LTpos.x; i < line_end; i++)
    {
        for (int j = LTpos.y; j < pt_end; j++)
        {
            SVzNL3DPosition* a_pt = &scanLines[i].p3DPosition[j];
            if (a_pt->pt3D.z < 1e-4)
                continue;

            if (minZ < 0)
            {
                peak = { i, j , 0, a_pt->pt3D.z };
                minZ = a_pt->pt3D.z;
            }
            else
            {
                if (minZ > a_pt->pt3D.z)
                {
                    peak = { i, j, 0,  a_pt->pt3D.z };
                    minZ = a_pt->pt3D.z;
                }
            }
        }
    }
    *pkPos = peak;
    return;
}

///搜索局部最高点（z最小点）。
///搜索方法：每次步进搜索窗口长度的一半。对局部最高点进行标记，防止被重复记录。
void sg_getLocalPeaks(SVzNL3DLaserLine* scanLines, int lineNum, std::vector<SSG_2DValueI>& peaks, SSG_localPkParam searchWin)
{
    int ptNum = scanLines->nPositionCnt;
    cv::Mat mask = cv::Mat::zeros(ptNum, lineNum, CV_32SC1);
    int winNum_cols = lineNum / (searchWin.seachW_lines/2);
    if ((lineNum % (searchWin.seachW_lines/2)) > 0)
        winNum_cols = winNum_cols + 1;
    int winNum_rows = ptNum / (searchWin.searchW_pts/2);
    if ((ptNum % (searchWin.searchW_pts/2)) > 0)
        winNum_rows = winNum_rows + 1;

    for (int i = 0; i < winNum_rows; i++)
    {
        for (int j = 0; j < winNum_cols; j++)
        {
            SVzNL2DPoint LTpos = {i* searchWin.seachW_lines / 2, j* searchWin.searchW_pts / 2 };
            SSG_2DValueI pkPos = { -1, -1, 0, 0 };
            _findPeak(scanLines, lineNum, LTpos, searchWin, &pkPos);
            if ((pkPos.x >= 0) && (pkPos.y >= 0))
            {
                //边界处的极值点去除
                if ((pkPos.x != LTpos.x) && (pkPos.x != (LTpos.x + searchWin.seachW_lines - 1)) &&
                    (pkPos.y != LTpos.y) && (pkPos.y != (LTpos.y + searchWin.searchW_pts - 1)))
                {
                    int maskValue = mask.at<int>(pkPos.y, pkPos.x);
                    if (maskValue == 0)
                    {
                        peaks.push_back(pkPos);
                        mask.at<int>(pkPos.y, pkPos.x) = 1;
                    }
                }
            }
        }
    }
    return;
}

void _polarScan(cv::Mat& edgeMask, SVzNL2DPoint scanSeed, SSG_polarScanParam polarScanParam, std::vector< SSG_2DValueI>& contours, int* maxEdgeId)
{
    int scanTime = (int)(360.0 / polarScanParam.angleStep);
    int edgeId = 0;
    for (int i = 0; i < scanTime; i++)
    {
        double angle = i * polarScanParam.angleStep;
        double sinTheta = sin(angle * PI / 180);
        double cosTheta = cos(angle * PI / 180);
        double r = 0;
        while (1)
        {
            r += polarScanParam.radiusStep;
            int x = (int)(scanSeed.x + r * cosTheta);
            int y = (int)(scanSeed.y - r * sinTheta);
            if ((x >= edgeMask.cols) || (x < 0) ||
                (y >= edgeMask.rows) || (y < 0))
            {
                //SSG_2DValueI edge_pt = { -1,-1, -1, -1 };
                //contours.push_back(edge_pt);
                break;
            }
            else
            {
                int maskValue = edgeMask.at<int>(y, x);
                if (maskValue > 0)
                {
                    edgeId = edgeId < maskValue ? maskValue : edgeId;
                    SSG_2DValueI edge_pt = { x, y, maskValue, r };
                    contours.push_back(edge_pt);
                    break;
                }
            }
        }
    }
    *maxEdgeId = edgeId;
    return;
}

void _getRgnVldContour(std::vector< SSG_2DValueI>& contourPts, std::vector< SSG_2DValueI>& vldContour, int maxEdgeId, const int noiseRunLenTh)
{
    std::vector<int> idIndexing(maxEdgeId + 1);
    //游程分析
    std::vector< SSG_RUN> contourRuns;
    SSG_RUN a_run = { -1,-1,-1};
    for (int i = 0, i_max = (int)contourPts.size(); i < i_max; i++)
    {
        SSG_2DValueI* a_pt = &contourPts[i];
        if (a_pt->x < 0)
            continue;

        if (a_run.start < 0)
        {
            a_run.start = i;
            a_run.len = 1;
            a_run.value = a_pt->value;
        }
        else
        {
            if (a_run.value == a_pt->value)
                a_run.len++;
            else
            {
                contourRuns.push_back(a_run);
                //新的游程
                a_run.start = i;
                a_run.len = 1;
                a_run.value = a_pt->value;
            }
       }
    }
    //处理最后一个
    if (contourRuns.size() > 0)
    {
        //检查是否和第一个合并
        SSG_RUN* first_run = &contourRuns[0];
        if (first_run->value == a_run.value)
        {
            first_run->start = a_run.start;
            first_run->len += a_run.len;
        }
        else
            contourRuns.push_back(a_run);
    }
    else
        contourRuns.push_back(a_run);

    if (contourRuns.size() > 0)
    {
        //保留长的边界，小的短的边界可能是噪声
        for (int i = 0, i_max = (int)contourRuns.size(); i < i_max; i++)
        {
            SSG_RUN* curr_run = &contourRuns[i];
            if(curr_run->len > noiseRunLenTh)
            {
                idIndexing[curr_run->value] = 1;
            }
        }
        for (int i = 0, i_max = (int)contourPts.size(); i < i_max; i++)
        {
            SSG_2DValueI a_pt = contourPts[i];
            if (a_pt.x < 0)
                continue;

            if (idIndexing[a_pt.value] > 0)
                vldContour.push_back(a_pt);
        }
    }
    return;
}
/// <summary>
/// 区域生长法：以局部最高点作为生长种子进行生长
/// 生长方法与一般的区域生长不同：以种子点为圆心作圆周扫描，记录扫描到的边界
/// </summary>
/// <param name="scanLines"></param>
/// <param name="lineNum"></param>
/// <param name="edgeMask"></param>
/// <param name="peaks"></param>
void sg_peakPolarScan(cv::Mat& edgeMask, SVzNL2DPoint a_peak, SSG_polarScanParam polarScanParam, std::vector< SSG_2DValueI>& rgnContour)
{
    std::vector< SSG_2DValueI> contours;
    int maxEdgeId = 0;
    _polarScan(edgeMask, a_peak, polarScanParam, contours, &maxEdgeId);

    int noiseRunLenTh = (int)(11.25 / polarScanParam.angleStep); //小于22.5度的边界为噪声
    _getRgnVldContour(contours, rgnContour, maxEdgeId, noiseRunLenTh);
    return;
}

//取出指定的contour点
void sg_getContourPts(
    std::vector< SSG_lineConotours>& contour_all, 
    int vldEdgeId,
    std::vector< SSG_2DValueI>& contourFilter, 
    int* lowLevelFlag)
{
    *lowLevelFlag = 0;
    //生成有效contour
    //统计block(遮挡)的比例
    int blockPtNum = 0;
    for (int m = 0, m_max = (int)contour_all.size(); m < m_max; m++)
    {
        std::vector<SSG_contourPtInfo>& a_line_contourPts = contour_all[m].contourPts;
        for (int i = 0, i_max = (int)a_line_contourPts.size(); i < i_max; i++)
        {
            SSG_contourPtInfo* a_contourPt = &a_line_contourPts[i];
            int an_edgeIdx = a_contourPt->edgeId;
            if (an_edgeIdx == vldEdgeId)
            {
                if (a_contourPt->blockFlag > 0)
                    blockPtNum++;
                SSG_2DValueI a_filterPt = { a_contourPt->x, a_contourPt->y, a_contourPt->type, a_contourPt->edgePt.z };
                contourFilter.push_back(a_filterPt);
            }
        }
    }
    blockPtNum = blockPtNum * 2;
    if (blockPtNum > contourFilter.size())
        *lowLevelFlag = 1;
    return;
}

//取出指定的contour点
void sg_getPairingContourPts(
    std::vector<SSG_conotourPair>& contourPairs,
    std::vector<SSG_intPair>& idPairs,
    std::vector< SSG_conotourPair>& contourFilter,
    SVzNLRangeD range,
    bool isTBDir, 
    int* lowLevelFlag_0,
    int* lowLevelFlag_1)
{
    *lowLevelFlag_0 = 0;
    *lowLevelFlag_1 = 0;
    //生成有效contour
    //统计block(遮挡)的比例
    int blockPtNum_0 = 0;
    int blockPtNum_1 = 0;
    for (int m = 0, m_max = (int)contourPairs.size(); m < m_max; m++)
    {
        SSG_conotourPair& a_ptPair = contourPairs[m];
        for (int i = 0, i_max = (int)idPairs.size(); i < i_max; i++)
        {
            SSG_intPair& a_idPair = idPairs[i];
            if ( (a_ptPair.edgeId_0 == a_idPair.data_0) && 
                (a_ptPair.edgeId_1 == a_idPair.data_1))
            {
                bool isValid = false;
                if (true == isTBDir)
                {
                    if ((a_ptPair.contourPt_0.edgePt.x > range.min) && (a_ptPair.contourPt_0.edgePt.x < range.max) &&
                        (a_ptPair.contourPt_1.edgePt.x > range.min) && (a_ptPair.contourPt_1.edgePt.x < range.max))
                        isValid = true;
                }
                else
                {
                    if ((a_ptPair.contourPt_0.edgePt.y > range.min) && (a_ptPair.contourPt_0.edgePt.y < range.max) &&
                        (a_ptPair.contourPt_1.edgePt.y > range.min) && (a_ptPair.contourPt_1.edgePt.y < range.max))
                        isValid = true;
                }
                if (true == isValid)
                {
                    if (a_ptPair.contourPt_0.blockFlag > 0)
                        blockPtNum_0++;
                    if (a_ptPair.contourPt_1.blockFlag > 0)
                        blockPtNum_1++;
                    contourFilter.push_back(contourPairs[m]);
                    break;
                }
            }
        }
    }
    int totalNum = (int)contourFilter.size();
    if(blockPtNum_0 > totalNum/2)
        *lowLevelFlag_0 = 1;
    if (blockPtNum_1 > totalNum / 2)
        *lowLevelFlag_1 = 1;
      
    return;
}

//取出contour点
void sg_contourPostProc(std::vector< SSG_contourPtInfo>& contour, int maxEdgeIdx, double sameConturDistTh,
    std::vector< SSG_2DValueI>& contourFilter, int sideID, int* blockFlag)
{
    *blockFlag = 0;
    if (contour.size() == 0)
        return;

    std::vector< SSG_contourEdgeInfo> edgeInfo;
    edgeInfo.resize(maxEdgeIdx + 1);
    std::vector<int> egdeIndexing;
    for (int i = 0, i_max = (int)contour.size(); i < i_max; i++)
    {
        SSG_contourPtInfo* a_contourPt = &contour[i];
        int edgeIdx = a_contourPt->edgeId;
        if (edgeInfo[edgeIdx].ptNum == 0) //新的Edge
        {
            edgeInfo[edgeIdx].sPt = a_contourPt->edgePt;
            egdeIndexing.push_back(edgeIdx);
        }
        edgeInfo[edgeIdx].ePt = a_contourPt->edgePt;
        edgeInfo[edgeIdx].scanDist += a_contourPt->scanDist;
        edgeInfo[edgeIdx].ptNum += 1;
    }
    //计算平均scanDist
    //保留scanDist最小的，只有点数大于总点数的10%才是有效edge
    int lenTh = (int)(contour.size() * 3) / 10;
    int nearestIdx = -1;
    double minScanDist = -1;
    for (int i = 0; i < egdeIndexing.size(); i++)
    {
        int edgeId = egdeIndexing[i];
        edgeInfo[edgeId].scanDist = edgeInfo[edgeId].scanDist / edgeInfo[edgeId].ptNum;
        if (edgeInfo[edgeId].ptNum > lenTh)
        {
            if (minScanDist < 0)
            {
                nearestIdx = i;
                minScanDist = edgeInfo[edgeId].scanDist;
            }
            else
            {
                if (minScanDist > edgeInfo[edgeId].scanDist)
                {
                    nearestIdx = i;
                    minScanDist = edgeInfo[edgeId].scanDist;
                }
            }
        }
        else
            edgeInfo[edgeId].ptNum = 0; //invalid
    }
    if (nearestIdx < 0)
        return;

    //以最小距离为基础，检查其余线段与此线段的距离。小于sameConturDistTh为同一线段
    //生成有效contour
    //统计block(遮挡)的比例
    int blockPtNum = 0;
    for (int i = 0; i < egdeIndexing.size(); i++)
    {
        int edgeId = egdeIndexing[i];
        if (0 == edgeInfo[edgeId].ptNum)
            continue;

        double scanDist = edgeInfo[edgeId].scanDist;
        double dist_diff = scanDist - minScanDist;
        if (dist_diff < sameConturDistTh)
        {
            for (int m = 0, m_max = (int)contour.size(); m < m_max; m++)
            {
                SSG_contourPtInfo* a_contourPt = &contour[m];
                int an_edgeIdx = a_contourPt->edgeId;
                if (an_edgeIdx == edgeId)
                {
                    if (a_contourPt->blockFlag > 0)
                        blockPtNum++;
                    SSG_2DValueI a_filterPt = { a_contourPt->x, a_contourPt->y, a_contourPt->type, a_contourPt->edgePt.z, sideID };
                    contourFilter.push_back(a_filterPt);
                }
            }
        }
    }
    blockPtNum = blockPtNum * 2;
    if (blockPtNum > contourFilter.size())
        *blockFlag = 1;
    return;
}

void _checkVEdgePt(
    SVzNL3DLaserLine* laser3DPoints,
    int lineNum,
    int px,
    int py,
    int linePtNum,
    SSG_2DValueI a_peak,
    SVzNL3DPosition* a_pt,
    int* a_pt_edgeId,
    double pk_y,
    bool rgnPtAsEdge,
    double scanDistTh,
    bool dirUp, 
    SSG_contourPtInfo* edgePt,
    bool* stopFlag,
    bool* foundHScanEdge)
{
    int ptNum = laser3DPoints[0].nPositionCnt;
    int objId = (a_pt->nPointIdx >> 8) & 0xff;
    int vType = (a_pt->nPointIdx) & 0x00ff;//屏蔽rgnID
    int hType = vType >> 4;
    vType &= 0x0f;

    *stopFlag = false;
    double scanDist = abs(pk_y - a_pt->pt3D.y);
    if ((vType > 0) || (py == 0) ||(py == linePtNum-1)|| ((true == rgnPtAsEdge) && (objId > 0)))
    {
        //碰撞检测（检测是否为下层目标)
        if ((vType == 0) && ((true == rgnPtAsEdge) && (objId > 0)))
        {
            vType = LINE_FEATURE_RGN_EDGE;
            if (true == dirUp)
                *a_pt_edgeId = 5;
            else
                *a_pt_edgeId = 6;
        }

        int blockFlag = 0;
        if (true == dirUp)
        {
            if ((LINE_FEATURE_L_JUMP_H2L == vType) || (LINE_FEATURE_L_SLOPE_H2L == vType))
                blockFlag = 1;
        }
        else
        {
            if ((LINE_FEATURE_L_JUMP_L2H == vType) || (LINE_FEATURE_L_SLOPE_L2H == vType))
                blockFlag = 1;
        }

        if (scanDist > scanDistTh)
            *stopFlag = true;

        if (false == *stopFlag)
        {
            *edgePt = { px, py, vType, *a_pt_edgeId, blockFlag, scanDist, a_pt->pt3D };

            if ((LINE_FEATURE_RGN_EDGE == vType) || (LINE_FEATURE_L_JUMP_H2L == vType) ||
                (LINE_FEATURE_L_JUMP_L2H == vType) ||
                (LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType))// ||
                //(LINE_FEATURE_CORNER_V == vType))
                *stopFlag = true;
            else if((*a_pt_edgeId == 5) || (*a_pt_edgeId == 6))
                *stopFlag = true;
        }
    }
#if 1
    else if ((vType == 0) && (hType > 0))
    {
        //
        if ((LINE_FEATURE_L_JUMP_H2L == hType) || (LINE_FEATURE_L_JUMP_L2H == hType))
        {
            //继续向前检测5步
            bool tmp_flag = true;
            for (int m = 0; m < 5; m++)
            {
                int chk_y;
                if (true == dirUp)
                {
                    chk_y = py - m;
                    if (chk_y < 0)
                        break;
                }
                else
                {
                    chk_y = py + m;
                    if (chk_y >= ptNum)
                        break;
                }
                int chk_vType = (laser3DPoints[px].p3DPosition[chk_y].nPointIdx) & 0x000f;//屏蔽rgnID
                if (chk_vType > 0)
                {
                    tmp_flag = false;
                    break;
                }
            }

            if (true == tmp_flag)
            {
                *foundHScanEdge = true;
                *stopFlag = true;
            }
        }
    }
#endif
    return;
}

void _checkHEdgePt(
    SVzNL3DLaserLine* laser3DPoints,
    int lineNum,
    int px,
    int py,
    SSG_2DValueI a_peak,
    SVzNL3DPosition* a_pt,
    int* a_pt_edgeId,
    double pk_x,
    bool rgnPtAsEdge,
    double scanDistTh,
    bool dirLeft,
    SSG_contourPtInfo* edgePt,
    bool* stopFlag,
    bool* foundVScanEdge)
{
    int objId = (a_pt->nPointIdx >> 8) & 0xff;
    int vType = (a_pt->nPointIdx) & 0x00ff;//屏蔽rgnID
    int hType = vType >> 4;
    vType &= 0x0f;

    *stopFlag = false;
    double scanDist = abs(pk_x - a_pt->pt3D.x);
    if ((hType > 0) || (px == 0) ||(px == lineNum-1)|| ((true == rgnPtAsEdge) && (objId > 0)))
    {
        //碰撞检测（检测是否为下层目标)
        if ((hType == 0) && ((true == rgnPtAsEdge) && (objId > 0)))
        {
            hType = LINE_FEATURE_RGN_EDGE;
            if (true == dirLeft)
                *a_pt_edgeId = 7;
            else
                *a_pt_edgeId = 8;
        }

        int blockFlag = 0;
        if (true == dirLeft)
        {
            if ((LINE_FEATURE_L_JUMP_H2L == hType) || (LINE_FEATURE_L_SLOPE_H2L == hType))
                blockFlag = 1;
        }
        else
        {
            if ((LINE_FEATURE_L_JUMP_L2H == hType) || (LINE_FEATURE_L_SLOPE_L2H == hType))
                blockFlag = 1;
        }

        if (scanDist > scanDistTh)
            *stopFlag = true;

        if (false == *stopFlag)
        {
            *edgePt = { px, py, hType, *a_pt_edgeId, blockFlag, scanDist, a_pt->pt3D };

            if ((LINE_FEATURE_RGN_EDGE == hType) || (LINE_FEATURE_L_JUMP_H2L == hType) ||
                (LINE_FEATURE_L_JUMP_L2H == hType) ||
                (LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType))//||
                //(LINE_FEATURE_CORNER_V == hType))
                *stopFlag = true;
            else if ((*a_pt_edgeId == 7) || (*a_pt_edgeId == 8))
                *stopFlag = true;
        }
    }
#if 1
    else if ((hType == 0) && ( vType > 0))
    {
        if ((LINE_FEATURE_L_JUMP_H2L == vType) || (LINE_FEATURE_L_JUMP_L2H == vType))
        {
            //继续向前检测5步
            bool tmp_flag = true;
            for (int m = 0; m < 5; m++)
            {
                int chk_x;
                if (true == dirLeft)
                {
                    chk_x = px - m;
                    if (chk_x < 0)
                        break;
                }
                else
                {
                    chk_x = px + m;
                    if (chk_x >= lineNum)
                        break;
                }
                int chk_hType = (laser3DPoints[chk_x].p3DPosition[py].nPointIdx) & 0x00f0;//屏蔽rgnID
                if (chk_hType > 0)
                {
                    tmp_flag = false;
                    break;
                }
            }
            if (true == tmp_flag)
            {
                *foundVScanEdge = true;
                *stopFlag = true;
            }
        }
    }
#endif
    return;
}

void _searchUpDownEdgePts(
    SVzNL3DLaserLine* laser3DPoints, 
    int lineNum, 
    cv::Mat& featureEdgeMask,
    SSG_2DValueI a_peak, 
    int scan_x,
    bool rgnPtAsEdge,
    double scanDistTh,
    bool searchDir_up,
    std::vector< SSG_contourPtInfo>& edgePts,
    bool* foundHScanEdge
)
{
    *foundHScanEdge = false;
    double pk_y = laser3DPoints[a_peak.x].p3DPosition[a_peak.y].pt3D.y;
    int ptNum = laser3DPoints[0].nPositionCnt;
    if (true == searchDir_up)
    {
        SSG_contourPtInfo pre_pt;
        memset(&pre_pt, 0, sizeof(SSG_contourPtInfo));
        for (int y = a_peak.y; y >= 0; y--)
        {
if (y == 84)
    int kkk = 1;
            SVzNL3DPosition* a_pt = &laser3DPoints[scan_x].p3DPosition[y];
            int edgeId = featureEdgeMask.at<cv::Vec4i>(y, scan_x)[0];
            SSG_contourPtInfo _top;
            memset(&_top, 0, sizeof(SSG_contourPtInfo));
            _top.x = -1; //无效
            bool stopFlag = false;
            _checkVEdgePt(
                laser3DPoints,
                lineNum,
                scan_x,
                y,
                ptNum, 
                a_peak,
                a_pt,
                &edgeId,
                pk_y,
                rgnPtAsEdge,
                scanDistTh,
                true, //Dir_UP
                &_top,
                &stopFlag,
                foundHScanEdge);

            if ( (_top.x >= 0) && (_top.type > 0) && (edgeId > 0))
            {
                if ((_top.type > 0) && (_top.y >= a_peak.y - 1))  //在区域外面
                    stopFlag = true;
                else
                {
                    //进行Edge检查，检查Edge有没有突然变化 
                    if (((LINE_FEATURE_L_JUMP_H2L == pre_pt.type) || (LINE_FEATURE_L_JUMP_L2H == pre_pt.type)) &&
                        ((LINE_FEATURE_L_JUMP_H2L == _top.type) || (LINE_FEATURE_L_JUMP_L2H == _top.type) ))
                          //(LINE_FEATURE_LINE_ENDING_0 == _top.type) || (LINE_FEATURE_LINE_ENDING_1 == _top.type)))
                    {
                        double dist_diff = abs(pre_pt.scanDist - _top.scanDist);
                        if (dist_diff > 100)
                        {
                            stopFlag = true;
                            *foundHScanEdge = true;
                        }
                        else
                            edgePts.push_back(_top);
                    }
                    else
                        edgePts.push_back(_top);
                }
            }
            if (true == stopFlag)
                break;
            pre_pt = _top;
        }
    }
    else
    {
        SSG_contourPtInfo pre_pt;
        memset(&pre_pt, 0, sizeof(SSG_contourPtInfo));
        for (int y = a_peak.y; y < ptNum; y++)
        {
if (y == 336)
    int kkk = 1;
            SVzNL3DPosition* a_pt = &laser3DPoints[scan_x].p3DPosition[y];
            int edgeId = featureEdgeMask.at<cv::Vec4i>(y, scan_x)[0];
            SSG_contourPtInfo _btm;
            memset(&_btm, 0, sizeof(SSG_contourPtInfo));
            _btm.x = -1; //无效
            bool stopFlag = false;
            _checkVEdgePt(
                laser3DPoints,
                lineNum,
                scan_x,
                y,
                ptNum, 
                a_peak,
                a_pt,
                &edgeId,
                pk_y,
                rgnPtAsEdge,
                scanDistTh,
                false, //dir_down
                &_btm,
                &stopFlag,
                foundHScanEdge);
            if ( (_btm.x >= 0) && (_btm.type > 0) && (edgeId > 0))
            {
                if ((_btm.type > 0) && (_btm.y <= a_peak.y + 1))//在区域外面
                    stopFlag = true;
                else
                {
                    //进行Edge检查，检查Edge有没有突然变化 
                    if (((LINE_FEATURE_L_JUMP_H2L == pre_pt.type) || (LINE_FEATURE_L_JUMP_L2H == pre_pt.type)) &&
                        ((LINE_FEATURE_L_JUMP_H2L == _btm.type) || (LINE_FEATURE_L_JUMP_L2H == _btm.type)))// ||
                            //(LINE_FEATURE_LINE_ENDING_0 == _btm.type) || (LINE_FEATURE_LINE_ENDING_1 == _btm.type)))
                    {
                        double dist_diff = abs(pre_pt.scanDist - _btm.scanDist);
                        if (dist_diff > 100)
                        {
                            stopFlag = true;
                            *foundHScanEdge = true;
                        }
                        else
                            edgePts.push_back(_btm);
                    }
                    else
                        edgePts.push_back(_btm);
                }
            }
            if (true == stopFlag)
                break;
            pre_pt = _btm;
        }
    }
}

void _searchLeftRightEdgePts(
    SVzNL3DLaserLine* laser3DPoints,
    int lineNum,
    cv::Mat& featureEdgeMask,
    SSG_2DValueI a_peak,
    int scan_y,
    bool rgnPtAsEdge,
    double scanDistTh,
    bool searchDir_left,
    std::vector< SSG_contourPtInfo>& edgePts,
    bool* foundVScanEdge)
{
    *foundVScanEdge = false;
    double pk_x = laser3DPoints[a_peak.x].p3DPosition[a_peak.y].pt3D.x;
    if (true == searchDir_left)
    {
        SSG_contourPtInfo pre_pt;
        memset(&pre_pt, 0, sizeof(SSG_contourPtInfo));
        for (int x = a_peak.x; x >= 0; x--)
        {
if (x == 4)
    int kkk = 1;
            SVzNL3DPosition* a_pt = &laser3DPoints[x].p3DPosition[scan_y];
            int edgeId = featureEdgeMask.at<cv::Vec4i>(scan_y, x)[0];
            SSG_contourPtInfo _left;
            memset(&_left, 0, sizeof(SSG_contourPtInfo));
            _left.x = -1; //无效
            bool stopFlag = false;
            _checkHEdgePt(
                laser3DPoints,
                lineNum,
                x,
                scan_y,
                a_peak,
                a_pt,
                &edgeId,
                pk_x,
                rgnPtAsEdge,
                scanDistTh,
                true, //DIR_left
                &_left,
                &stopFlag,
                foundVScanEdge);

            if ((_left.x >= 0) && (_left.type > 0) && (edgeId > 0))
            {
                if ((_left.type > 0) && (_left.x >= a_peak.x - 1))  //在区域外面
                    stopFlag = true;
                else
                {
                    //进行Edge检查，检查Edge有没有突然变化 
                    if (((LINE_FEATURE_L_JUMP_H2L == pre_pt.type) || (LINE_FEATURE_L_JUMP_H2L == pre_pt.type)) &&
                        ((LINE_FEATURE_L_JUMP_H2L == _left.type) || (LINE_FEATURE_L_JUMP_H2L == _left.type)))// ||
                            //(LINE_FEATURE_LINE_ENDING_0 == _left.type) || (LINE_FEATURE_LINE_ENDING_1 == _left.type)))
                    {
                        double dist_diff = abs(pre_pt.scanDist - _left.scanDist);
                        if (dist_diff > 100)
                        {
                            stopFlag = true;
                            *foundVScanEdge = true;
                        }
                        else
                            edgePts.push_back(_left);
                    }
                    else
                        edgePts.push_back(_left);
                }
            }
            if (true == stopFlag)
                break;
            pre_pt = _left;
        }
    }
    else
    {
        SSG_contourPtInfo pre_pt;
        memset(&pre_pt, 0, sizeof(SSG_contourPtInfo));
        for (int x = a_peak.x; x < lineNum; x++)
        {
if (x == 374)
    int kkk = 1;
            SVzNL3DPosition* a_pt = &laser3DPoints[x].p3DPosition[scan_y];
            int edgeId = featureEdgeMask.at<cv::Vec4i>(scan_y, x)[0];
            SSG_contourPtInfo _right;
            memset(&_right, 0, sizeof(SSG_contourPtInfo));
            _right.x = -1; //无效
            bool stopFlag = false;
            _checkHEdgePt(
                laser3DPoints,
                lineNum,
                x,
                scan_y,
                a_peak,
                a_pt,
                &edgeId,
                pk_x,
                rgnPtAsEdge,
                scanDistTh,
                false, //DIR_right
                &_right,
                &stopFlag,
                foundVScanEdge);
            if ( (_right.x >= 0) && (_right.type > 0) && (edgeId > 0))
            {
                if ((_right.type > 0) && (_right.x <= a_peak.x + 1))//在区域外面
                    stopFlag = true;
                else
                {
                    //进行Edge检查，检查Edge有没有突然变化 
                    if (((LINE_FEATURE_L_JUMP_H2L == pre_pt.type) || (LINE_FEATURE_L_JUMP_H2L == pre_pt.type)) &&
                        ((LINE_FEATURE_L_JUMP_H2L == _right.type) || (LINE_FEATURE_L_JUMP_H2L == _right.type))) //||
                           // (LINE_FEATURE_LINE_ENDING_0 == _right.type) || (LINE_FEATURE_LINE_ENDING_1 == _right.type)))
                    {
                        double dist_diff = abs(pre_pt.scanDist - _right.scanDist);
                        if (dist_diff > 100)
                        {
                            stopFlag = true;
                            *foundVScanEdge = true;
                        }
                        else
                            edgePts.push_back(_right);
                    }
                    else
                        edgePts.push_back(_right);
                }
            }
            if (true == stopFlag)
                break;
            pre_pt = _right;
        }
    }
}

//从Peak点进行水平垂直扫描得到区域边界
void sg_peakXYScan(
    SVzNL3DLaserLine* laser3DPoints, 
    int lineNum, 
    cv::Mat& featureEdgeMask, 
    SSG_2DValueI a_peak, 
    SSG_treeGrowParam growParam, 
    SSG_bagParam bagParam, 
    bool rgnPtAsEdge, 
    //int stopEdgeId_T, int stopEdgeId_B, int stopEdgeId_L, int stopEdgeId_R,
    std::vector< SSG_lineConotours>& topContour,
    std::vector< SSG_lineConotours>& bottomContour,
    std::vector< SSG_lineConotours>& leftContour,
    std::vector< SSG_lineConotours>& rightContour,
    int* maxEdgeId_top, 
    int* maxEdgeId_btm, 
    int* maxEdgeId_left, 
    int* maxEdgeId_right)
{
    if (featureEdgeMask.at<cv::Vec4i>(a_peak.y, a_peak.x)[3] == 0)
        return;
    int ptNum = laser3DPoints[0].nPositionCnt;

    double pk_x = laser3DPoints[a_peak.x].p3DPosition[a_peak.y].pt3D.x;
    double pk_y = laser3DPoints[a_peak.x].p3DPosition[a_peak.y].pt3D.y;
    *maxEdgeId_top = 0;
    *maxEdgeId_btm = 0;

    double scanDistTh = bagParam.bagL -a_peak.valueD /2;
    //从Peak点向左扫描
    for (int x = a_peak.x; x >= 0; x--)
    {
if (x == 0)
    int kkk = 1;
        
        //最多扫描bagL宽度
        double curr_x = laser3DPoints[x].p3DPosition[a_peak.y].pt3D.x;
        double x_diff = abs(curr_x - pk_x);
        if (x_diff > scanDistTh)
            break;

        bool foundHScanEdge = false;

        std::vector< SSG_contourPtInfo> a_line_upEdgePts;
        _searchUpDownEdgePts(
            laser3DPoints,
            lineNum,
            featureEdgeMask,
            a_peak,
            x,
            rgnPtAsEdge,
            scanDistTh,
            true, //searchDir_up,
            a_line_upEdgePts,
            &foundHScanEdge);

        std::vector< SSG_contourPtInfo> a_line_downEdgePts;
        _searchUpDownEdgePts(
            laser3DPoints,
            lineNum,
            featureEdgeMask,
            a_peak,
            x,
            rgnPtAsEdge,
            scanDistTh,
            false, //searchDir_down,
            a_line_downEdgePts,
            &foundHScanEdge);

        if ( true == foundHScanEdge)
            break;
        else
        {
            for (int m = 0, m_max = (int)a_line_upEdgePts.size(); m < m_max; m++)
            {
                if (*maxEdgeId_top < a_line_upEdgePts[m].edgeId)
                    *maxEdgeId_top = a_line_upEdgePts[m].edgeId;
            }
            if (a_line_upEdgePts.size() > 0)
            {
                SSG_lineConotours _line_upContours = { x,a_line_upEdgePts };
                topContour.insert(topContour.begin(), _line_upContours);
            }

            for (int m = 0, m_max = (int)a_line_downEdgePts.size(); m < m_max; m++)
            {
                if (*maxEdgeId_btm < a_line_downEdgePts[m].edgeId)
                    *maxEdgeId_btm = a_line_downEdgePts[m].edgeId;
            }
            if (a_line_downEdgePts.size() > 0)
            {
                SSG_lineConotours _line_downContours = { x,a_line_downEdgePts };
                bottomContour.insert(bottomContour.begin(), _line_downContours);
            }
        }
    }
    //从Peak点向右扫描
    for (int x = a_peak.x+1; x < lineNum; x++)
    {
if (x == 34)
    int kkk = 1;

        //最多扫描bagL宽度
        double curr_x = laser3DPoints[x].p3DPosition[a_peak.y].pt3D.x;
        double x_diff = abs(curr_x - pk_x);
        if (x_diff > scanDistTh)
            break;

        bool foundHScanEdge = false;

        std::vector< SSG_contourPtInfo> a_line_upEdgePts;
        _searchUpDownEdgePts(
            laser3DPoints,
            lineNum,
            featureEdgeMask,
            a_peak,
            x,
            rgnPtAsEdge,
            scanDistTh,
            true, //searchDir_up,
            a_line_upEdgePts,
            &foundHScanEdge);

        std::vector< SSG_contourPtInfo> a_line_downEdgePts;
        _searchUpDownEdgePts(
            laser3DPoints,
            lineNum,
            featureEdgeMask,
            a_peak,
            x,
            rgnPtAsEdge,
            scanDistTh,
            false, //searchDir_down,
            a_line_downEdgePts,
            &foundHScanEdge);

        if (true == foundHScanEdge)
            break;
        else
        {
            for (int m = 0, m_max = (int)a_line_upEdgePts.size(); m < m_max; m++)
            {
                if (*maxEdgeId_top < a_line_upEdgePts[m].edgeId)
                    *maxEdgeId_top = a_line_upEdgePts[m].edgeId;
            }
            if (a_line_upEdgePts.size() > 0)
            {
                SSG_lineConotours _line_upContours = { x,a_line_upEdgePts };
                topContour.push_back(_line_upContours);
            }

            for (int m = 0, m_max = (int)a_line_downEdgePts.size(); m < m_max; m++)
            {
                if (*maxEdgeId_btm < a_line_downEdgePts[m].edgeId)
                    *maxEdgeId_btm = a_line_downEdgePts[m].edgeId;
            }
            if (a_line_downEdgePts.size() > 0)
            {
                SSG_lineConotours _line_downContours = { x,a_line_downEdgePts };
                bottomContour.push_back(_line_downContours);
            }
        }
    }

    *maxEdgeId_left = 0;
    *maxEdgeId_right = 0;
    //从Peak点向上扫描
    for (int y= a_peak.y; y >= 0; y--)
    {
if (y == 380)
    int kkk = 1;

        //最多扫描bagL宽度
        double curr_y = laser3DPoints[a_peak.x].p3DPosition[y].pt3D.y;
        double y_diff = abs(curr_y - pk_y);
        if (y_diff > scanDistTh)
            break;

        bool foundVScanEdge = false;

        std::vector< SSG_contourPtInfo> a_line_leftPts;
        _searchLeftRightEdgePts(
            laser3DPoints,
            lineNum,
            featureEdgeMask,
            a_peak,
            y,
            rgnPtAsEdge,
            scanDistTh,
            true,// searchDir_left,
            a_line_leftPts,
            &foundVScanEdge);

        std::vector< SSG_contourPtInfo> a_line_rightPts;
        _searchLeftRightEdgePts(
            laser3DPoints,
            lineNum,
            featureEdgeMask,
            a_peak,
            y,
            rgnPtAsEdge,
            scanDistTh,
            false,// searchDir_right,
            a_line_rightPts,
            &foundVScanEdge);


        if ( true == foundVScanEdge) 
            break;
        else
        {
            for (int m = 0, m_max = (int)a_line_leftPts.size(); m < m_max; m++)
            {
                if (*maxEdgeId_left < a_line_leftPts[m].edgeId)
                    *maxEdgeId_left = a_line_leftPts[m].edgeId;
            }
            if (a_line_leftPts.size() > 0)
            {
                SSG_lineConotours _line_leftContours = { y, a_line_leftPts };
                leftContour.insert(leftContour.begin(), _line_leftContours);
            }
            for (int m = 0, m_max = (int)a_line_rightPts.size(); m < m_max; m++)
            {
                if (*maxEdgeId_right < a_line_rightPts[m].edgeId)
                    *maxEdgeId_right = a_line_rightPts[m].edgeId;
            }
            if (a_line_rightPts.size() > 0)
            {
                SSG_lineConotours _line_rightContours = { y, a_line_rightPts };
                rightContour.insert(rightContour.begin(), _line_rightContours);
            }
        }
    }
    //从Peak点向下扫描
    for (int y = a_peak.y+1; y < ptNum; y++)
    {
        if (y == 285)
            int kkk = 1;
        //最多扫描bagL宽度
        double curr_y = laser3DPoints[a_peak.x].p3DPosition[y].pt3D.y;
        double y_diff = abs(curr_y - pk_y);
        if (y_diff > scanDistTh)
            break;

        bool foundVScanEdge = false;

        std::vector< SSG_contourPtInfo> a_line_leftPts;
        _searchLeftRightEdgePts(
            laser3DPoints,
            lineNum,
            featureEdgeMask,
            a_peak,
            y,
            rgnPtAsEdge,
            scanDistTh,
            true,// searchDir_left,
            a_line_leftPts,
            &foundVScanEdge);

        std::vector< SSG_contourPtInfo> a_line_rightPts;
        _searchLeftRightEdgePts(
            laser3DPoints,
            lineNum,
            featureEdgeMask,
            a_peak,
            y,
            rgnPtAsEdge,
            scanDistTh,
            false,// searchDir_right,
            a_line_rightPts,
            &foundVScanEdge);


        if (true == foundVScanEdge)
            break;
        else
        {
            for (int m = 0, m_max = (int)a_line_leftPts.size(); m < m_max; m++)
            {
                if (*maxEdgeId_left < a_line_leftPts[m].edgeId)
                    *maxEdgeId_left = a_line_leftPts[m].edgeId;
            }
            if (a_line_leftPts.size() > 0)
            {
                SSG_lineConotours _line_leftContours = { y, a_line_leftPts };
                leftContour.push_back( _line_leftContours);
            }
            for (int m = 0, m_max = (int)a_line_rightPts.size(); m < m_max; m++)
            {
                if (*maxEdgeId_right < a_line_rightPts[m].edgeId)
                    *maxEdgeId_right = a_line_rightPts[m].edgeId;
            }
            if (a_line_rightPts.size() > 0)
            {
                SSG_lineConotours _line_rightContours = { y, a_line_rightPts };
                rightContour.push_back(_line_rightContours);
            }
        }
    }
    return;
}
float EDistance(int x1, int y1, int x2, int y2)
{
    return sqrt(float((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)));
}
float MDistance(int x1, int y1, int x2, int y2)
{
    return (float)abs(x1 - x2) + (float)abs(y1 - y2);
}
float CDistance(int x1, int y1, int x2, int y2)
{
    return std::max((float)abs(x1 - x2), (float)abs(y1 - y2));
}
float Distance(int x1, int y1, int x2, int y2, int type)
{
    if (type == 0)
    {
        return EDistance(x1, y1, x2, y2);
    }
    else if (type == 1)
    {
        return MDistance(x1, y1, x2, y2);
    }
    else //if (type == 2)
    {
        return CDistance(x1, y1, x2, y2);
    }
}
/// <summary>
/// 距离变换
/// </summary>
/// <param name="input"> float型 </param>
/// <param name="output"></param>
/// <param name="distType"></param>
/// mask1 
/// q1	q2
/// q3	P
/// q4
///mask2
///   	q4
///  P	q3
/// q1	q2
void sg_distanceTrans(const cv::Mat input, cv::Mat& output, int distType)
{
    cv::Mat BinaryImage = input.clone();
    float* pRowOne;
    float* pRowNext;
    float distance;
    float Mindis;
    for (int i = 1; i < BinaryImage.rows - 1; i++)
    {
        pRowOne = BinaryImage.ptr<float>(i);
        for (int j = 1; j < BinaryImage.cols; j++)
        {
            pRowNext = BinaryImage.ptr<float>(i - 1);

            distance = Distance(i, j, i - 1, j - 1, distType);//q1
            Mindis = std::min((float)pRowOne[j], distance + pRowNext[j - 1]);

            distance = Distance(i, j, i - 1, j, distType);//q2
            Mindis = std::min(Mindis, distance + pRowNext[j]);

            pRowNext = BinaryImage.ptr<float>(i);
            distance = Distance(i, j, i, j - 1, distType);//q3
            Mindis = std::min(Mindis, distance + pRowNext[j-1]);

            pRowNext = BinaryImage.ptr<float>(i + 1);//q4
            distance = Distance(i, j, i + 1, j - 1, distType);
            Mindis = std::min(Mindis, distance + pRowNext[j - 1]);
            pRowOne[j] = Mindis;
        }
    }
    for (int i = BinaryImage.rows - 2; i > 0; i--)
    {
        pRowOne = BinaryImage.ptr<float>(i);
        for (int j = BinaryImage.cols - 2; j >= 0; j--)
        {
            pRowNext = BinaryImage.ptr<float>(i + 1);

            distance = Distance(i, j, i + 1, j, distType);//q1
            Mindis = std::min((float)pRowOne[j], distance + pRowNext[j]);

            distance = Distance(i, j, i + 1, j + 1, distType);//q2
            Mindis = std::min(Mindis, distance + pRowNext[j + 1]);

            pRowNext = BinaryImage.ptr<float>(i);//q3
            distance = Distance(i, j, i, j + 1, distType);
            Mindis = std::min(Mindis, distance + pRowNext[j + 1]);

            pRowNext = BinaryImage.ptr<float>(i - 1);//q4
            distance = Distance(i, j, i - 1, j + 1, distType);
            Mindis = std::min(Mindis, distance + pRowNext[j + 1]);
            pRowOne[j] = Mindis;
        }
    }
    //将周边一圈置0
    float* row_0 = BinaryImage.ptr<float>(0);
    float* row_last = BinaryImage.ptr<float>(input.rows - 1);
    for (int i = 0; i < BinaryImage.cols; i++)
    {
        row_0[i] = 0;
        row_last[i] = 0;
    }
    for (int i = 0; i < input.rows; i++)
    {
        BinaryImage.ptr<float>(i)[0] = 0;
        BinaryImage.ptr<float>(i)[input.cols - 1] = 0;
    }
    output = BinaryImage;
}

//在searchWin中搜索距离变换最高点
void _findDistTransformPeak(cv::Mat& distTransform, SVzNL2DPoint LTpos, SSG_localPkParam searchWin, SSG_2DValueI* pkPos)
{
    int line_end = LTpos.x + searchWin.seachW_lines;
    if (line_end >= distTransform.cols)
        line_end = distTransform.cols;
    int pt_end = LTpos.y + searchWin.searchW_pts;
    if (pt_end >= distTransform.rows)
        pt_end = distTransform.rows;

    SSG_2DValueI peak = { -1, -1, 0, 0, 0 };
    double maxValue = -1;
    for (int i = LTpos.x; i < line_end; i++)
    {
        for (int j = LTpos.y; j < pt_end; j++)
        {
            float value = distTransform.at<float>(j, i); 
            if (maxValue < 0)
            {
                peak = { i, j , 0, value };
                maxValue = value;
            }
            else
            {
                if (maxValue <value)
                {
                    peak = { i, j, 0,  value , 0};
                    maxValue = value;
                }
            }
        }
    }
    *pkPos = peak;
    return;
}

/// <summary>
/// 以5x5方式寻找localPeaks
/// </summary>
/// <param name="input"></param>
/// <param name="peaks"></param>
void sg_getLocalPeaks_distTransform(cv::Mat& input, std::vector<SSG_2DValueI>& peaks, SSG_localPkParam searchWin)
{
    cv::Mat mask = cv::Mat::zeros(input.rows,input.cols, CV_32SC1);
    int winNum_cols = input.cols / (searchWin.seachW_lines / 2);
    if ((input.cols % (searchWin.seachW_lines / 2)) > 0)
        winNum_cols = winNum_cols + 1;
    int winNum_rows = input.rows / (searchWin.searchW_pts / 2);
    if ((input.rows % (searchWin.searchW_pts / 2)) > 0)
        winNum_rows = winNum_rows + 1;

    for (int i = 0; i < winNum_rows; i++)
    {
        for (int j = 0; j < winNum_cols; j++)
        {
            SVzNL2DPoint LTpos = { j * searchWin.searchW_pts / 2 , i * searchWin.seachW_lines / 2 };
            SSG_2DValueI pkPos = { -1, -1, 0, 0 };
            _findDistTransformPeak(input, LTpos, searchWin, &pkPos);
            if ((pkPos.x >= 0) && (pkPos.y >= 0))
            {
                //边界处的极值点去除
                if ((pkPos.x != LTpos.x) && (pkPos.x != (LTpos.x + searchWin.seachW_lines - 1)) &&
                    (pkPos.y != LTpos.y) && (pkPos.y != (LTpos.y + searchWin.searchW_pts - 1)))
                {
                    int maskValue = mask.at<int>(pkPos.y, pkPos.x);
                    if (maskValue == 0)
                    {
                        peaks.push_back(pkPos);
                        mask.at<int>(pkPos.y, pkPos.x) = 1;
                    }
                }
            }
        }
    }
    return;
}