#include "main.h"

#include "HxDevice.h"

CarInfoInput car_info;

static HxDevice device[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 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(BROADCAST_PORT);
    QObject::connect(HxBroadcast::context(), &HxBroadcast::receive_event, [=](QByteArray data) { broadcast_receive_event(data); });

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

    algorithm_initialization();

    car_info.fVelocity = 50;

    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(true);

    /* 目标跟踪事件检测初始化 */
    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

    /* 读取配置文件 */
    auto settings = new QSettings("/opt/config.ini", QSettings::IniFormat);

    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", "0;0;125;0;125;0;0;0;0;0;0;0;0;125;0;0;0;0;0;0;0;0;0;0;0;0").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;

    /* 获取通道配置信息 */
    for (int i = 1; i <= CHANNEL_MAX; i++)
    {
        settings->beginGroup(QString("channel_%1").arg(i, 2, 10, QLatin1Char('0')));

        auto url = settings->value("url").toString();
        auto file = settings->value("file").toString();
        auto type = settings->value(QString("mode")).toInt();

        if (url.isEmpty() && file.isEmpty())
            goto ENDGROUP;

        if (type == 0) /* ADAS */
        {
            detect_channel |= ADAS_DETECT_CHANNEL;
        }
        else if (type == 2) /* DSM */
        {
            detect_channel |= DMS_DETECT_CHANNEL;
        }
        else /* BSD */
        {
            switch (type)
            {
            case 6: /* 前 */
                detect_channel |= FRONT_BSD_DETECT_CHANNEL;
                break;
            case 7: /* 后 */
                detect_channel |= REAR_BSD_DETECT_CHANNEL;
                break;
            case 4:/* 左前 */
                detect_channel |= LEFT_BSD_FRONT_DETECT_CHANNEL;
                break;
            case 1:/* 右前 */
                detect_channel |= RIGHT_BSD_FRONT_DETECT_CHANNEL;
                break;
            case 5:/* 左后 */
                detect_channel |= LEFT_BSD_REAR_DETECT_CHANNEL;
                break;
            case 3:/* 右后 */
                detect_channel |= RIGHT_BSD_REAR_DETECT_CHANNEL;
                break;

            default:
                goto ENDGROUP;
            }

            BsdWarnRegion region;

            auto array = settings->value("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().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
        }

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

        device[i].startup(type, url, file);

        HxTrace::debug_write_line("application", QString("channel: %1 type: %2 url: %3 file: %4").arg(i).arg(type).arg(url, file));

    ENDGROUP:
        settings->endGroup();
    }

#if USE_ALGORITHM
    MvSetDetectChannel(detect_channel);
#endif
}

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

    if (pObjectTrackEventResult->nEventType)
    {
        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));

        auto channel = 0;

        QJsonObject msginfo, object_infos, face_land_marks;

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

        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);
    }
}