#include "bin.h"
#include "run.h"
#include <string>
#include <iostream>
#include <math.h>
#include <TMatrixD.h>
#include <TVectorD.h>

using std::string;
using namespace std;

Bin::Bin() {

  no_of_events = 0;
  A_Ac.ResizeTo(6,6);
  b_Ac.ResizeTo(6,1);
  A_noAc.ResizeTo(6,6);
  b_noAc.ResizeTo(6,1);

  sum_sin = 0;
  sum_sin2 = 0;
  sum_cos = 0;
  sum_cos2 = 0;

  is_calc = false;
}

Bin::~Bin() {
  A_Ac.Clear();
  b_Ac.Clear();
  A_noAc.Clear();
  b_noAc.Clear();
}

void Bin::add(double val_x, Run *run) {
  no_of_events++;

  Double_t h = run->get_helicity();
  if (h > 0) no_of_events_plus++;
  if (h < 0) no_of_events_minus++;

  Double_t phi = run->get_phi_fpp() * DEGTORAD;
  Double_t sin_phi = sin(phi);
  Double_t cos_phi = cos(phi);

  sum_sin += sin_phi;
  sum_sin2 += sin_phi * sin_phi;
  sum_cos += cos_phi;
  sum_cos2 += cos_phi * cos_phi;

  x.add(val_x);
  Ac_calc.add(run->Ac);
  kin_Ebeam.add(run->kin_Ebeam);
  kin_Ee.add(run->kin_Ee);
  kin_theta_e.add(run->kin_theta_e);
  kin_tau.add(run->kin_tau);
  kin_epsilon.add(run->kin_epsilon);
  px_prime.add(run->px_prime);
  pz_prime.add(run->pz_prime);
  for (int i=0; i<6; i++) {
    for (int j=0; j<6; j++) {
      A_Ac(i,j) += run->Ac * run->Ac * run->lambda[i] * run->lambda[j];
      A_noAc(i,j) += run->lambda[i] * run->lambda[j];
    }
    b_Ac(i,0) += run->Ac * run->lambda[i];
    b_noAc(i,0) += run->lambda[i];
  }

  is_calc = false;
}

// -------------------------------------------------------------------------

void  Bin::add(Run *run) {
  add(0., run);
}

// -------------------------------------------------------------------------

