//
// Created by martin on 28. 3. 2021.
//

#include <opencv2/core.hpp>
#include "utility.h"
#include <cmath>


#include "ellipseValidation.h"

double semiAxis(Eigen::Matrix<double, 6, 1> &ellipseEQ, bool major) {
    //ellipseEQ [A,B,C,D,E,F]
    //using the calculation from wikipedia which has equation :
    //A*x^2 + B*xy + C*y^2 + D*x + E*y + F while the equation that i get is in form :
    // a1*x^2 + 2*a2 * xy .. 2*a4 * x + 2*a5*y
    double A, B, C, D, E, F;
    A = ellipseEQ(0);
    B = 2*ellipseEQ(1);
    C = ellipseEQ(2);
    D = 2*ellipseEQ(3);
    E = 2*ellipseEQ(4);
    F = ellipseEQ(5);

    //(1*major) + (-1 * ! major) to represent the +- in the equation


    double left = 2*((A*E*E) + (C * D * D) - (B*D*E) + (B*B - 4*A*C)*F);
    double right = (A+C) + (((1 * major) - (1*!major)) * sqrt((A-C)*(A-C) + B * B));


    double top = -std::sqrt(left * right);

    double bot = B * B - 4 * A * C;

    return top / bot;
}



cv::Point2i centre(Eigen::Matrix<double, 6, 1> &ellipseEQ) {
    double A, B, C, D, E;
    A = ellipseEQ(0);
    B = 2*ellipseEQ(1);
    C = ellipseEQ(2);
    D = 2*ellipseEQ(3);
    E = 2*ellipseEQ(4);

    int x = round((2*C*D-B*E)/(B*B-4*A*C));
    int y = round((2*A*E-B*D)/(B*B-4*A*C));
    return cv::Point2i (x,y);
}



double angle(Eigen::Matrix<double, 6, 1> &ellipseEQ) {
    //ellipseEQ [A,B,C,D,E,F]
    double A, B, C;
    A = ellipseEQ(0);
    B = 2*ellipseEQ(1);
    C = ellipseEQ(2);

    if (B == 0) {
        if (A > C) {
            return M_PI / 2; // if A > C then return 90 degrees
        } else {
            return 0;
        }
    }
    return atan((1/B)*(C - A - sqrt((A - C )* (A - C) + B * B )));
}



bool ShapeIndex (const ellipseParameters &ellipseEQ, const double theta) {
    double major = ellipseEQ.major;
    double minor = ellipseEQ.minor;

    // if either of the constraints below fail then return false
    // did not use return constraint 1 && constraint2 as this way it is easier to read
    if ((major*minor)/ std::sqrt(major * major + minor * minor) <= std::sqrt(2)/(std::sqrt(2)-1) ) {
        return false;
    }

//    theta = M_PI/9;
    if (std::sqrt(major*minor) < (std::sqrt(2)/2) / (1 - std::cos(theta/2)) ) {
        return false;
    }

    return true;
}


bool location_index(cv::Point2i V_i ,const cv::Mat& edges) {
    for (int i = -1; i <=1 ; i++) {
        for (int j = -1; j <= 1; j++) {
            if (V_i.x+i < 0 || V_i.x+i >= edges.size().width || V_i.y+j < 0 || V_i.y+j >= edges.size().height) {
                continue;
            }
            //if the point in the 8 neighborhood is not zero then it is in correct location
            if (edges.at<bool>(cv::Point2i(V_i.x+i,V_i.y+j))) {
                return true;
            }
        }
    }
    return false;
}




