#include "BeltTearingAlgo.h"
#include <climits>
#include <iostream>

BeltTearingAlgo::BeltTearingAlgo() {
    algoParam.differnceBinTh = 1.0;
    algoParam.extractPara.gapChkWin = 7;
    algoParam.extractPara.sameGapTh = 20.0;
    algoParam.scanXScale = 4.0;
    algoParam.scanYScale = 0.6;
    algoParam.tearingMinGap = 50.0; //两个同位置的纵撕的最小间隔。大于此门限视为两个撕裂
    algoParam.tearingMinLen = 30.0;
    lineIndex = 0;

    // 初始化
    ResetParameter();
}

void BeltTearingAlgo::ResetParameter() {
    total_tearings.clear();
    beltTearings_new.clear();
    beltTearings_growing.clear();
    beltTearings_ended.clear();
    beltTearings_unknown.clear();
    lineIndex = 0;
    int errCode = 0;
    sg_detectBeltTearing(
        NULL,  //空扫描线，用于复位内部静态变量
        0,
        0,
        &errCode,
        hLineWorkers,
        beltTearings_new,
        beltTearings_growing,
        beltTearings_ended,
        beltTearings_unknown,  //未判明，应用无需处理。
        algoParam);
}

void BeltTearingAlgo::Training(std::vector<SVzNL3DLaserLine> lines) {
    // 本算法无需训练
}

void BeltTearingAlgo::Training(SVzNL3DLaserLine* lines) {
    // 本算法无需训练
}

std::vector<SSG_beltTearingInfo> BeltTearingAlgo::Predit(SVzNL3DLaserLine* a_line, int lineId) {
    total_tearings.clear();
    int errCode = 0;
    sg_detectBeltTearing(
        a_line,  //一条扫描线
        lineIndex,
        a_line->nPositionCnt,
        &errCode,
        hLineWorkers,
        beltTearings_new,
        beltTearings_growing,
        beltTearings_ended,
        beltTearings_unknown,  //未判明，应用无需处理。
        algoParam);
    if (beltTearings_ended.size() > 0) //收集撕裂点
    {
        total_tearings.insert(total_tearings.end(), beltTearings_ended.begin(), beltTearings_ended.end());
    }

    return total_tearings;
}

std::vector<SSG_beltTearingInfo> BeltTearingAlgo::Predit(std::vector<SVzNL3DLaserLine> lines, int lineIdx) {
    total_tearings.clear();
    if (lines.empty()) {
        return total_tearings;
    }

    if (lineIndex >= INT_MAX - lines.size() ) {
        lineIndex = 0;
    }

    hLineWorkers.resize(lines[0].nPositionCnt);
    int errCode = 0;
    for(int i = 0; i < lines.size(); i ++) {
        SVzNL3DLaserLine* a_line = &lines[i];        
        sg_detectBeltTearing(
            a_line,  //一条扫描线
            lineIndex,
            a_line->nPositionCnt,
            &errCode,
            hLineWorkers,
            beltTearings_new,
            beltTearings_growing,
            beltTearings_ended,
            beltTearings_unknown,  //未判明，应用无需处理。
            algoParam);
        lineIndex += 1;
        // if (beltTearings_unknown.size() > 0) {
        //     std::cout << "未知的撕裂数量: " << beltTearings_unknown.size() << std::endl;
        // }

        // if (beltTearings_growing.size() > 0) {
        //     std::cout << "进行中的撕裂数量: " << beltTearings_growing.size() << std::endl;
        // }

        if (beltTearings_new.size() > 0) //收集撕裂点
        {
            total_tearings.insert(total_tearings.end(), beltTearings_new.begin(), beltTearings_new.end());
        }
        
        if (beltTearings_ended.size() > 0) //收集撕裂点
        {
            total_tearings.insert(total_tearings.end(), beltTearings_ended.begin(), beltTearings_ended.end());
        }
        else if (i == lines.size() - 1)  //测试数据最后一条扫描线。（实际中没有）
        {
            total_tearings.insert(total_tearings.end(), beltTearings_growing.begin(), beltTearings_growing.end());
        }
    }

    // TODO: 返回值有可能为空，也有可能为当前检测的撕裂
    return total_tearings;
}