#include "ImageMerge.h"
#include "CheckUtil.hpp"
#include "CheckErrorCodeDefine.hpp"

ImageMerge::ImageMerge()
{
    bshowimg = false;
    m_bcalSucc = false;
}

ImageMerge::~ImageMerge() {}

int ImageMerge::CalMergeRoi(DetConfig *pDetConfig, cv::Mat &img1, const cv::Mat &img2)
{
    // cv::imwrite("merge1.png", img1);
    // cv::imwrite("merge2.png", img2);
    m_bcalSucc = false;
    std::cout << img1.size() << std::endl;
    std::cout << img2.size() << std::endl;
    // if (img1.size() != img2.size())
    // {
    //     return -1;
    // }

    long t1, t2;
    t1 = CheckUtil::getcurTime();
    int re = 0;
    cv::Rect Left_Roi;
    cv::Rect Right_Roi;

    bool bleft = ProductSide_left(img1);
    cv::Mat leftImg = img1;
    cv::Mat rightImg = img2;
    m_bleft_img1 = bleft;
    if (bleft)
    {
        leftImg = img1;
        rightImg = img2;
    }
    else
    {
        leftImg = img2;
        rightImg = img1;
    }
    int max_len = img1.cols;
    if (img2.cols > max_len)
    {
        max_len = img2.cols;
    }
    m_ALLImgSize.width = max_len * 2;
    m_ALLImgSize.height = rightImg.rows;

    // cv::Mat AllImg = cv::Mat::zeros(rightImg.rows, max_len * 2, rightImg.type());

    {

        cv::Rect Left_Tem_Roi;
        cv::Point Left_Tem_Point_Up;
        cv::Point Left_Tem_Point_Donw;
        cv::Rect Right_Tem_Roi;
        cv::Point Right_Tem_Point_Up;
        cv::Point Right_Tem_Point_Donw;
        // left Img

        {
            re = GetRectAndPoint(leftImg, Left_Tem_Roi, Left_Tem_Point_Up, Left_Tem_Point_Donw, true, pDetConfig->strcam, pDetConfig->strchannel + "left");
            if (re != 0)
            {
                // m_pdetlog->AddCheckstr(PrintLevel_1, "Detect_Pre", "Cam   %s  Mergimg  GetRectAndPoint TAA error  %d", strcam.c_str(), re);
                return 1;
            }
            re = GetRectAndPoint(rightImg, Right_Tem_Roi, Right_Tem_Point_Up, Right_Tem_Point_Donw, false, pDetConfig->strcam, pDetConfig->strchannel + "right");
            if (re != 0)
            {

                return 1;
            }
            cv::Rect roi_right;
            roi_right.x = 0;
            roi_right.y = 0;
            roi_right.height = rightImg.rows;
            roi_right.width = rightImg.cols;

            m_roi_right_img = roi_right;
            m_roi_right_ALLimg = roi_right;

            // rightImg(roi_right).copyTo(AllImg(roi_right));

            int hlen_left = Left_Tem_Point_Donw.y - Left_Tem_Point_Up.y;
            int hlen_right = Right_Tem_Point_Donw.y - Right_Tem_Point_Up.y;
            float fhscle = hlen_right * 1.0f / hlen_left;
            int dy = Right_Tem_Point_Up.y - Left_Tem_Point_Up.y;
            cv::Rect allroi = Left_Tem_Roi;
            allroi.height *= fhscle;
            allroi.x = Right_Tem_Roi.x + Right_Tem_Roi.width;
            allroi.y += dy;

            // cv::Size sz;
            // sz.width = allroi.width;
            // sz.height = allroi.height;
            m_roi_left_img = Left_Tem_Roi;
            m_roi_left_ALLimg = allroi;
            // cv::resize(leftImg(Left_Tem_Roi), AllImg(allroi), sz);
        }
    }

    m_bcalSucc = true;
    re = detMergeImg(img1, img2);
    if (re != 0)
    {
        return 1;
    }
    // img1 = AllImg;
    t2 = CheckUtil::getcurTime();
    //cv::imwrite(pDetConfig->strcam + pDetConfig->strchannel + "_merg.png", img1);
    //  m_pdetlog->AddCheckstr(PrintLevel_1, "Detect_Pre", "Cam   %s  Mergimg use time %ld ms", pDetConfig->strcam.c_str(), t2 - t1);
    return 0;
}