// make the calculation later
double gradient_index (double major_axis, double minor_axis , double angle, cv::Point2i ellipse_centre, double d_tol,int index,int N_V) {

    cv::Point2i mid = sampling_point(major_axis,minor_axis,angle,ellipse_centre,index,N_V);
    double e = sqrt(1-pow(minor_axis,2)/pow(major_axis,2));
    double R = d_tol * major_axis * e/ (1+e);

    int i = 1;
    while (ArcVector(mid, sampling_point(major_axis, minor_axis, angle, ellipse_centre, (index + i + N_V - 1) % N_V, N_V)).size() < R) {
        i++;
    }
    if (i >1) {
        i--;
    }

    //possibly the distance between mid and mid+1 > R
    cv::Point2i next = sampling_point(major_axis,minor_axis,angle,ellipse_centre,(index + i + N_V)%N_V,N_V);
    i = 1;
    while (ArcVector(mid, sampling_point(major_axis, minor_axis, angle, ellipse_centre, (index - i + N_V) % N_V, N_V)).size() < R) {
        i++;
    }

    if (i >1) {
        i--;
    }

    cv::Point2i previous = sampling_point(major_axis,minor_axis,angle,ellipse_centre,(index - i + N_V -1)%N_V,N_V);

    ArcVector l_i(previous, next);


    ArcVector g_i;
    double V_i_angle = 2* M_PI *index / N_V;

    //[−wẼ sin θ Vi , hẼ cosθ Vi ]R(θ Ẽ ), -> gi
    double x = sin(V_i_angle) * (-major_axis);
    double y = cos(V_i_angle) * minor_axis;

    //rotate the tangent with the orientation of ellipse
    g_i.x = x * cos(angle) - y * sin(angle);
    g_i.y = x * sin(angle) + y * cos(angle);

    double cosOfVec = l_i.dotProd(g_i)/ (l_i.size() * g_i.size());


    if (cosOfVec > 1) {
        cosOfVec = 1;
    }
    double vec_angle = acos(cosOfVec);

    return 0.5 - 2/M_PI * vec_angle;
}


double weight_index (double major_axis, double minor_axis, double angle) {
    return (major_axis*minor_axis) / major_axis * major_axis * cos(angle) * cos(angle) + minor_axis * minor_axis * sin(angle) * sin(angle);
}


int sampling_points_count(double major_axis, double minor_axis) {
    return (minor_axis+major_axis)*M_PI >= 360? 360 : std::round((minor_axis+major_axis)*M_PI);
}

cv::Point2i sampling_point(const double &major_axis, const double &minor_axis,const double &angle ,cv::Point2i ellipse_centre, int index, int N_v) {
    cv::Point2i V_i (major_axis*cos(2*M_PI*index/N_v),minor_axis*sin(2*M_PI*index/N_v));
    // multiplying a matrix by a vector not vice versa
     int x = (V_i.x * cos(angle) - V_i.y * sin(angle));
     int y = (V_i.x * sin(angle) + V_i.y * cos(angle));

    V_i.x = x;
    V_i.y = y;
    return V_i + ellipse_centre;
}

ellipseParameters ellipse_equation(Eigen::Matrix<double, 6, 1> &ellipseEQ) {

    ellipseParameters params;

    params.major = semiAxis(ellipseEQ, true);
    params.minor = semiAxis(ellipseEQ, false);
    double ellipse_angle = angle(ellipseEQ);
    params.theta = ellipse_angle;
    params.center  = centre(ellipseEQ);
    if (params.minor > params.major) {
        double temp = params.minor;
        params.minor = params.major;
        params.major = temp;
        params.theta += M_PI/2;

    }

    return params;
}


double score(Eigen::Matrix<double, 6, 1> &ellipseEQ,const ellipseParameters & params , cv::Mat & canny,const double theta) {


    if (std::isnan(params.major) || std::isnan(params.minor) || std::isnan(params.theta)) {
        return 0;
    }
    double total_weight = 0;

    double Escore = 0;
    if (!ShapeIndex(params,theta)) {
        return 0;
    }
    int N_v = sampling_points_count(params.major,params.minor);
    for (int i = 0; i < N_v; i++) {


        double angle = 2*M_PI*i/N_v;
        cv::Point2i sampled = sampling_point(params.major,params.minor, params.theta,params.center,i,N_v);
        bool location = location_index(sampled, canny);
        double gradient = gradient_index(params.major,params.minor,params.theta,params.center,0.05,i,N_v);
//        std::cout << gradient << std::endl;
        double weight = weight_index(params.major,params.minor, angle);
        total_weight += weight;
        Escore += (0.5 + location  * gradient) * weight;
    }

    return Escore / total_weight; //  Escore / (N_v * total_weight) most likely it should have not been divided by N_v
}


