#include "asj_pe_pullup.h"
#include "utils.h"

// 根据box的x坐标进行排序
static bool compareByX(const Hand_ST& b1, const Hand_ST& b2) {
    return b1.object.stRect.x < b2.object.stRect.x;
}

int Asj_PE_Pullup::init()
{
    // 一些预设定的参数
    handAngle = 160.0; // 双手弯曲角度
    legAngle = 90.0; // 膝盖弯曲角度
    shouldersRatio = 0.5; // 双肩系数 双手的距离<=双肩的距离*(1+shouldersRatio)
    handQueueLength = 20; // 有关手部状态的队列长度
    overTime = 10; // 每个引体向上计数时候的超时时间/秒
    handKeypointDisThresh = 0.1; // 手部关键点到杠的距离阈值

    // 初始化标志位flag
    prevFrameStatus = false;
    isStraighten = false;
    isEyeoverbar = false;
    isFinishone = false;
    eyeStatus = false;

    // 初始化队列长度
    prepareStatueQueue.init(50);
    eyeStatusQueue.init(5);
    handStatusQueue.init(handQueueLength);
    leftHandStatusQueue.init(handQueueLength);
    rightHandStatusQueue.init(handQueueLength);

    // 初始化结果结构体
    std::memset(&pullupResult_st, 0, sizeof(PullUpResult_ST));

    return StatusCode::SUCCESS;
}

int Asj_PE_Pullup::set(PullUp_ST pullup_s)
{
    std::memcpy(&pullup_st, &pullup_s, sizeof(PullUp_ST));
    
    // 初始化杠的参数
    leftBar = pullup_s.asjPullUpLine[0];
    rightBar = pullup_s.asjPullUpLine[1];
    centerBar.x = (leftBar.x + rightBar.x) / 2;
    centerBar.y = (leftBar.y + rightBar.y) / 2;
    centerBar.score = 0.0;

    std::cout << "arm_bend_flag: " << pullup_st.arm_bend_flag << std::endl;
    std::cout << "hand_back_flag: " << pullup_st.hand_back_flag << std::endl;
    std::cout << "body_straight_flag: " << pullup_st.body_straight_flag << std::endl;
    std::cout << "hand_off_bar_flag: " << pullup_st.hand_off_bar_flag << std::endl;
    std::cout << "knee_bend_flag: " << pullup_st.knee_bend_flag << std::endl;
    std::cout << "exam_interval_sencond: " << pullup_st.exam_interval_sencond << std::endl;
    return StatusCode::SUCCESS;
}

int Asj_PE_Pullup::get(PullUp_ST *pullup_s)
{
    std::memcpy(pullup_s, &pullup_st, sizeof(PullUp_ST));
    return StatusCode::SUCCESS;
}

int Asj_PE_Pullup::examReset()
{
    prepareStatueQueue.reset(); // 重置记录准备状态队列
    eyeStatusQueue.reset(); // 重置眼部是否过杠的队列状态
    handStatusQueue.reset(); // 重置手部队列状态
    leftHandStatusQueue.reset(); // 重置左手队列状态
    rightHandStatusQueue.reset(); // 重置右手队列状态

    // 重新初始化标志位flag
    prevFrameStatus = false;
    isStraighten = false;
    isEyeoverbar = false;
    isFinishone = false;
    eyeStatus = false; 

    std::memset(&pullupResult_st, 0, sizeof(PullUpResult_ST)); // 重置相关结果参数
    return StatusCode::SUCCESS;
}