int ImageMerge::detMergeImg(cv::Mat &img1, const cv::Mat &img2)
{
    if (m_bcalSucc == false)
    {
        return -1;
    }
    cv::Mat AllImg = cv::Mat::zeros(m_ALLImgSize.height, m_ALLImgSize.width, img1.type());
    cv::Mat leftImg = img1;
    cv::Mat rightImg = img2;

    if (m_bleft_img1)
    {
        leftImg = img1;
        rightImg = img2;
    }
    else
    {
        leftImg = img2;
        rightImg = img1;
    }
    rightImg(m_roi_right_img).copyTo(AllImg(m_roi_right_ALLimg));

    cv::Size sz;
    sz.width = m_roi_left_ALLimg.width;
    sz.height = m_roi_left_ALLimg.height;

    cv::resize(leftImg(m_roi_left_img), AllImg(m_roi_left_ALLimg), sz);
    img1 = AllImg;
    return 0;
}

int ImageMerge::GetRectAndPoint(const cv::Mat &img, cv::Rect &roi, cv::Point &merg_p_up, cv::Point &merg_p_down, bool bleft, std::string strcam, std::string strchannel)
{
    bshowimg = true;

    int A_line_up = 0;
    int A_line_down = 0;

    Edge_Search_Config config;

    if (bshowimg)
    {
        if (img.channels() != 1)
        {
            showimg = img.clone();
        }
        else
        {
            cv::cvtColor(img, showimg, cv::COLOR_GRAY2BGR);
        }
    }

    config.nSearchCount = 10;
    config.strchannel = strchannel;
    config.directSign = DirectSign_UP;
    if (bleft)
    {
        config.roi = cv::Rect(0, 0, 200, img.rows * 0.8);
    }
    else
    {
        config.roi = cv::Rect(img.cols - 200, 0, 200, img.rows * 0.8);
    }

    int re123 = GetEdgePoint(img, &config, A_line_up);
    if (re123 != 0)
    {
        // printf("DirectSign_UP::111111111111GetEdgePoint111111 A_line_up   %d\n", re123);
        return 1;
    }

    config.directSign = DirectSign_DOWN;
    config.strchannel = strchannel + "ccc";
    config.roi = cv::Rect(0, img.rows * 0.2, 200, img.rows * 0.8 - 1);

    if (bleft)
    {
        config.roi = cv::Rect(0, img.rows * 0.2, 200, img.rows * 0.8 - 1);
    }
    else
    {
        config.roi = cv::Rect(img.cols - 200, img.rows * 0.2, 200, img.rows * 0.8 - 1);
    }

    re123 = GetEdgePoint(img, &config, A_line_down);
    if (re123 != 0)
    {
        // printf("DirectSign_UP::111111111111GetEdgePoint111111 A_line_down   %d\n", re123);
        return 1;
    }

    {
        if (bshowimg)
        {
            cv::line(showimg, cv::Point(0, A_line_down), cv::Point(img.cols, A_line_down), cv::Scalar(0, 255, 0), 2);
            cv::line(showimg, cv::Point(0, A_line_up), cv::Point(img.cols, A_line_up), cv::Scalar(0, 255, 0), 2);

            cv::imwrite(strcam + strchannel + "_line.png", showimg);
        }
    }
    // printf("DirectSign_UP::111111111111111111 re123   %d\n", re123);
    // getchar();

    cv::Mat small, smallbin;
    cv::resize(img, small, cv::Size(600, 1500));

    cv::threshold(small, smallbin, 80, 255,
                  cv::THRESH_BINARY);

    /* 3. contours */
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(smallbin, contours,
                     cv::RETR_EXTERNAL,
                     cv::CHAIN_APPROX_SIMPLE);

    if (contours.empty())
        return 1;

    /* 4. max contour */
    double maxArea = 0;
    int idx = -1;
    for (int i = 0; i < contours.size(); i++)
    {
        double area = cv::contourArea(contours[i]);
        if (area > maxArea)
        {
            maxArea = area;
            idx = i;
        }
    }

    if (idx < 0)
        return 1;

    cv::Rect rSmall = cv::boundingRect(contours[idx]);

    /* 5. map to original */
    float sx = (float)img.cols / smallbin.cols;
    float sy = (float)img.rows / smallbin.rows;
    cv::Rect rectInSrc;

    rectInSrc.x = int(rSmall.x * sx) - 50;
    rectInSrc.y = int(rSmall.y * sy) - 50;
    rectInSrc.width = int(rSmall.width * sx) + 100;
    rectInSrc.height = int(rSmall.height * sy) + 100;

    if (rectInSrc.y >= A_line_up)
    {
        rectInSrc.y = A_line_up - 5;
    }
    if (rectInSrc.y + rectInSrc.height <= A_line_down)
    {
        rectInSrc.height = A_line_down + 5 - rectInSrc.y;
    }

    if (rectInSrc.x < 0)
        rectInSrc.x = 0;
    if (rectInSrc.y < 0)
        rectInSrc.y = 0;
    if (rectInSrc.x + rectInSrc.width > img.cols)
        rectInSrc.width = img.cols - rectInSrc.x;
    if (rectInSrc.y + rectInSrc.height > img.rows)
    {
        rectInSrc.height = img.rows - rectInSrc.y;
    }

    merg_p_up.y = A_line_up;
    merg_p_up.x = img.cols;
    merg_p_down.y = A_line_down;
    merg_p_down.x = img.cols;
    roi = rectInSrc;

    if (bshowimg)
    {
        cv::rectangle(showimg, rectInSrc, cv::Scalar(0, 0, 255), 2);
        cv::imwrite(strcam + strchannel + "_rect.png", showimg);
    }

    return 0;
}

