#include <unistd.h>
#include <cstdlib>
#include <algorithm>
#include "fall_run_wander.h"

/** 判断跌倒:
    两个特征：1、宽高的比率；2、比率的变化速度（取变化后的和最初设计的小值）3、跌倒后的重心下降 4、跌倒过程小于1.5s
     return 跌倒的概率，以及持续的时间
    */

//void detect_fall(DETECTBOX tmpbox, std::shared_ptr<Track>& track)
//{
//    float rate = tmpbox(2)/tmpbox(3);
//    if(rate < track->rate)
//    {
//        track->rate = rate;
//        track->isFall = false;
//    }
//
//
//    // 更新track->rate_queue以及记录minRate的相关信息
//    if(track->rate_queue.size()>=5)
//    {
//        track->rate_queue.pop();
//
//    }else if(track->rate_queue.isEmpty())
//    {
//        track->minRate = rate;
//        gettimeofday(&track->min_rate_time,0);
//
//        track->center_point.x = float(tmpbox(0)+tmpbox(2)/2);
//        track->center_point.y = float(tmpbox(1)+tmpbox(3)/2);
//        track->human_h = tmpbox(3);
//    }
//
//    if(track->rate_queue.size()>0)
//    {
//        if(track->rate_queue.min()>=rate)
//        {
//            track->minRate = rate;
//            gettimeofday(&track->min_rate_time,0);
//            track->center_point.x = float(tmpbox(0)+tmpbox(2)/2);
//            track->center_point.y = float(tmpbox(1)+tmpbox(3)/2);
//            if(rate>0.25)
//            {
//                track->human_h = tmpbox(3);
//                track->human_w = tmpbox(2);
//            }
//        }
//    }
//
//    if(track->hisMinRate>=rate)
//    {
//        track->hisMinRate=rate;
//        track->min_center_point.x = float(tmpbox(0)+tmpbox(2)/2);
//        track->min_center_point.y = float(tmpbox(1)+tmpbox(3)/2);
//    }
//
//    track->rate_queue.push(rate);
//
//    // 判断是否跌倒
////    float fall_threshold = 1.3; //默认调参1.3(能和蹲下区分的参数)
//    gettimeofday(&track->fall_e_time,0); // 当前比例时间
//    track->time_substract(&track->fall_diff_time,&track->min_rate_time,&track->fall_e_time); //距离最小比例的时间
//    int fall_rate_time = (int)(track->fall_diff_time.tv_sec*1000 + track->fall_diff_time.tv_usec/1000);
//    float fall_threshold = m_staticStruct::fall_rate;
//
//    if((rate > std::min(fall_threshold, 5*track->rate)||track->human_h>=tmpbox(3)*2*track->human_w/tmpbox(2)) && fall_rate_time<2000 && std::min(track->min_center_point.y, track->center_point.y)<float(tmpbox(1)+tmpbox(3)/2))
//    {
//        track->isFall = true;
////                std::cout << fall_threshold <<"-- fall_threshold--:" << rate << ":" << 5*track->rate <<std::endl;
//    }
//    else if(rate < 0.9)
//    {
//        track->isFall = false;
//        track->fall_total_time = 0 ;
//        track->first_Fall = true;
//    }
//
//    // 计算跌倒置信度
//    if(track->isFall)
//    {
//        /**-- start计算累积跌倒时间*/
//        if(track->first_Fall) //是否开始计算跌倒时间
//        {
//            track->first_Fall = false;
//            gettimeofday(&track->fall_s_time,0);
//        }
//        gettimeofday(&track->fall_e_time,0);
//        track->time_substract(&track->fall_diff_time,&track->fall_s_time,&track->fall_e_time); //距离最小比例的时间
//        track->fall_total_time = (int)(track->fall_diff_time.tv_sec + track->fall_diff_time.tv_usec/1000000);
//
//        /** -- end计算累积跌倒时间*/
//
//        track->rateScale = int(std::min(float((rate - 0.6)*72), 100.0f));
//
//        if(track->rateScale<0)
//        {
//            track->rateScale = 0-track->rateScale;
//        }
//    }else
//    {
//        track->rateScale = int(std::max(float(10 - (fall_threshold - rate)*10), 0.0f));
//    }
//
//    //    std::cout << track->fall_total_time << "::" << fall_rate_time << ":" <<track->min_center_point.y <<"-"<< track->center_point.y << ":"<<float(tmpbox(1)+tmpbox(3)/2) <<"-- fall--:" << rate << ":" << track->isFall <<std::endl;
//    //    std::cout << track->human_h << ":::f:::" << tmpbox(3)*4*track->human_w/tmpbox(2) <<std::endl;
//}

