#include "main.h"

#include "HxDevice.h"

CarInfoInput car_info;

static HxDevice adas_device, dsm_device, bsd_devices[6];

/* 报警参数配置 */
EventWarnParamConfig warm_param_config;

static ObjectEventDetectConfig event_detect_config;
/* ADAS 相机内外参 */
static CameraCalibration adas_camera_calibration;

static void broadcast_receive_event(QByteArray data);

static void algorithm_initialization();
static void set_bsd_warn_region(int type, QString param);
static void set_algorithm_detection_callback(int type);

/* 算法报警处理函数 */
static void algorithm_detection_callback(int nDataChannel, ObjectTrackEventResult* pObjectTrackEventResult, void* pPrivData);

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

    /*  需要设置的运行时默认工作路径 */
    QDir::setCurrent(QCoreApplication::applicationDirPath());

    /* 设置最大线程个数 */
    QThreadPool::globalInstance()->setMaxThreadCount(100);

    /* UDP广播 初始化 */
    HxBroadcast::initialization(5001, "127.0.0.1");
    QObject::connect(HxBroadcast::context(), &HxBroadcast::receive_event, [=](QByteArray data) { broadcast_receive_event(data); });

    WRITE_SYSTEM_LOG("系统启动", "HxIVA");

    algorithm_initialization();

    car_info.fVelocity = 50;

    /* 向服务程序发送 心跳数据 */
    HxTask::run([=]()
                {
                    HxBroadcast::publish_json(0, { {"name", "HxIVA"}, {"version", "1.00"} });
                }, 1000, "heart_third");

    return a.exec();
}

void broadcast_receive_event(QByteArray data)
{
    int action_type = -1;

    auto document = QJsonDocument::fromJson(data);
    auto root = document.object();
    if (!root.contains("action_type"))
        return;

    action_type = root.value("action_type").toInt();

    switch (action_type)
    {
    case 100:
        HxTask::invoke([=](QJsonObject _msginfo){

            if(_msginfo.size() == 0)
                return;

            car_info.fVelocity = _msginfo.value("speed").toDouble();
            car_info.nBrake = _msginfo.value("brake").toInt();
            car_info.nLLight = _msginfo.value("left").toInt();
            car_info.nRLight = _msginfo.value("right").toInt();

        }, root.value("msgInfo").toObject());
        break;
    }
}

