//
// Created by ivan on 17.01.2022.
//

#include "processing_functions.h"

vector<float> operator + (vector<float>& lhs, vector<float>& rhs){
    for (int i=0; i < lhs.size(); i++){
        lhs[i] += rhs[i];
    }

    return lhs;
}

vector<float> operator + (vector<float>& lhs, float rhs){
    for (float & lh : lhs){
        lh += rhs;
    }
    return lhs;
}

vector<float> operator - (vector<float>& lhs, float rhs){
    for (float & lh : lhs){
        lh -= rhs;
    }
    return lhs;
}

vector<float> operator - (vector<float>& arg){
    for (auto& el : arg){
        el = -el;
    }
    return arg;
}

vector<float> operator - (vector<float>& lhs, vector<float>& rhs){
    for (int i=0; i<lhs.size(); i++){
        lhs[i] -= rhs[i];
    }
    return lhs;
}

vector<float> operator * (vector<float>& lhs, float rhs){
    for (float & lh : lhs){
        lh *= rhs;
    }
    return lhs;
}

bool operator == (vector<float>& lhs, float rhs){
    bool flag = true;
    for (auto& lh : lhs){
        if (lh != rhs){
            flag = false;
        }
    }
    return flag;
}

void calc_avg_velocity(const vector<vector<float>>& recent_velocities, vector<float>& avg_velocity){
    float avgX = 0, avgY = 0, avgZ = 0;
    for (const auto & recent_velocitie : recent_velocities){
        avgX += recent_velocitie[0];
        avgY += recent_velocitie[1];
        avgZ += recent_velocitie[2];
    }

    auto mysize = recent_velocities.size();
    cout << "mysize is: " << mysize;
    avgX /= (float)mysize;
    avgY /= (float)mysize;
    avgZ /= (float)mysize;

    avg_velocity[0] = avgX;
    avg_velocity[1] = avgY;
    avg_velocity[2] = avgZ;

    cout << "calc_avg_velocity success" << endl;
}

// avg_velocity is a vector, since we need the average speed for axes XYZ.
void calculate_velocity(const vector<vector<float>>& recent_values, vector<float>& avg_velocity){

    vector<vector<float>> recent_velocities;

    cout << "recent_values size: " << recent_values.size() << " recent_velocities size: " << recent_velocities.size() << endl;

    for (int i=(int)recent_values.size()-1; i>0; i--){
        vector<float> lhs = recent_values[i]; vector<float> rhs = recent_values[i-1];
        vector<float> diff = lhs - rhs;
        recent_velocities.push_back(diff);
    }
    cout << "for loop success" << endl;
    cout << "recent_values size: " << recent_values.size() << " recent_velocities size: " << recent_velocities.size() << endl;

//    cout << "recent_velocities are: " << recent_velocities[0][0] << recent_velocities[1][0] << recent_velocities[2][0];
    calc_avg_velocity(recent_velocities, avg_velocity);

    cout << "calculate_velocity success" << endl;
}

void process_lost_euler(vector<float>& euler_prev, vector<float>& euler_now, int recent_values_desired_length,
                        const vector<vector<float>>& recent_values, vector<float>& avg_velocity){

    // Если каждый элемент вектора равен нулю.
    bool empty_velocity = avg_velocity == 0.0;
    float epsilon = 0.2;

    cout << "compar_result success" << endl;
    if (recent_values.size() == recent_values_desired_length and empty_velocity){
        cout << "Entered first if" << endl;
        calculate_velocity(recent_values, avg_velocity);
        cout << "calculate_velocity success" << endl;
    }

    if (!empty_velocity){
        cout << "entered second if" << endl;
        if ( abs((recent_values[recent_values.size() - 1][1] - recent_values[recent_values.size() - 2][1])) < epsilon ){
            euler_now = euler_prev;
        }
        else{
            euler_now = euler_prev + avg_velocity;
        }
        cout << "vector summation success" << endl;
    }
    euler_prev = euler_now;

    cout << "process_lost_euler success" << endl;
}

void fill_recent_values(const vector<float>& value, vector<vector<float>>& recent_values, int length){
    if (recent_values.size() != length) {
        recent_values.push_back(value);
    }
    else{
        recent_values.erase(recent_values.begin());
        recent_values.push_back(value);
    }

    cout << "fill_recent_values success" << endl;
}

void process_euler(std::vector<float>& euler_prev, std::vector<float>& euler_now, std::vector<float>& skew_angle,
                   std::vector<int>& all_maps_id, int& prevID, int& currID, vector<float>& avg_velocity){

    if (currID != prevID){
        if ( std::find(all_maps_id.begin(), all_maps_id.end(), currID) == all_maps_id.end() ){
            skew_angle = euler_prev;
            euler_now = euler_now + skew_angle;
            all_maps_id.push_back(currID);
        }
        else{
            euler_now = euler_now + skew_angle;
        }
    }
    else{
        euler_now = euler_now + skew_angle;
    }

    euler_prev = euler_now;
    prevID = currID;

    bool compar_result = avg_velocity == 0.0;
    if (!compar_result){
        avg_velocity[0] = 0.0;
        avg_velocity[1] = 0.0;
        avg_velocity[2] = 0.0;
    }

    cout << "process_euler success" << endl;
}

// For Monocular and Monocular-Inertial processing. 10.02.2022. Podmogilnyi Ivan
cv::Mat& extract_rot(cv::Mat& rot, const cv::Mat& trans) {
    // cv::Mat rot(3, 3, CV_32F, 0.0);
    for (int row = 0; row < 3; ++row) {
        for (int col = 0; col < 3; ++col) {
            rot.at<float>(row, col) = trans.at<float>(row, col);
        }
    }
    return rot;
}

vector<float>& extract_translation(vector<float>& transl, const cv::Mat& tcw) {
    for (int row=0; row<3; ++row){
        transl[row] = tcw.at<float>(row, 3);
    }
    return transl;
}

bool isMatEmpty(const cv::Mat& mat){
    for (int i=0; i<mat.rows; i++){
        for (int j=0; j<mat.cols; j++){
            if ( mat.at<float>(i, j) != 0.0f ){
                return false;
            }
        }
    }
    return true;
}

bool isMatEye(const cv::Mat& mat){
    cv::Mat eye = cv::Mat::eye(mat.rows, mat.cols, CV_32F);
    for (int i=0; i<mat.rows; i++){
        for (int j=0; j<mat.cols; j++){
            if ( mat.at<float>(i, j) != eye.at<float>(i, j) ){
                return false;
            }
        }
    }
    return true;

}