#include <vector>
#include <math.h>
#include <stdexcept>

double median3(double a, double b, double c){
    return (2*a + 2*b + abs(a + b - 2*c - abs(a - b)) - abs( a + b - 2*c + abs(a - b)))/4;
}

double firstIsogonicCenter(double a, double b, double c){
    double fic = pow(a,4) -2*pow((pow(b,2)-pow(c,2)),2)+ 
            pow(a,2)*((pow(b,2)+pow(c,2))+sqrt(3*(-a+b+c)*(a+b-c)*(a-b+c)*(a+b+c)));
}

std::vector<double> calculateFermatPoint(const std::vector<std::pair<double, double>> &pts)
{
    if (pts.size() < 3)
    {
        throw std::invalid_argument("At least 3 points are required to calculate the Fermat point.");
    }

    double a,b,c;
    std::pair<double,double> pa,pb,pc;
    pa = pts[0];
    pb = pts[1];
    pc = pts[2];
    a = sqrt(pow(pb.first - pc.first, 2) + pow(pb.second - pc.second, 2));
    b = sqrt(pow(pa.first - pc.first, 2) + pow(pa.second - pc.second, 2));
    c = sqrt(pow(pa.first - pb.first, 2) + pow(pa.second - pb.second, 2));

    //barycentric coordinates of first isogonic centers
    double f_abc,f_bca,f_cab;
     f_abc= firstIsogonicCenter(a,b,c);
     f_bca= firstIsogonicCenter(b,c,a);
     f_cab= firstIsogonicCenter(c,a,b);

    //barycentric coordinates of the FIC
    double fic1,fic2;
    fic1 = f_abc/(f_abc+f_bca+f_cab);
    fic2 = f_bca/(f_abc+f_bca+f_cab);

    //barycentric coordinates of the FFP
    double ffp1,ffp2,ffp3;
    ffp1 =median3(0.0,fic1,1.0);
    ffp2 =median3(0.0,fic2,1.0);
    ffp3 =1.0-ffp1-ffp2;

    std::vector<double> fermat;

    fermat.push_back( pa.first*ffp1 +pb.first*ffp2 +pc.first*ffp3 );
    fermat.push_back( pa.second*ffp1+pb.second*ffp2+pc.second*ffp3 );

    return fermat;


}