void Asj_PE_Pullup::findNearestPeople(sampleRunJoint_RESULT_S *mResults, int &personIndex)
{
    float maxDistance = std::numeric_limits<float>::max();
    int personIndexTemp = -1;
    handBox.clear();
    for(int i = 0;i < mResults->nObjectSize;i++){
        if (mResults->pstObjectItems[i].labelId == 0)
        {
            float x = mResults->pstObjectItems[i].stRect.x;
            float y = mResults->pstObjectItems[i].stRect.y + mResults->pstObjectItems[i].stRect.h / 2;
            float distance = EUCLIDEAN_DISTANCE(centerBar.x, centerBar.y, x, y);
            if (maxDistance > distance){
                maxDistance = distance;
                personIndexTemp = i;
            }
        }
        else if (mResults->pstObjectItems[i].labelId == 1)
        {
            // 判断手是否与bar相交
            if (isLineIntersectRect(leftBar, rightBar, mResults->pstObjectItems[i].stRect))
            {
                Hand_ST hand;
                memset(&hand, 0, sizeof(Hand_ST));
                hand.object = mResults->pstObjectItems[i];
                hand.idx = i;
                handBox.push_back(hand);
            }
        }
    }

    personIndex = personIndexTemp;

    // 防止手没有识别到的情况,利用手的关键点与杠的y轴距离进行计算
    if (personIndex != -1)
    {
        sampleRunJoint_POINT_S *landmark = mResults->pstObjectItems[personIndex].landmark;
        if (handBox.size() != 2)
        {
            handBox.clear();
            if (std::abs(landmark[7].y - leftBar.y) < handKeypointDisThresh)
            {
                Hand_ST hand;
                memset(&hand, 0, sizeof(Hand_ST));
                handBox.push_back(hand);
            }
            if (std::abs(landmark[8].y - leftBar.y) < handKeypointDisThresh)
            {
                Hand_ST hand;
                memset(&hand, 0, sizeof(Hand_ST));
                handBox.push_back(hand);
            }
        }
    }
}

int Asj_PE_Pullup::fbHeadDetect(sampleRunJoint_POINT_S *landmark, bool isLeft)
{
    if (isLeft)
    {
        if (landmark[1].x < landmark[5].x)
        {
            return true;
        } else {
            return false;
        }
    }
    else
    {
        if (landmark[1].x > landmark[5].x)
        {
            return true;
        } else {
            return false;
        }
    }
}

bool Asj_PE_Pullup::checkHandIsStraighten(sampleRunJoint_POINT_S *landmark)
{
    float left_hand_angle = CALC_ANGLE(landmark[5], landmark[3], landmark[7]);
    float right_hand_angle = CALC_ANGLE(landmark[6], landmark[4], landmark[8]);
    return (right_hand_angle > handAngle && left_hand_angle > handAngle);
}

bool Asj_PE_Pullup::checkLegIsStraighten(sampleRunJoint_POINT_S *landmark)
{
    float left_leg_angle = CALC_ANGLE(landmark[11], landmark[9], landmark[13]);
    float right_leg_angle = CALC_ANGLE(landmark[12], landmark[10], landmark[14]);
    return (right_leg_angle > handAngle && left_leg_angle > handAngle);
}

bool Asj_PE_Pullup::checkIsBackHand()
{
    if (leftHandStatusQueue.real_size() == leftHandStatusQueue.size())
    {
        if ((float(leftHandStatusQueue.counts_one()) / leftHandStatusQueue.size()) < 0.85)
        {
            return true;
        }
    }

    if (rightHandStatusQueue.real_size() == rightHandStatusQueue.size())
    {
        if ((float(rightHandStatusQueue.counts_one()) / rightHandStatusQueue.size()) < 0.85)
        {
            return true;
        }
    }

    return false;
}