/* 算法模块初始化 */
static void algorithm_initialization()
{
    /* adas检测配置 */
    strcpy(event_detect_config.szAdasDetectConfigPathName, "/opt/algmode/adas_detect.bin");
    /* adas跟踪配置 */
    strcpy(event_detect_config.szAdasTrackConfigPathName, "/opt/algmode/adas_track.bin");
    /* bsd检测配置 */
    strcpy(event_detect_config.szBsdDetectConfigPathName, "/opt/algmode/bsd_detect.bin");
    // strcpy(event_detect_config.szRightBsdFrontDetectConfigPathName, "/opt/algmode/bsd_detect.bin");
    /* dsm人脸检测配置 */
    strcpy(event_detect_config.szDsmFaceDetectConfigPathName, "/opt/algmode/dsm_face_detect.bin");
    /* dsm人脸特征点检测配置 */
    strcpy(event_detect_config.szDsmFaceLandMarksDetectConfigPathName, "/opt/algmode/dsm_face_landmarks_detect.bin");
    /* dsm人脸认证检测配置 */
    strcpy(event_detect_config.szDsmFaceVerificationDetectConfigPathName, "/opt/algmode/dsm_face_verification_detect.bin");
    /* dsm人眼认证检测配置 */
    strcpy(event_detect_config.szDsmEyeLandMarksDetectConfigPathName, "/opt/algmode/dsm_eye_landmarks_detect.bin");
    /* dsm人脸认证检测配置 */
    strcpy(event_detect_config.szDsmSmokeConfPathName, "/opt/algmode/dsm_smoke_detect.bin");
    strcpy(event_detect_config.szDsmCallConfPathName, "/opt/algmode/dsm_call_detect.bin");
    strcpy(event_detect_config.szDsmFaceFeaturePathName, "/opt/algmode/dsm_face_feature.bin");
    strcpy(event_detect_config.szDsmHeadPoseConfPathName, "/opt/algmode/data_68kp");

#if USE_ALGORITHM
    /* 输出调试信息 */
    MvSetPrintf(false);

    /* 目标跟踪事件检测初始化 */
    if (MvObjectEventDetectInit(&event_detect_config, RIGHT_BSD_FRONT_DETECT_CHANNEL |
                                                          RIGHT_BSD_REAR_DETECT_CHANNEL |
                                                          LEFT_BSD_FRONT_DETECT_CHANNEL |
                                                          LEFT_BSD_REAR_DETECT_CHANNEL |
                                                          FRONT_BSD_DETECT_CHANNEL |
                                                          REAR_BSD_DETECT_CHANNEL |
                                                          DMS_DETECT_CHANNEL |
                                                          ADAS_DETECT_CHANNEL) != 0)
    {
        // HxLog::append("initialization", "object event detect init failed!!");
        qDebug() << "object event detect init failed!!";
        return;
    }
#endif

    /* 读取配置文件 */
#if defined(Q_OS_WIN)
    auto settings = new QSettings("config.ini", QSettings::IniFormat);
#else
    auto settings = new QSettings("config.ini", QSettings::IniFormat);
#endif

    auto start_event_warn_kind = settings->value("warn_param/start_event_warn_kind", "1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1;1").toString().split(";");
    auto abnormal_warn_frame_count = settings->value("warn_param/abnormal_warn_frame_count", "0;0;0;0;0;0;0;50;75;25;50;75;50;50;50;25;75;50;50;50;50;50;75;50;50;100").toString().split(";");
    auto normal_frame_count = settings->value("warn_param/normal_frame_count", "0;0;0;0;0;0;0;25;25;25;25;25;25;25;25;25;25;25;25;25;25;25;0;0;0;0").toString().split(";");
    auto abnormal_warn_interval_frame_count = settings->value("warn_param/abnormal_warn_interval_frame_count", "125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125;125").toString().split(";");
    auto abnormal_warn_score_threshold = settings->value("warn_param/abnormal_warn_score_threshold", "0;0;0;0;0;0;0;0.7;0.8;0.4;0.65;0.95;0.75;0.3;0.65;0.5;0.87;25;25;20;15;0.75;0.3;0.5;0.5;0.5").toString().split(";");
    auto normal_warn_score_threshold = settings->value("warn_param/normal_warn_score_threshold", "0;0;0;0;0;0;0;0.35;0.35;0.7;0.7;0.7;0.55;0.8;0.4;0.55;0.13;20;20;15;10;0.55;0.3;0.3;0.5;0.5").toString().split(";");

    for (int i = 0; i < EVENT_WARN_NUM; i++)
    {
        warm_param_config.bStartEventWarnKind[i] = QVariant(start_event_warn_kind.at(i)).toBool();
        warm_param_config.nAbnormalWarnFrameCount[i] = abnormal_warn_frame_count.at(i).toInt();
        warm_param_config.nNormalFrameCount[i] = normal_frame_count.at(i).toInt();
        warm_param_config.nAbnormalWarnIntervalFrameCount[i] = abnormal_warn_interval_frame_count.at(i).toInt();
        warm_param_config.fAbnormalWarnScoreThreshold[i] = abnormal_warn_score_threshold.at(i).toFloat();
        warm_param_config.fNormalWarnScoreThreshold[i] = normal_warn_score_threshold.at(i).toFloat();
    }

    warm_param_config.nHmwTime = settings->value("warn_param/hmw_time", 1200).toInt();
    warm_param_config.nPcwTime = settings->value("warn_param/pcw_time", 2000).toInt();
    warm_param_config.nFcwTime = settings->value("warn_param/fcw_time", 2400).toInt();
    warm_param_config.fHmwVel = settings->value("warn_param/hmw_vel", 30).toDouble();
    warm_param_config.fFcwVel = settings->value("warn_param/fcw_vel", 30).toDouble();
    warm_param_config.fLdwVel = settings->value("warn_param/ldw_vel", 55).toDouble();
    warm_param_config.fPcwVel = settings->value("warn_param/pcw_vel", 50).toDouble();
    warm_param_config.fBsdFirstVel = settings->value("warn_param/bsd_first_vel", 30).toDouble();
    warm_param_config.fBsdSecondVel = settings->value("warn_param/bsd_second_vel", 30).toDouble();
    warm_param_config.fBsdThirdVel = settings->value("warn_param/bsd_third_vel", 30).toDouble();
    warm_param_config.nLdwDistance = settings->value("warn_param/ldw_distance", -5).toInt();
    warm_param_config.fDsmVel = settings->value("warn_param/dsm_vel", 5).toDouble();

#if USE_ALGORITHM
    /* 设置报警参数 */
    if (MvSetEventWarnParamConfig(&warm_param_config) != 0)
        // HxLog::append("initialization", "set event warn param config failed!!");
        qDebug() << "set event warn param config failed!!";
#endif

    adas_camera_calibration.fCarLen = settings->value("adas_camera_calibration/car_len", 4527).toInt();
    adas_camera_calibration.fCarWidth = settings->value("adas_camera_calibration/car_width", 1830).toInt();
    adas_camera_calibration.fRefCenter = settings->value("adas_camera_calibration/ref_center", 0).toInt();
    adas_camera_calibration.fRefTop = settings->value("adas_camera_calibration/ref_top", 1695).toInt();
    adas_camera_calibration.fDisLen2Tyre = settings->value("adas_camera_calibration/dis_len_2_tyre", 0).toInt();
    adas_camera_calibration.fCameraHeight = settings->value("adas_camera_calibration/camera_height", 1500).toInt();
    adas_camera_calibration.fCameraFocus = settings->value("adas_camera_calibration/camera_focus", 6.00).toDouble();
    adas_camera_calibration.fCameraDx = settings->value("adas_camera_calibration/camera_dx", 0.00).toDouble();
    adas_camera_calibration.fPitch = settings->value("adas_camera_calibration/pitch", -0.32).toDouble();
    adas_camera_calibration.fYaw = settings->value("adas_camera_calibration/yaw", 0.00).toDouble();

#if USE_ALGORITHM
    /* 相机标定 */
    if (MvCameraCalibration(&adas_camera_calibration) != 0)
        // HxLog::append("initialization", "camera calibration failed!!");
        qDebug() << "setcamera calibration failed!!";
#endif

    int detect_channel = 0;

    /* adas */
    {
        auto type = 0;
        auto url = settings->value("ADAS/url").toString();
        auto file = settings->value("ADAS/file").toString();

        HxTrace::debug_write_line("application", QString("[ADAS] url: %1 file: %2").arg(url, file));

        if (!(url.isEmpty() && file.isEmpty()))
        {
            detect_channel |= ADAS_DETECT_CHANNEL;

            /* adas 设备启动 */
            adas_device.startup(type, url, file);

            /* 设置回调函数 */
            set_algorithm_detection_callback(type);

        }
    }

    /* dsm */
    {
        auto type = 2;
        auto url = settings->value("DSM/url").toString();
        auto file = settings->value("DSM/file").toString();

        HxTrace::debug_write_line("application", QString("[DSM] url: %1 file: %2").arg(url, file));

        if (!(url.isEmpty() && file.isEmpty()))
        {
            detect_channel |= DMS_DETECT_CHANNEL;

            /* dsm 设备启动 */
            dsm_device.startup(type, url, file);

            /* 设置回调函数 */
            set_algorithm_detection_callback(type);

        }
    }

    /* BSD_A 前 */
    {
        auto type = 6;
        auto url = settings->value("BSD_A/url").toString();
        auto file = settings->value("BSD_A/file").toString();
        auto param = settings->value("BSD_A/param", "1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720").toString();

        HxTrace::debug_write_line("application", QString("[BSD A] url: %1 file: %2").arg(url, file));

        if (!(url.isEmpty() && file.isEmpty()))
        {
            detect_channel |= FRONT_BSD_DETECT_CHANNEL;

            /* BSD 设备启动 */
            bsd_devices[0].startup(type, url, file);

            /* 设置检测区域 */
            set_bsd_warn_region(type, param);

            /* 设置回调函数 */
            set_algorithm_detection_callback(type);
        }
    }

    /* BSD_B 后 */
    {
        auto type = 7;
        auto url = settings->value("BSD_B/url").toString();
        auto file = settings->value("BSD_B/file").toString();
        auto param = settings->value("BSD_B/param", "1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720").toString();

        HxTrace::debug_write_line("application", QString("[BSD B] url: %1 file: %2").arg(url, file));

        if (!(url.isEmpty() && file.isEmpty()))
        {
            detect_channel |= REAR_BSD_DETECT_CHANNEL;

            /* BSD 设备启动 */
            bsd_devices[1].startup(type, url, file);

            /* 设置检测区域 */
            set_bsd_warn_region(type, param);

            /* 设置回调函数 */
            set_algorithm_detection_callback(type);
        }
    }

    /* BSD_C 左前 */
    {
        auto type = 4;
        auto url = settings->value("BSD_C/url").toString();
        auto file = settings->value("BSD_C/file").toString();
        auto param = settings->value("BSD_C/param", "1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720").toString();

        HxTrace::debug_write_line("application", QString("[BSD C] url: %1 file: %2").arg(url, file));

        if (!(url.isEmpty() && file.isEmpty()))
        {
            detect_channel |= LEFT_BSD_FRONT_DETECT_CHANNEL;

            /* BSD 设备启动 */
            bsd_devices[2].startup(type, url, file);

            /* 设置检测区域 */
            set_bsd_warn_region(type, param);

            /* 设置回调函数 */
            set_algorithm_detection_callback(type);
        }
    }

    /* BSD_D 右前 */
    {
        auto type = 1;
        auto url = settings->value("BSD_D/url").toString();
        auto file = settings->value("BSD_D/file").toString();
        auto param = settings->value("BSD_D/param", "1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720").toString();

        HxTrace::debug_write_line("application", QString("[BSD D] url: %1 file: %2").arg(url, file));

        if (!(url.isEmpty() && file.isEmpty()))
        {
            detect_channel |= RIGHT_BSD_FRONT_DETECT_CHANNEL;

            /* BSD 设备启动 */
            bsd_devices[3].startup(type, url, file);

            /* 设置检测区域 */
            set_bsd_warn_region(type, param);

            /* 设置回调函数 */
            set_algorithm_detection_callback(type);
        }
    }

    /* BSD_E 左后 */
    {
        auto type = 5;
        auto url = settings->value("BSD_E/url").toString();
        auto file = settings->value("BSD_E/file").toString();
        auto param = settings->value("BSD_E/param", "1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720").toString();

        HxTrace::debug_write_line("application", QString("[BSD E] url: %1 file: %2").arg(url, file));

        if (!(url.isEmpty() && file.isEmpty()))
        {
            detect_channel |= LEFT_BSD_REAR_DETECT_CHANNEL;

            /* BSD 设备启动 */
            bsd_devices[4].startup(type, url, file);

            /* 设置检测区域 */
            set_bsd_warn_region(type, param);

            /* 设置回调函数 */
            set_algorithm_detection_callback(type);
        }
    }

    /* BSD_F 右后 */
    {
        auto type = 3;
        auto url = settings->value("BSD_F/url").toString();
        auto file = settings->value("BSD_F/file").toString();
        auto param = settings->value("BSD_F/param", "1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720$1280,0 0,0 0,720 1280,720").toString();

        HxTrace::debug_write_line("application", QString("[BSD F] url: %1 file: %2").arg(url, file));

        if (!(url.isEmpty() && file.isEmpty()))
        {
            detect_channel |= RIGHT_BSD_REAR_DETECT_CHANNEL;

            /* BSD 设备启动 */
            bsd_devices[3].startup(type, url, file);

            /* 设置检测区域 */
            set_bsd_warn_region(type, param);

            /* 设置回调函数 */
            set_algorithm_detection_callback(type);
        }
    }

#if USE_ALGORITHM
    MvSetDetectChannel(detect_channel);
#endif
}