void  Bin::do_calc() {
    if (is_calc) return;
    is_calc = true;

    Px = 0; dPx = 0;
    Py = 0; dPy = 0;
    Pz = 0; dPz = 0;
    R = 0; dR = 0;
    Ac = 0; dAc = 0;
    a1 = 0; da1 = 0;
    b1 = 0; db1 = 0;

    if (no_of_events <= 1) return;

    Double_t e0 = kin_Ebeam.get_mean();
    Double_t ef = kin_Ee.get_mean();
    Double_t theta_e = kin_theta_e.get_mean();
    Double_t e = kin_epsilon.get_mean();
    Double_t t = kin_tau.get_mean();
    Double_t kin_factor = -mu_p * (e0 + ef) / (2*m_p) * tan(theta_e/2);

    // do extraction of polarization observables using the beam-helicity
    // and analyzing-power weighted sums (A_Ac and b_Ac)

    a1 = sum_sin / sum_sin2;
    b1 = sum_cos / sum_cos2;
    da1 = sqrt(1.0/sum_sin2);
    db1 = sqrt(1.0/sum_cos2);

    TMatrixD A0(3,3);
    TMatrixD b0(3,1);

    A0(0,0) = A_Ac(3,3); A0(0,1) = A_Ac(3,1); A0(0,2) = A_Ac(3,5);
    A0(1,0) = A_Ac(1,3); A0(1,1) = A_Ac(1,1); A0(1,2) = A_Ac(1,5);
    A0(2,0) = A_Ac(5,3); A0(2,1) = A_Ac(5,1); A0(2,2) = A_Ac(5,5);

    b0(0,0) = b_Ac(3,0);
    b0(1,0) = b_Ac(1,0);
    b0(2,0) = b_Ac(5,0);

//    cout << A_noAc(3,3) << " " << A_noAc(3,1) << " " << A_noAc(3,5) << endl;
//    cout << A_noAc(1,3) << " " << A_noAc(1,1) << " " << A_noAc(1,5) << endl;
//    cout << A_noAc(5,3) << " " << A_noAc(5,1) << " " << A_noAc(5,5) << endl;

//    cout << b_noAc(3,0) << endl;
//    cout << b_noAc(1,0) << endl;
//    cout << b_noAc(5,0) << endl;

//    A0(0,0) = A_noAc(3,3); A0(0,1) = A_noAc(3,1); A0(0,2) = A_noAc(3,5);
//    A0(1,0) = A_noAc(1,3); A0(1,1) = A_noAc(1,1); A0(1,2) = A_noAc(1,5);
//    A0(2,0) = A_noAc(5,3); A0(2,1) = A_noAc(5,1); A0(2,2) = A_noAc(5,5);

//    b0(0,0) = b_noAc(3,0);
//    b0(1,0) = b_noAc(1,0);
//    b0(2,0) = b_noAc(5,0);

//    A0(0,0) = A_Ac(0,0); A0(0,1) = A_Ac(0,4); A0(0,2) = A_Ac(0,5);
//    A0(1,0) = A_Ac(4,0); A0(1,1) = A_Ac(4,4); A0(1,2) = A_Ac(4,5);
//    A0(2,0) = A_Ac(5,0); A0(2,1) = A_Ac(5,4); A0(2,2) = A_Ac(5,5);

//    b0(0,0) = b_Ac(0,0);
//    b0(1,0) = b_Ac(4,0);
//    b0(2,0) = b_Ac(5,0);

//    A0(0,0) = A_noAc(0,0); A0(0,1) = A_noAc(0,4); A0(0,2) = A_noAc(0,5);
//    A0(1,0) = A_noAc(4,0); A0(1,1) = A_noAc(4,4); A0(1,2) = A_noAc(4,5);
//    A0(2,0) = A_noAc(5,0); A0(2,1) = A_noAc(5,4); A0(2,2) = A_noAc(5,5);

//    b0(0,0) = b_noAc(0,0);
//    b0(1,0) = b_noAc(4,0);
//    b0(2,0) = b_noAc(5,0);

    // Phil: why not allow the determinant to be less than zero? is it ever non-positive?
    // I'm guessing that we don't need to check for this, and just give an error and exit
    // the program if the determinant happens to be zero.
    if (A0.Determinant() > 0.) {
//        TMatrix c0 = A0.Invert() * b0;
        TMatrixD c0 = A0.Invert() * b0;
  
        Px = c0(0,0); dPx = sqrt(A0(0,0));
        Py = c0(1,0); dPy = sqrt(A0(1,1));
        Pz = c0(2,0); dPz = sqrt(A0(2,2));
  
        R =  Px/Pz * kin_factor;
        dR = fabs(R) * sqrt(pow(dPx/Px, 2) + pow(dPz/Pz, 2));
    }

    // Do extraction of <P'x*Ac> and <P'z*Ac> for estimation of
    // the analyzing power Ac. The extraction is based on
    // sums (A and b) with an assumed analyzing power of 1.

    A0(0,0) = A_noAc(3,3); A0(0,1) = A_noAc(3,1); A0(0,2) = A_noAc(3,5);
    A0(1,0) = A_noAc(1,3); A0(1,1) = A_noAc(1,1); A0(1,2) = A_noAc(1,5);
    A0(2,0) = A_noAc(5,3); A0(2,1) = A_noAc(5,1); A0(2,2) = A_noAc(5,5);

    b0(0,0) = b_noAc(3,0);
    b0(1,0) = b_noAc(1,0);
    b0(2,0) = b_noAc(5,0);

//    A0(0,0) = A_noAc(0,0); A0(0,1) = A_noAc(0,4); A0(0,2) = A_noAc(0,5);
//    A0(1,0) = A_noAc(4,0); A0(1,1) = A_noAc(4,4); A0(1,2) = A_noAc(4,5);
//    A0(2,0) = A_noAc(5,0); A0(2,1) = A_noAc(5,4); A0(2,2) = A_noAc(5,5);
//
//    b0(0,0) = b_noAc(0,0);
//    b0(1,0) = b_noAc(4,0);
//    b0(2,0) = b_noAc(5,0);
    
    if (A0.Determinant() > 0.) {
        TMatrix c0 = A0.Invert() * b0;

        PxAc = c0(0,0); dPxAc = sqrt(A0(0,0));
        PzAc = c0(2,0); dPzAc = sqrt(A0(2,2));

        Double_t r = R / mu_p;
        Double_t a = -2 * sqrt(t*(1+t)) * tan(theta_e/2.);
        Double_t b = t/e;
        Double_t c = (e0 + ef)/m_p * sqrt(t*(1+t)) * tan(theta_e/2)*tan(theta_e/2);

//       Double_t Px_r = -2 * sqrt(t*(1+t)) * r * tan(theta_e/2.) / (r*r + t/e);
//       Double_t Pz_r = (e0 + ef)/m_p * sqrt(t*(1+t)) * tan(theta_e/2)*tan(theta_e/2)/(r*r + t/e);
//       cout << "Px_r, Pz_r = " << Px_r << " , " << Pz_r << endl;

        Ac = r*PxAc/a + b*PzAc/c;
        dAc = sqrt(pow(2*PxAc*c/a/a/PzAc,2)*pow(dPxAc,2) +
            pow((-c*PxAc*PxAc/a/a/PzAc/PzAc + b/c),2) * pow(dPzAc,2));
    }
    R_calc = px_prime.get_mean() / pz_prime.get_mean() * kin_factor;
}