int Asj_PE_Pullup::processPrepareCPP(sampleRunJoint_RESULT_S *mResults, int &prepareStatus)
{
    sampleRunJoint_POINT_S *landmark;
    int personIndex = -1;

    findNearestPeople(mResults, personIndex);
    if (personIndex != -1)
    {
        // 判断双手是否在杠上
        if (handBox.size() == 2)
        {
            // 当前帧正反手识别
            if (pullup_st.hand_back_flag)
            {
                std::sort(handBox.begin(), handBox.end(), compareByX);
                rightHandStatusQueue.push(int(fbHeadDetect(handBox[0].object.landmark, false)));
                leftHandStatusQueue.push(int(fbHeadDetect(handBox[1].object.landmark, true)));
            }
            // 获取这个离杠最近的人的关键点
            landmark = mResults->pstObjectItems[personIndex].landmark;

            // 判断手臂是否伸直
            if(pullup_st.arm_bend_flag && !checkHandIsStraighten(landmark))
            {
                prepareStatus |= (1 << PullUpPrepareCode::PULLUP_PREPARE_HANDSNOTSTRAIGHTEN);
            }

            // 判断腿部是否伸直
            if(pullup_st.knee_bend_flag && !checkLegIsStraighten(landmark))
            {
                prepareStatus |= (1 << PullUpPrepareCode::PULLUP_PREPARE_KNEENOTSTRAIGHTEN);
            }

            // 判断双手距离是否少于双肩距离*(1+shouldersRatio)
            if (EUCLIDEAN_DISTANCE(landmark[7].x, landmark[7].y, landmark[8].x, landmark[8].y) > 
            (EUCLIDEAN_DISTANCE(landmark[3].x, landmark[3].y, landmark[4].x, landmark[4].y) * (1 + shouldersRatio)))
            {
                prepareStatus |= (1 << PullUpPrepareCode::PULLUP_PREPARE_HANDSWIDERTHANSHOULDERS);
            }

            // 判断是否正反手
            if (pullup_st.hand_back_flag && checkIsBackHand())
            {
                prepareStatus |= (1 << PullUpPrepareCode::PULLUP_PREPARE_BACKHAND);
            }
        }
        else
        {
            prepareStatus |= (1 << PullUpPrepareCode::PULLUP_PREPARE_HANDSNOTONBAR);
        }
    }
    else
    {
        prepareStatus |= (1 << PullUpPrepareCode::PULLUP_PREPARE_NOHUMAN);
    }

    if (prepareStatus == 0)
    {
        prepareStatueQueue.push(1);
    }
    else
    {
        prepareStatueQueue.push(0);
    }

    // 判断是否充满队列
    if (prepareStatueQueue.real_size() == prepareStatueQueue.size())
    {
        // 符合准备动作次数充满队列 (不检测正反手 或者 检测正反手且不是反手)
        if (prepareStatueQueue.counts_one() == prepareStatueQueue.size() && (!pullup_st.hand_back_flag || (pullup_st.hand_back_flag && !checkIsBackHand())))
        {
            prepareStatus |= (1 << PullUpPrepareCode::PULLUP_PREPARE_SUCCESS);
        }
    }
    
    return StatusCode::SUCCESS;
}

extern "C"{
    int Asj_PE_Pullup::processPrepare(sampleRunJoint_RESULT_S *mResults, int *prepareStatus)
    {
        int ret = StatusCode::SUCCESS;
        try{
            ret = processPrepareCPP(mResults, *prepareStatus);
        } catch (const std::exception& e) {
            printf("[%s Line:%d] Error:%s\n", __FUNCTION__, __LINE__, e.what());
            ret = StatusCode::FAILED;
        }
        
        return ret;
    }
}