static void set_bsd_warn_region(int type, QString param)
{
    Q_UNUSED(type);

    BsdWarnRegion region;

    Q_UNUSED(region);

    auto array = param.split("$");
    auto points = array[0].split(" ");
    for (int a = 0; a < 4; a++)
    {
        auto p = points.at(a).split(",");

        region.tFirstRegionPoint[a].x = p[0].toInt();
        region.tFirstRegionPoint[a].y = p[1].toInt();
    }

    points = array[1].split(" ");
    for (int a = 0; a < 4; a++)
    {
        auto p = points.at(a).split(",");

        region.tSecondRegionPoint[a].x = p[0].toInt();
        region.tSecondRegionPoint[a].y = p[1].toInt();
    }

    points = array[2].split(" ");
    for (int a = 0; a < 4; a++)
    {
        auto p = points.at(a).split(",");

        region.tThirdRegionPoint[a].x = p[0].toInt();
        region.tThirdRegionPoint[a].y = p[1].toInt();
    }

#if USE_ALGORITHM
    MvSetBsdWarnRegion(type, &region);
#endif
}

static void set_algorithm_detection_callback(int type)
{
    Q_UNUSED(type);

#if USE_ALGORITHM
    if (MvSetAlgResultFuncCallback(type, algorithm_detection_callback, nullptr) != 0)
    {
        HxTrace::debug_write_line("application", "MvSetAlgResultFuncCallback failed!!");
        return;
    }
#endif
}