int ImageMerge::GetEdgePoint(const cv::Mat &img, Edge_Search_Config *pEdge_Search_Config, int &outP)
{
    bool bdbuge = false;
    bdbuge = bshowimg;
    if (img.empty())
    {
        return -11;
    }
    if (img.channels() != 1)
    {
        printf("*****************************channels\n");

        return -12;
    }
    // 检查roi;
    if (!CheckUtil::RoiInImg(pEdge_Search_Config->roi, img))
    {
        return -13;
    }
    if (!pEdge_Search_Config->CheckConfigValid())
    {
        return -14;
    }
    std::vector<cv::Point> pointList;
    bool search_UP_DOWN = false;
    cv::Rect roi = pEdge_Search_Config->roi;
    int sx = roi.x;
    int ex = roi.x + roi.width;
    int sy = roi.y;
    int ey = roi.y + roi.height;

    int nSearchLen = roi.width;                          // 搜索区域的长度
    int nCurPoint_Step = pEdge_Search_Config->stepCount; // 当前点搜索的步长

    // 搜索小范围确认
    int nhalfrang = (int)pEdge_Search_Config->nSearchrange / 2;
    int nrange_start = 0 - nhalfrang;
    int nrange_end = 0 - nhalfrang;
    int num = 0;
    while (true)
    {
        num++;
        nrange_end++;
        if (num >= pEdge_Search_Config->nSearchrange)
        {
            break;
        }
    }
    // 判断方向
    switch (pEdge_Search_Config->directSign)
    {
    case DirectSign_UP:
        search_UP_DOWN = true;
        nSearchLen = roi.width;
        sx = roi.x;
        ex = roi.x + roi.width;
        sy = roi.y;
        ey = roi.y + roi.height;
        nCurPoint_Step = pEdge_Search_Config->stepCount;
        break;
    case DirectSign_DOWN:
        search_UP_DOWN = true;
        nSearchLen = roi.width;
        nCurPoint_Step = -pEdge_Search_Config->stepCount;

        sx = roi.x;
        ex = roi.x + roi.width;
        sy = roi.y + roi.height - 1;
        ey = roi.y;
        break;
    case DirectSign_Left:
        nSearchLen = roi.height;
        search_UP_DOWN = false;
        nCurPoint_Step = pEdge_Search_Config->stepCount;

        sx = roi.x;
        ex = roi.x + roi.width;
        sy = roi.y;
        ey = roi.y + roi.height;
        break;
    case DirectSign_Right:
        nSearchLen = roi.height;
        search_UP_DOWN = false;
        nCurPoint_Step = -pEdge_Search_Config->stepCount;

        sx = roi.x + roi.width - 1;
        ex = roi.x;
        sy = roi.y;
        ey = roi.y + roi.height;
        break;
    default:
        break;
    }

    // 搜索 点数
    int nSearchPointNum = pEdge_Search_Config->nSearchCount;
    // 搜素点的间隔
    int nSearchPointStep = 1;
    if (nSearchPointNum > 1)
    {
        nSearchPointStep = int(nSearchLen * 1.0f / (nSearchPointNum - 1));
    }

    if (bdbuge)
    {
        cv::rectangle(showimg, roi, cv::Scalar(255, 0, 0), 2);
        printf("nSearchPointStep %d nCurPoint_Step %d,nrange_start %d %d\n", nSearchPointStep, nCurPoint_Step, nrange_start, nrange_end);
    }

    uchar *pdata = (uchar *)img.data;
    int offt = 0;
    // 搜索 上下边
    if (search_UP_DOWN)
    {
        // 每个搜索点 分布在X方向。
        for (int x = sx; x < ex; x = x + nSearchPointStep)
        {
            int bOKNum = 0;
            int cur_x = x;
            int cur_y = sy;
            bool bSucc = false;
            int succ_y = 0;

            // 对每个搜索点进行 y方向搜索
            for (int y = sy;; y = y + nCurPoint_Step)
            {

                if (y < 0 || y >= img.rows)
                {
                    continue;
                }

                //
                if (nCurPoint_Step > 0 && y > ey)
                {
                    break;
                }
                if (nCurPoint_Step < 0 && y < ey)
                {
                    break;
                }
                offt = y * img.cols;
                int range_okNum = 0;
                // 对一定范围的点进行判断
                for (int k = nrange_start; k < nrange_end; k++)
                {
                    int rangeX = x + k;
                    if (rangeX < 0 || rangeX >= img.cols)
                    {
                        continue;
                    }
                    offt += rangeX;
                    if (offt < 0 || offt >= img.cols * img.rows)
                    {
                        printf("rangeX %d nCurPoint_Step %d off %d  x %d  y %d sy %d ey %d  %d %d\n", rangeX, nCurPoint_Step, offt, x, y, sy, ey, img.cols, img.rows);
                    }

                    if (pdata[offt] >= pEdge_Search_Config->nValueThreshold) // 找到
                    {
                        range_okNum++;
                    }
                }
                if (range_okNum >= pEdge_Search_Config->nSearchrange)
                {
                    if (bdbuge)
                    {
                        cv::circle(showimg, cv::Point(x, y), 1, cv::Scalar(0, 0, 255));
                    }
                    if (bOKNum == 0)
                    {
                        cur_y = y;
                    }

                    bOKNum++;

                    // 连续搜索到 满足要求的点。
                    if (bOKNum >= pEdge_Search_Config->nLimit)
                    {
                        bSucc = true;
                        succ_y = y;
                        break;
                    }
                }
                else
                {
                    // 如果有间断，需要重新计算连续情况。
                    bOKNum = 0;
                    if (bdbuge)
                    {
                        cv::circle(showimg, cv::Point(x, y), 1, cv::Scalar(0, 255, 0));
                    }
                }
            }
            if (bSucc)
            {
                cv::Point p(cur_x, cur_y);
                if (bdbuge)
                {
                    cv::circle(showimg, p, 3, cv::Scalar(255, 0, 0));
                }
                pointList.push_back(p);

                if (pointList.size() > 5)
                {
                    // printf("=============feeeee\n");
                    sy = succ_y - 100;
                    ey = succ_y + 100;

                    switch (pEdge_Search_Config->directSign)
                    {
                    case DirectSign_UP:
                        sy = succ_y - 100;
                        ey = succ_y + 100;
                        break;
                    case DirectSign_DOWN:
                        sy = succ_y + 100;
                        ey = succ_y - 100;
                        break;

                    default:
                        break;
                    }

                    if (sy < 0)
                    {
                        sy = 0;
                    }
                    if (ey >= img.rows)
                    {
                        ey = img.rows - 1;
                    }
                    if (ey < 0)
                    {
                        ey = 0;
                    }
                    if (sy >= img.rows)
                    {
                        sy = img.rows - 1;
                    }
                }
            }
        }
    }
    else // 搜索 左右边
    {
        // 每个搜索点 分布在X方向。
        for (int y = sy; y < ey; y = y + nSearchPointStep)
        {
            int bOKNum = 0;
            int cur_x = sx;
            int cur_y = y;
            bool bSucc = false;
            int succ_x = 0;

            // 对每个搜索点进行 y方向搜索
            for (int x = sx;; x = x + nCurPoint_Step)
            {
                if (x < 0 || x >= img.cols)
                {
                    continue;
                }
                //
                if (nCurPoint_Step > 0 && x > ex)
                {
                    break;
                }
                if (nCurPoint_Step < 0 && x < ex)
                {
                    break;
                }

                int range_okNum = 0;
                // 对一定范围的点进行判断
                for (int k = nrange_start; k < nrange_end; k++)
                {
                    int rangey = y + k;
                    if (rangey < 0 || rangey >= img.rows)
                    {
                        continue;
                    }
                    offt = rangey * img.cols + x;
                    if (offt < 0 || offt >= img.cols * img.rows)
                    {
                        printf("rangey %d off %d  x %d  y %d ey %d  %d %d\n", rangey, offt, x, y, ey, img.cols, img.rows);
                    }
                    if (pdata[offt] >= pEdge_Search_Config->nValueThreshold) // 找到
                    {
                        range_okNum++;
                    }
                }
                if (range_okNum >= pEdge_Search_Config->nSearchrange)
                {
                    if (bdbuge)
                    {
                        cv::circle(showimg, cv::Point(x, y), 1, cv::Scalar(0, 0, 255));
                    }
                    if (bOKNum == 0)
                    {
                        cur_x = x;
                    }

                    bOKNum++;
                    // 连续搜索到 满足要求的点。
                    if (bOKNum >= pEdge_Search_Config->nLimit)
                    {
                        bSucc = true;
                        succ_x = x;
                        break;
                    }
                }
                else
                {
                    // 如果有间断，需要重新计算连续情况。
                    bOKNum = 0;
                    if (bdbuge)
                    {
                        cv::circle(showimg, cv::Point(x, y), 1, cv::Scalar(0, 255, 0));
                    }
                }
            }
            if (bSucc)
            {
                cv::Point p(cur_x, cur_y);
                if (bdbuge)
                {
                    cv::circle(showimg, p, 3, cv::Scalar(255, 0, 0));
                }
                pointList.push_back(p);
                if (pointList.size() > 5)
                {
                    // printf("=============feeeee\n");
                    sx = succ_x - 100;
                    ex = succ_x + 100;
                    if (sx < 0)
                    {
                        sx = 0;
                    }
                    if (ex >= img.cols)
                    {
                        ex = img.cols - 1;
                    }
                }
            }
        }
    }

    if (bdbuge)
    {
        cv::imwrite(pEdge_Search_Config->strchannel + "show.png", showimg);
    }

    {
        const int maxDeviation = 10; // 可调阈值：最大允许偏离像素

        std::vector<cv::Point> avgPoints;

        int PNum = pointList.size();
        // 第一次计算粗略均值
        int sum_y = 0, sum_x = 0;
        for (const auto &pt : pointList)
        {

            sum_x += pt.x;
            sum_y += pt.y;
        }
        float avg_y = sum_y * 1.0f / PNum;
        float avg_x = sum_x * 1.0f / PNum;

        // 过滤异常点
        std::vector<cv::Point> filtered;
        for (const auto &pt : pointList)
        {
            if (search_UP_DOWN) // 横向：判断 y 偏差
            {

                if (std::abs(pt.y - avg_y) <= maxDeviation)
                    filtered.push_back(pt);
            }
            else // 纵向：判断 x 偏差
            {
                if (std::abs(pt.x - avg_x) <= maxDeviation)
                    filtered.push_back(pt);
            }
        }

        if (filtered.empty())
            return -1;

        int sum_fx = 0, sum_fy = 0;
        for (const auto &pt : filtered)
        {
            sum_fx += pt.x;
            sum_fy += pt.y;
        }

        if (search_UP_DOWN) // 横向：判断 y 偏差
        {
            outP = sum_fy / (int)filtered.size();
        }
        else // 纵向：判断 x 偏差
        {
            outP = sum_fx / (int)filtered.size();
        }
    }

    return 0;
}