int Asj_PE_Pullup::processExamCPP(sampleRunJoint_RESULT_S *mResults, PullUpResult_ST &result)
{
    sampleRunJoint_POINT_S *landmark;
    int personIndex = -1;

    findNearestPeople(mResults, personIndex);
    // 存放手部状态
    if (handBox.size() == 2)
    {
        handStatusQueue.push(1);
    }
    else
    {
        handStatusQueue.push(0);
    }

    // 持续数帧检测不到双手在杠上(双手落杠检测开启的情况下)
    if (pullup_st.hand_off_bar_flag && handStatusQueue.real_size() == handStatusQueue.size()  && handStatusQueue.real_size() == handStatusQueue.counts_zero())
    {
        pullupResult_st.pullUp_HandDropBar += 1;
    }

    // 判断是否有人(离杠最近)出现在画面 且 双手在杠上
    if (personIndex != -1 && handBox.size() == 2)
    // if (personIndex != -1)
    {
        // 当前帧正反手识别
        if (pullup_st.hand_back_flag)
        {
            std::sort(handBox.begin(), handBox.end(), compareByX);
            rightHandStatusQueue.push(int(fbHeadDetect(handBox[0].object.landmark, false)));
            leftHandStatusQueue.push(int(fbHeadDetect(handBox[1].object.landmark, true)));
        }
        // 获取这个离杠最近的人的关键点
        landmark = mResults->pstObjectItems[personIndex].landmark;
        printf("[%s Line:%d] -----------left:%.3f right:%.3f\n", __FUNCTION__, __LINE__, std::abs(landmark[7].y - leftBar.y), std::abs(landmark[8].y - leftBar.y));
        // 判断眼睛是否过杠
        isEyeoverbar = ((POINT_ON_SIDE(leftBar, rightBar, landmark[1]) >= 0) && (POINT_ON_SIDE(leftBar, rightBar, landmark[2]) >= 0));
        if (isEyeoverbar && !eyeStatus)
        {
            // 记录眼睛曾过杠
            eyeStatus = true;
            // 刷新记录眼睛是否过杠的队列
            eyeStatusQueue.reset();
        }

        // 把当前眼睛是否过杠的状态push到队列里
        eyeStatusQueue.push(int(isEyeoverbar));

        // 判断下颜是否过杠,这里的下颜是实际的下颜与双眼中点的中点坐标.
        sampleRunJoint_POINT_S faceCenter = calculateCentroid(landmark[0], calculateCentroid(landmark[1], landmark[2]));
        bool faceOnSide = POINT_ON_SIDE(leftBar, rightBar, faceCenter) >= 0;

        // 前一帧没过杠 且 目前这帧过杠 且 本次下颜没有过杠
        if (!prevFrameStatus && faceOnSide && !isFinishone)
        {
            // 手臂没有伸直
            if (pullup_st.arm_bend_flag && !isStraighten)
            {
                pullupResult_st.pullUp_HandNotStraighten += 1;
                pullupResult_st.pullup_illegal_flag |= (1 << PullUpExamCode::PULLUP_EXAM_HANDNOTSTRAIGHTEN);
            }
            // 反手
            else if (pullup_st.hand_back_flag && checkIsBackHand())
            {
                pullupResult_st.pullUp_BackHand += 1;
                pullupResult_st.pullup_illegal_flag |= (1 << PullUpExamCode::PULLUP_EXAM_BACKHAND);
            }
            // 满足条件
            else 
            {
                gettimeofday(&nowTime, NULL);
                if (pullup_st.exam_interval_sencond && nowTime.tv_sec - preFinishTime.tv_sec > overTime)
                {
                    pullupResult_st.pullup_OverTime += 1;
                    pullupResult_st.pullup_illegal_flag |= (1 << PullUpExamCode::PULLUP_EXAM_OVERTIME);
                }
                pullupResult_st.pullUpCount += 1; // TOOD 超时的时候算不算当前这个
                // 记录时间
                preFinishTime = nowTime;
            }
            // 记录本次是下颜已经过杠
            isFinishone = true;
            // 重置手臂伸直标志位
            isStraighten = false;
        }
        // 眼部曾过杠的情况下(eyeStatus用于刷新眼部状态是否曾高于杠) 且 已经持续一定帧数眼部没有过杠 且 下颜没有过杠
        else if (eyeStatus && eyeStatusQueue.counts_zero() == eyeStatusQueue.size() && !isFinishone)
        {
            pullupResult_st.pullUp_NotOverBar += 1;
            pullupResult_st.pullup_illegal_flag |= (1 << PullUpExamCode::PULLUP_EXAM_NOTOVERBAR);
            eyeStatus = false;
            eyeStatusQueue.reset();
            isStraighten = false;
        }

        // 手臂没伸直
        if (!isStraighten)
        {
            // 满足条件
            if (checkHandIsStraighten(landmark))
            {
                isStraighten = true;
            }
        }
        // 眼部没过杠
        if (!isEyeoverbar)
        {
            if (isFinishone)
            {
                eyeStatus = false;
            }
            isFinishone = false;
        }
        // 记录前一帧的状态
        prevFrameStatus = faceOnSide;
    }
    std::memcpy(&result, &pullupResult_st, sizeof(PullUpResult_ST));
    return StatusCode::SUCCESS;
}

extern "C"{
    int Asj_PE_Pullup::processExam(sampleRunJoint_RESULT_S *mResults, PullUpResult_ST *result)
    {
        int ret = StatusCode::SUCCESS;
        try{
            ret = processExamCPP(mResults, *result);
        } catch (const std::exception& e) {
            printf("[%s Line:%d] Error:%s\n", __FUNCTION__, __LINE__, e.what());
            ret = StatusCode::FAILED;
        }
        
        return ret;
    }
}