#include <cmath>

#include "ILineFit.hxx"

COMET::ILineFit::ILineFit(void) 
    : fPoints(0), fX(0), fA(0), fB(0), fC(0), fD(0), fE(0) { }

COMET::ILineFit::ILineFit(int n, double x[], double y[], double sigma[]) 
    : fPoints(0), fX(0), fA(0), fB(0), fC(0), fD(0), fE(0) {
    for (int i=0; i<n; ++i) {
        AddPoint(x[i],y[i],sigma[i]);
    }
}
    
void COMET::ILineFit::AddPoint(double x, double y, double sigma) {
    if (sigma<=0) return;
    ++fPoints;
    const double sigma2 = sigma*sigma;
    fX += x;
    fA += y/sigma2;
    fB += 1.0/sigma2; 
    fC += x/sigma2;
    fD += x*y/sigma2;
    fE += x*x/sigma2;
}

double COMET::ILineFit::GetCentralIndependent(void) const {
    if (fPoints<1) return 0.0;
    return fX/fPoints;
}

bool COMET::ILineFit::IsValid(void) const {
    if (fPoints<2) return false;
    double numerator = fA*fC - fB*fD;
    double denominator = fC*fC - fB*fE;
    if (std::abs(denominator) <= (1E-6)*std::abs(numerator)) return false;
    return true;
}

void COMET::ILineFit::RemovePoint(double x, double y, double sigma) {
    if (sigma<=0) return;
    if (fPoints<=1) {
        fPoints = 0;
        fX = fA = fB = fC = fD = fE = 0.0;
        return;
    }
    --fPoints;
    const double sigma2 = sigma*sigma;
    fX -= x;
    fA -= y/sigma2;
    fB -= 1.0/sigma2; 
    fC -= x/sigma2;
    fD -= x*y/sigma2;
    fE -= x*x/sigma2;
}

double COMET::ILineFit::GetSlope(void) const {
    if (fPoints<2) return 0.0;
    double numerator = fA*fC - fB*fD;
    double denominator = fC*fC - fB*fE;
    if (std::abs(denominator) <= (1E-6)*std::abs(numerator)) return 0.0;
    return numerator/denominator;
}

double COMET::ILineFit::GetSlopeUncertainty(void) const {
    if (fPoints<2) return 0.0;
    double numerator = -fB;
    double denominator = fC*fC - fB*fE;
    if (std::abs(denominator) <= (1E-6)*std::abs(numerator)) return 0.0;
    return sqrt(numerator/denominator);
}

double COMET::ILineFit::GetIntercept(void) const {
    if (fPoints<1) return 0;
    return (fA - GetSlope()*fC)/fB;
}

double COMET::ILineFit::GetInterceptUncertainty(void) const {
    if (fPoints<2) return 0.0;
    double numerator = -fA;
    double denominator = fC*fC - fB*fE;
    if (std::abs(denominator) <= (1E-6)*std::abs(numerator)) return 0.0;
    return sqrt(numerator/denominator);
}

double COMET::ILineFit::GetValue(double y) const {
    return GetIntercept() + y*GetSlope();
}