int ImageMerge::GetLine(const cv::Mat &img, std::vector<cv::Point> &pointList, int lineNum, int xory, int &outP)
{
    if (pointList.size() < 0)
        return -1;

    const int maxDeviation = 10; // 可调阈值：最大允许偏离像素

    std::vector<cv::Point> avgPoints;

    int PNum = pointList.size();
    // 第一次计算粗略均值
    int sum_y = 0, sum_x = 0;
    for (const auto &pt : pointList)
    {

        sum_x += pt.x;
        sum_y += pt.y;
    }
    float avg_y = sum_y * 1.0f / PNum;
    float avg_x = sum_x * 1.0f / PNum;

    // 过滤异常点
    std::vector<cv::Point> filtered;
    for (const auto &pt : pointList)
    {
        if (xory == 0) // 横向：判断 y 偏差
        {
            if (std::abs(pt.y - avg_y) <= maxDeviation)
                filtered.push_back(pt);
        }
        else // 纵向：判断 x 偏差
        {
            if (std::abs(pt.x - avg_x) <= maxDeviation)
                filtered.push_back(pt);
        }
    }
    if (filtered.empty())
        return -1;

    int sum_fx = 0, sum_fy = 0;
    for (const auto &pt : filtered)
    {
        sum_fx += pt.x;
        sum_fy += pt.y;
    }

    if (xory == 0) // 横向：判断 y 偏差
    {
        outP = sum_fy / (int)filtered.size();
    }
    else // 纵向：判断 x 偏差
    {
        outP = sum_fx / (int)filtered.size();
    }

    return 0;
}

bool ImageMerge::ProductSide_left(const cv::Mat &img)
{

    cv::Rect Roi_left, Right_Roi;
    Roi_left.x = 0;
    Roi_left.y = 0;
    Roi_left.width = img.cols * 0.1;
    Roi_left.height = img.rows;

    Right_Roi.x = img.cols - img.cols * 0.1;
    Right_Roi.y = 0;
    Right_Roi.width = img.cols * 0.1;
    Right_Roi.height = img.rows;

    cv::Mat left_img = img(Roi_left);
    cv::Mat right_img = img(Right_Roi);

    cv::Size sz = cv::Size(200, 600);

    cv::resize(left_img, left_img, sz);
    cv::resize(right_img, right_img, sz);
    int nonZeroCount_left = cv::countNonZero(left_img);
    int nonZeroCount_right = cv::countNonZero(right_img);
    if (nonZeroCount_left >= nonZeroCount_right)
    {
        return true;
    }
    else
    {
        return false;
    }

    return false;
}