// -------------------------------------------------------------------------

int Bin::get_n()  {return no_of_events;}
int Bin::get_np()  {return no_of_events_plus;}
int Bin::get_nm()  {return no_of_events_minus;}
double Bin::get_x()  {return x.get_mean();}
double Bin::get_Px()  {do_calc(); return Px;}
double Bin::get_dPx() {do_calc(); return dPx;}
double Bin::get_Py()  {do_calc(); return Py;}
double Bin::get_dPy() {do_calc(); return dPy;}
double Bin::get_Pz()  {do_calc(); return Pz;}
double Bin::get_dPz() {do_calc(); return dPz;}
double Bin::get_R()  {do_calc(); return R;}
double Bin::get_dR() {do_calc(); return dR;}

double Bin::get_Ac()  {do_calc(); return Ac;}
double Bin::get_dAc() {do_calc(); return dAc;}
double Bin::get_Ac_calc()  {do_calc(); return Ac_calc.get_mean();}

double Bin::get_Ac(double R0, double dR0) {
  if (no_of_events == 0) return 0.;

  do_calc();
  Double_t e0 = kin_Ebeam.get_mean();
  Double_t ef = kin_Ee.get_mean();
  Double_t theta_e = kin_theta_e.get_mean();
  Double_t e = kin_epsilon.get_mean();
  Double_t t = kin_tau.get_mean();
  Double_t r = R0 / mu_p;
  Double_t a = -2 * sqrt(t*(1+t)) * tan(theta_e/2.);
  Double_t b = t/e;
  Double_t c = (e0 + ef)/m_p * sqrt(t*(1+t)) * tan(theta_e/2)*tan(theta_e/2);

  return r*PxAc/a + b*PzAc/c;
}

double Bin::get_dAc(double R0, double dR0)  {
  if (no_of_events == 0) return 0.;

  do_calc();
  Double_t e0 = kin_Ebeam.get_mean();
  Double_t ef = kin_Ee.get_mean();
  Double_t theta_e = kin_theta_e.get_mean();
  Double_t e = kin_epsilon.get_mean();
  Double_t t = kin_tau.get_mean();
  Double_t r = R0 / mu_p;
  Double_t dr = dR0 / mu_p;
  Double_t a = -2 * sqrt(t*(1+t)) * tan(theta_e/2.);
  Double_t b = t/e;
  Double_t c = (e0 + ef)/m_p * sqrt(t*(1+t)) * tan(theta_e/2)*tan(theta_e/2);

  return sqrt(
	      pow(PxAc/a,2) * pow(dr,2) +
	      pow(r/a,2) * pow(dPxAc,2) +
	      pow(b/c,2) * pow(dPzAc,2)
	      );
}

double Bin::get_px_prime_calc() {do_calc(); return px_prime.get_mean();}
double Bin::get_pz_prime_calc() {do_calc(); return pz_prime.get_mean();}
double Bin::get_r_calc() {do_calc(); return R_calc;}
double Bin::get_a1() {do_calc(); return a1;}
double Bin::get_da1() {do_calc(); return da1;}
double Bin::get_b1() {do_calc(); return b1;}
double Bin::get_db1() {do_calc(); return db1;}
/*
void  Bin::out() {

  cout << "Number of events = " << no_of_events << endl;
  cout << "       Px = " << get_Px() << " +/- " << get_dPx() << endl;
  cout << "       Py = " << get_Py() << " +/- " << get_dPy() << endl;
  cout << "       Pz = " << get_Pz() << " +/- " << get_dPz() << endl;
  cout << "       R  = " << get_R()  << " +/- " << get_dR()  << endl;
  cout << "Analyzing power:" << endl;
  cout << "    model: Ac = " << get_Ac_calc() << endl;
  cout << "     data: Ac = " << get_Ac()  << " +/- " << get_dAc()  << endl;
}
*/