/**
    算法思想: 判断点的人体脚点的移动移动速度，移动的速度为单帧的移动距离比上单帧画面所花费的时间
    做了一个10帧的队列，对队列中的速度进行平滑处理

    改进点：根据不同的画面位置 进行移动距离的放缩
           横纵画面距离的权重平衡（横纵的变化速度不一样）
*/
float detect_runing(DETECTBOX tmpbox, std::shared_ptr<Track>& track, cv::Mat img)
{
    float human_pointX =  tmpbox(0) + tmpbox(2)/2;
    float human_pointY = tmpbox(1);
    double rate = tmpbox(2)/tmpbox(3);
    double curr_area = tmpbox(2)*tmpbox(3);

    float move_velocity = 0;
    float ave_velocity = 0;
    if(track->is_runing_p)
    {
        track->last_point.x = human_pointX;
        track->last_point.y = human_pointY;
        track->is_runing_p = false;
    }else
    {
        SPoint curr_point(human_pointX, human_pointY);
        float move_distance = calDistance(curr_point, track->last_point);
        if(tmpbox(1)<0 ||tmpbox(1)+tmpbox(3)>=img.rows)
        {
            ave_velocity=0;
            queue<float> empty;
            swap(empty,track->velocity_queue);
            track->sum_velocity=0;
            track->run_rate = rate;
        }
        move_velocity = move_distance/track->single_time;

//        std::cout << curr_point.x << ":" << curr_point.y <<"-------------------" <<track->last_point.x<< "::" << track->last_point.y <<std::endl;
//        std::cout << move_distance <<"-------- move_velocity:" << track->single_time << "::" << move_velocity <<std::endl;

        track->last_point.x = human_pointX;
        track->last_point.y = human_pointY;

        if(track->velocity_queue.size()==10)
        {
            float last_velocity = track->velocity_queue.front();
            track->sum_velocity -= last_velocity;
            track->velocity_queue.pop();
//                    std::cout << "-------- last_velocity:" << last_velocity <<std::endl;
        }
        track->velocity_queue.push(move_velocity);
        track->sum_velocity += move_velocity;
    }

    if (track->velocity_queue.size()>0)
    {
        ave_velocity = track->sum_velocity/track->velocity_queue.size();
    }else
    {
        ave_velocity = 0;
    }

    double diff_rate = fabs(rate - track->run_rate);
    double min_area = curr_area>track->rate_area?track->rate_area:curr_area;
    double max_area = curr_area>track->rate_area?curr_area:track->rate_area;
    double rate_area = fabs(min_area/max_area); //面积比例变化
    double last_diff_rate = fabs(rate - track->last_rate);
//    std::cout <<rate_area<<"::::"<< rate << " ---------------------------------rate:"<<track->run_rate <<"===== diff_rate:" << diff_rate  <<":::" << last_diff_rate <<std::endl;
    if(diff_rate>0.2 || last_diff_rate>0.2 || tmpbox(1)<0 || rate_area<0.8 ||tmpbox(1)+tmpbox(3)>=img.rows)
    {
        ave_velocity=0;
        queue<float> empty;
        swap(empty,track->velocity_queue);
        track->sum_velocity=0;
        track->run_rate = rate;
    }
    track->last_rate = rate;
    track->rate_area = curr_area;

//    std::cout << move_velocity << " :"<<track->sum_velocity <<"===== sum_velocity:" << track->velocity_queue.size() << "::" << ave_velocity <<std::endl;

//    std::cout << ave_velocity << "===== m_staticStruct::mv_velocity:" << std::endl;


    return ave_velocity;
}