#include "human_velocity.h"
#include <iomanip>
#include "math_utils.h"
HumanVelocity::HumanVelocity()
{
    printf("34343333333333");
}
HumanVelocity::HumanVelocity(int bottom_distance, int base_distance)
{
    this->MAX_SIZE = 10;
    for (int i = 0; i <this->MAX_SIZE ; ++i) {
        this->coords_structure.push_back({0,0,0});
        this->move_dist.push_back(0);
    }
    this->RPS = 25;
    this->total_distance = 0.0;
    this->store_pointer = 0;
    this->init_size = 0;
    this->real_world_rate = base_distance/bottom_distance;
    MathUtils mu;
    this->math_utils = mu;

}

void HumanVelocity::add_next_point(int point_x, int point_y, double point_z, double pers_rate)
{
    printf("add_next_point");
	this->coords_structure[this->store_pointer][0] = point_x;
	this->coords_structure[this->store_pointer][1] = point_y;
	this->coords_structure[this->store_pointer][2] = point_z; 
	this->store_pointer = (this->store_pointer + 1)%this->MAX_SIZE;
    this->init_size ++;
    if(this->init_size>1)
    {
//        printf("coords::::", this->coords_structure[(this->store_pointer -2 + this->MAX_SIZE)%this->MAX_SIZE]);
        double pers_distance = this->math_utils.cal_distance(this->coords_structure[(this->store_pointer -2 + this->MAX_SIZE)%this->MAX_SIZE],
                this->coords_structure[(this->store_pointer -1 + this->MAX_SIZE)%this->MAX_SIZE]);
        pers_distance = (pers_distance / pers_rate)*this->real_world_rate*0.1;

        double move_dist = sqrt(pow(pers_distance,2) + pow(
                                                           this->coords_structure[(this->store_pointer - 2 + this->MAX_SIZE) % this->MAX_SIZE][2] -
                                                           this->coords_structure[(this->store_pointer - 1 + this->MAX_SIZE) % this->MAX_SIZE][2],2));
        move_dist *= (this->coords_structure[this->store_pointer][2]/10);  

        this->total_distance += move_dist;
        this->move_dist[this->store_pointer-1] = move_dist;
    }

    this->total_distance -= this->move_dist[this->store_pointer];
}


double HumanVelocity::cal_human_velocity(int rps)
{
    this->RPS = rps;
    double h_velocity = this->total_distance*this->RPS/(this->MAX_SIZE - 2);

    return h_velocity/100;
}