#include "BAT/BCParameter.h"
#include "BatAlignmentModel.h"
#include "TpcAlignment.h"

BatAlignmentModel::BatAlignmentModel(AbsChi2* aligner)
{
    faligner=aligner;

    AddParameter("dx",-2.0,2.0,"dx (cm)");
    AddParameter("dy",-2.0,2.0,"dy (cm)");
    AddParameter("dphi",-4.0,4.0,"d#phi (degree)");
    AddParameter("dtheta",-4.0,4.0,"d#theta (degree)");
    AddParameter("dpsi",-4.0,4.0,"d#psi (degree)");
    AddParameter("ddrift",0.7,1.3,"ddrift scale");
    AddParameter("dz",-5,5,"dz (cm)");
    GetParameter("dx").SetNbins(500);
    GetParameter("dy").SetNbins(500);
    GetParameter("dphi").SetNbins(500);
    GetParameter("dtheta").SetNbins(500);
    GetParameter("dpsi").SetNbins(500);
    GetParameter("dz").SetNbins(500);
    SetPriorConstantAll();
    SetPriorDelta("ddrift",1);
    // SetPriorDelta("x",0);
    //SetPriorDelta("y",0);
    //    SetPriorDelta("dz",0);

    
    //testing observables:
    faligner->getRealParameters(std::vector<double>(7,0),realParameters);
    
    AddObservable("x", -1.5, 1.5,  "x (cm)");
    AddObservable("y", -1.5,1.5,  "y (cm)");
    AddObservable("phi", -5., 5.,  "phi (deg)");
    AddObservable("theta", -5., 5.,  "theta (deg)");
    AddObservable("psi", -5., 5.,  "psi (deg)");
    //AddObservable("drift", -3, 3,  "drift ()");
    AddObservable("z", -45, -40,  "z (cm)");
    
    
    GetObservable("x").SetNbins(500);
    GetObservable("y").SetNbins(500);
    GetObservable("phi").SetNbins(500);
    GetObservable("theta").SetNbins(500);
    GetObservable("psi").SetNbins(500);
    GetObservable("z").SetNbins(1000);
}
void BatAlignmentModel::CalculateObservables(const std::vector<double> &pars){
    faligner->getRealParameters(pars,realParameters);
    GetObservable(0).Value(realParameters[0]);
    GetObservable(1).Value(realParameters[1]);
    GetObservable(2).Value(realParameters[2]);
    GetObservable(3).Value(realParameters[3]);
    GetObservable(4).Value(realParameters[4]);
    //GetObservable(5).Value(realParameters[5]);
    GetObservable(5).Value(realParameters[6]);
}

double BatAlignmentModel::LogLikelihood(const std::vector<double> &pars){

    std::vector<double> test2=pars;
    double test =faligner->chisquare(pars.size(),test2.data());
    if(test>0){
        return -(test/2);
    }else{
        std::numeric_limits<double> a;
        return a.max();
    }
}