void algorithm_detection_callback(int nDataChannel, ObjectTrackEventResult* pObjectTrackEventResult, void* pPrivData)
{
    Q_UNUSED(nDataChannel);
    Q_UNUSED(pObjectTrackEventResult);
    Q_UNUSED(pPrivData);

    if(pObjectTrackEventResult->nEventType == 0)
        return;

    HxDevice *device;

    switch(nDataChannel)
    {
    case 0:
        device = &adas_device;
        break;
    case 2:
        device = &dsm_device;
        break;
    case 6:
        device = &bsd_devices[0];
        break;
    case 7:
        device = &bsd_devices[1];
        break;
    case 4:
        device = &bsd_devices[2];
        break;
    case 1:
        device = &bsd_devices[3];
        break;
    case 5:
        device = &bsd_devices[4];
        break;
    case 3:
        device = &bsd_devices[5];
        break;
    }


    /* 获取报警类型 */
    QList<int> event_types;
    for(int i = 0; i < 26; i++)
    {
        if(((pObjectTrackEventResult->nEventType >> i) & 0x01) == 1)
            event_types.append(i);
    }

    for(int i = 0; i < event_types.size(); i++)
    {
        auto event_type = event_types.at(i);
        if (event_type)
        {
            if(!device->determine_alarm_detection_timestamp(event_type))
                continue;

            HxTrace::debug_write_line("algorithm", QString("nDataChannel=%1, nFrameId=%2, nObjectNumber=%3, nEventType=%4\n").arg(nDataChannel).arg(pObjectTrackEventResult->nFrameId).arg(pObjectTrackEventResult->nObjectNumber).arg(pObjectTrackEventResult->nEventType));

            QJsonObject msginfo, object_infos, face_land_marks;

            msginfo.insert("timestamp", QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss"));
            msginfo.insert("channel", 0);
            msginfo.insert("level", pObjectTrackEventResult->nDangerLevel);
            msginfo.insert("event_type", event_type);

            if(pObjectTrackEventResult->nObjectNumber > 0)
            {
                for(int i = 0; i < pObjectTrackEventResult->nObjectNumber; i++)
                {
                    object_infos.insert("detect_type", pObjectTrackEventResult->objInfo[i].nDetectType);
                    object_infos.insert("left", pObjectTrackEventResult->objInfo[i].nLeft);
                    object_infos.insert("top", pObjectTrackEventResult->objInfo[i].nTop);
                    object_infos.insert("right", pObjectTrackEventResult->objInfo[i].nRight);
                    object_infos.insert("bottom", pObjectTrackEventResult->objInfo[i].nBottom);
                    object_infos.insert("distance", pObjectTrackEventResult->objInfo[i].fDist);
                    object_infos.insert("speed", pObjectTrackEventResult->objInfo[i].fVelo);
                    object_infos.insert("ttc", pObjectTrackEventResult->objInfo[i].fTTC);
                    object_infos.insert("target_post_x", pObjectTrackEventResult->objInfo[i].nTargetPosX);
                    object_infos.insert("target_post_y", pObjectTrackEventResult->objInfo[i].nTargetPosY);
                }
                msginfo.insert("object_infos", object_infos);
            }

            if(pObjectTrackEventResult->nObjectNumber > 0)
            {
                for(int i = 0; i < pObjectTrackEventResult->nFaceLandMarksNum; i++)
                {
                    face_land_marks.insert("x", pObjectTrackEventResult->tFaceLandMarks[i].x);
                    face_land_marks.insert("y", pObjectTrackEventResult->tFaceLandMarks[i].y);
                }
                msginfo.insert("face_land_marks", face_land_marks);
            }

            msginfo.insert("left_line_type", pObjectTrackEventResult->nLeftLineType);
            msginfo.insert("right_line_type", pObjectTrackEventResult->nRightLineType);

            HxBroadcast::publish_json(101, msginfo);
        }
    }
}