#ifndef KINEMATICS_H
#define KINEMATICS_H
#include <TMath.h>
#include <iostream>
#include <fstream>
#include <string>
#include <algorithm>
#include <cmath>
#include <vector>
#include <TVector3.h>
#include "EnergyLoss.h"
const double u_MeV = 931.49410372; //u in MeV

class Kinematics {
public:
	/*
		A(d,p)B is used as template, with A being beam, and p being ejectile.
		Always, make m3 the thing you detect, and m1 the beam
	*/
    double m_A, m_d, m_p, m_B;
    double E_beam;

/**
 * @{ \name List of funny impossible default values
 */
/**
 * \brief Default values used for all the physics values
 */
    double Q0=-9999, Qx=-9999;
    double P4=-9999, E4=-9999, T4=-9999; //heavy recoil momentum, totE, KE
    double P3=-9999, E3=-9999, T3=-9999; //light recoil
    double ET=-9999;
    double gamma4=-9999, beta4=-9999, theta4=-9999; //theta=heavy-recoil lab angle
    double brho=-9999;
/**
 * @}
 */
    Kinematics(double m1, double m2, double m3, double m4, double ebeam) {
	/*
		A(d,p)B is used as template, with A being beam, and p being ejectile.
		Always, make m3 the thing you detect, m2 the target, and m1 the beam
		
		ebeam is in MeV/u, all others are in amu
	*/
        m_A = m1;
        m_d = m2;
        m_p = m3, m_B = m4, E_beam = ebeam*m_A;
        Q0 = (- m_B - m_p + m_d + m_A)*u_MeV;
    }
    Kinematics() {}

    double getbeamCM_KE() { 
		return E_beam*(m_d/(m_A+m_d));
	}

	double getEbeam_givenQ(double e3, double Ex, double angle3_deg) { // e3 is detected particle energy, Ex is excitation energy, angle3_deg is detected particle angle in degrees
		double angle = angle3_deg*TMath::Pi()/180.0;
        double m1 = m_A * u_MeV;
        //double m2 = m_d * u_MeV;
        double m3 = m_p * u_MeV;
        double m4 = m_B * u_MeV; // Total dynamic mass of particle 4
        double Q = Q0-Ex;

		double AA = m1-m4;
		double BB = TMath::Sqrt(m1*m3*e3)*TMath::Cos(angle);
		double CC = (m3+m4)*e3 - m4*Q;
		
		double result_sq1 = (BB - TMath::Sqrt(BB*BB-AA*CC))/(AA);
		return result_sq1*result_sq1;		
	} // to use in anasenMS.cpp, call like: apkin_27Al.getEbeam_givenQ(Esx3, 0.0, thetab);

    void setValues(double m1, double m2, double m3, double m4, double ebeam) {
	/*
		Can be used to 'live update' say the beam energy in the case of active target detectors.
	*/

        m_A = m1;
        m_d = m2;
        m_p = m3, m_B = m4, E_beam = ebeam*m_A;
        Q0 = (- m_B - m_p + m_d + m_A)*u_MeV;
        //std::cout << "Q0 MeV: " << Q0 << std::endl;
    }

    void setEBeam(double Ebeam) {E_beam = Ebeam;}

    double getBeta4(double t3, double angle3);
    double getTheta4(double t3, double angle3);
    double getBrho(double t3, double angle3, double charge_state);
    double getExc(double t3, double angle3); //t3 is proton energy detected in ORRUBA, angle3 is proton angle in degrees

    double getBeta4_fromvec(double t3, const TVector3 &pos, const TVector3 &origin) {
        TVector3 local = pos - origin; //position w.r.t origin
        float angle = local.Theta()*180./M_PI;
        return getBeta4(t3, angle);
    }

    double getExc_fromvec(double t3, const TVector3 &pos, const TVector3 &origin) {
        TVector3 local = pos - origin; //position w.r.t origin
        float angle = local.Theta()*180./M_PI;
        return getExc(t3, angle);
    }

    void setValuesFromFile(const std::string& filename) {
    	(void) filename;
        /*std::ifstream in;
        in.open(filename);
        if(!in) {
            std::cerr<< "File not open at " << filename << std::endl;
            return;
        }
        for(std::string line; std::getline(in, line); ) {
            if(line.size()!=0 && line[0]=='#')
                ; //don't do anything with '#' lines
            else {
                std::stringstream ss(line);
                ss>>m_A>>m_d>>m_p>>m_B>>E_beam;
            }
        }
        in.close();*/

    }
};

//double Kinematics::getQval(double m1, double m2, double m3, double t1, double t3, double angle3)
double Kinematics::getExc(double t3, double angle3)
/*
  \brief Follows convention in Marion, 2013: (1 - beam, 2- target, 3-ejectile, 4-recoil)

  m1 is beam, (typically heavy nucleus)
  m2 is 'd', (light target)
  m3 is 'p', (light ejectile mass)
  t1 is beam kinetic energy,
  
  All calculations are done here, and other wrapper functions written make derived quantities from stuff calculated here.
  
  \param t3 lab-kinetic-energy in MeV
  \param angle3 lab-angle in deg of detected proton (if d,p) or other charged particle
  \return Excitation energy of the heavy-recoil nucleus in MeV
*/
{
    double m1 = m_A;
    double m2 = m_d;
    double m3 = m_p;
    double m4 = m_B;
    double t1 = E_beam; 

    m1 *= u_MeV;
    m2 *= u_MeV;
    m3 *= u_MeV;
    m4 *= u_MeV;

    double e1 = m1 + t1;
    double e3 = m3 + t3;
    ET = t1 + m1 + m2;
    double p1 = TMath::Sqrt(t1*t1 + 2*m1*t1);
    double p3 = TMath::Sqrt(t3*t3 + 2*m3*t3);
    double cosTheta = TMath::Cos(angle3*TMath::Pi()/180.);
//    return m1+m2-m3-TMath::Sqrt(m1*m1 + m2*m2 + m3*m3 + 2.*m2*e1 - 2.*e3*(e1+m2)+ 2.*p1*p3*cosTheta);
    double Q =  m1+m2-m3-TMath::Sqrt(m1*m1 + m2*m2 + m3*m3 + 2.*m2*e1 - 2.*e3*(e1+m2)+ 2.*p1*p3*cosTheta);
	
    Qx = Q;
    //Recoil properties just in case it's useful
    T4 = ET - e3 - (m1+m2-m3-Q);
    P4 = TMath::Sqrt(T4*T4 + 2*m4*T4);

    //this angle will not be affected by eloss
    theta4 = (180./M_PI)*TMath::ASin((p3/P4)*TMath::Sin(angle3*M_PI/180.));

    //recalculate everything other than angle with lowered Kinetic energy
    P4 = TMath::Sqrt(T4*T4 + 2*(m4+Q-Q0)*T4);
    gamma4 = T4/m4+1.;
    beta4 = TMath::Sqrt(1. - 1./(gamma4*gamma4));
    theta4 = (180./M_PI)*TMath::ASin((p3/P4)*TMath::Sin(angle3*M_PI/180.));
    
    //if(m2==m3) std::cout << "Q0 elastic: " << Q0 << " " << Q << "\n" ;

    return Q0 - Q;//Q0  = Q + Exc

}

double Kinematics::getBeta4(double t3, double angle3) 
/*
  \brief Follows convention in Marion, 2013: (1 - beam, 2- target, 3-ejectile, 4-recoil)

  m1 is beam, (typically heavy nucleus)
  m2 is 'd', (light target)
  m3 is 'p', (light ejectile mass)
  t1 is beam kinetic energy,
  \param t3 lab-kinetic-energy in MeV
  \param angle3 lab-angle in deg of detected proton (if d,p) or other charged particle
  \return doppler-shift beta (=v/c) of the heavy-recoil nucleus, calls getExc() to fill the value
*/
{
    getExc(t3, angle3);
    return beta4;
}

double Kinematics::getBrho(double t3, double angle3, double charge_state) {
/*
  \brief Follows convention in Marion, 2013: (1 - beam, 2- target, 3-ejectile, 4-recoil)

  m1 is beam, (typically heavy nucleus)
  m2 is 'd', (light target)
  m3 is 'p', (light ejectile mass)
  t1 is beam kinetic energy,
  \param t3 lab-kinetic-energy in MeV
  \param angle3 lab-angle in deg of detected proton (if d,p) or other charged particle
  \param charge_state charge state of the intended nucleus, in units of elementary charge (= +1 for H+, +2 for He2+ etc).
  \return b-rho value, generated from P4*3.3359e-3/charge_state where P4 is 4momentum of heavy-recoil calculated from orruba kinematics
*/
	getExc(t3,angle3);
	return P4*3.3359e-3/charge_state;
}

double Kinematics::getTheta4(double t3, double angle3) {
/*
  \brief Follows convention in Marion, 2013: (1 - beam, 2- target, 3-ejectile, 4-recoil)

  m1 is beam, (typically heavy nucleus)
  m2 is 'd', (light target)
  m3 is 'p', (light ejectile mass)
  t1 is beam kinetic energy,
  \param t3 lab-kinetic-energy in MeV
  \param angle3 lab-angle in deg of detected proton (if d,p) or other charged particle
  \param charge_state charge state of the intended nucleus, in units of elementary charge (= +1 for H+, +2 for He2+ etc).
  \return lab theta value of heavy recoil in degrees
*/
	getExc(t3,angle3);
	return theta4;
}

double InvertToBeamEnergy(double Ex,
                          double Tb_recon,
                          double theta_recon,
                          const Isotope& al27Iso,
                          const Isotope& alphaIso) {

    if (!std::isfinite(Ex) || !std::isfinite(Tb_recon) ||
        !std::isfinite(theta_recon) || !std::isfinite(al27Iso.Mass) ||
        !std::isfinite(alphaIso.Mass) || Tb_recon < 0.0 ||
        theta_recon < 0.0 || theta_recon > 180.0 ||
        al27Iso.Mass <= 0.0 || alphaIso.Mass <= 0.0) {
        std::cerr << "[InvertKin] Invalid input: Ex=" << Ex
                  << ", Tb_recon=" << Tb_recon
                  << ", theta_recon=" << theta_recon
                  << ", m(Al27)=" << al27Iso.Mass
                  << ", m(alpha)=" << alphaIso.Mass << std::endl;
        return -1.0;
    }

    // Known target state
    double Mstar = al27Iso.Mass + Ex;

    // Detected alpha (known per event)
    double E3   = Tb_recon + alphaIso.Mass;
    double p3sq = E3 * E3 - alphaIso.Mass * alphaIso.Mass;
    //double p3   = std::sqrt(p3sq);
    double cosT = std::cos(theta_recon * TMath::Pi() / 180.0);

    // Quadratic coefficients (E1 = EBeam_track + al27Iso.Mass)
    // C = m2 - E3,  K = M*² - m1² + p3² - C²
    double m1 = al27Iso.Mass;
    double m2 = alphaIso.Mass;   // target at rest

    double C  = m2 - E3;
    double K  = Mstar*Mstar - m1*m1 + p3sq - C*C;

    double a  = C*C - p3sq * cosT * cosT;
    double b  = -K * C;
    double c  = 0.25 * (K*K + 4.0 * p3sq * cosT*cosT * m1*m1);

    double disc = b*b - 4.0*a*c;
    if (!std::isfinite(disc) || disc < 0.0) {
        std::cerr << "[InvertKin] No real solution: discriminant=" << disc
                  << ", Ex=" << Ex << ", Talpha=" << Tb_recon
                  << ", theta=" << theta_recon << std::endl;
        return -1.0;   // sentinel for bad event
    }

    double sqrtDisc = std::sqrt(disc);

    // Solve for total beam energy E1. If the quadratic coefficient is
    // effectively zero, use the remaining linear equation instead.
    std::vector<double> candidates;
    const double aScale = std::max({1.0, std::abs(C * C),
                                    std::abs(p3sq * cosT * cosT)});
    if (std::abs(a) <= 1e-14 * aScale) {
        const double bScale = std::max(1.0, std::abs(K * C));
        if (std::abs(b) <= 1e-14 * bScale) {
            std::cerr << "[InvertKin] Degenerate equation: a=" << a
                      << ", b=" << b << ", c=" << c << std::endl;
            return -1.0;
        }
        candidates.push_back(-c / b);
    } else {
        candidates.push_back((-b + sqrtDisc) / (2.0 * a));
        candidates.push_back((-b - sqrtDisc) / (2.0 * a));
    }

    // Squaring loses the sign of the momentum term, so verify each candidate
    // against the unsquared equation K - 2*C*E1 = 2*p1*p3*cos(theta).
    auto isPhysical = [&](double E1) -> bool {
        if (!std::isfinite(E1) || E1 <= m1) return false;
        const double p1sq = E1 * E1 - m1 * m1;
        if (p1sq < 0.0) return false;
        const double lhs = K - 2.0 * C * E1;
        const double rhs = 2.0 * std::sqrt(p1sq * p3sq) * cosT;
        const double residualScale = std::max({1.0, std::abs(lhs), std::abs(rhs), std::abs(K)});
        return std::abs(lhs - rhs) <= 1e-8 * residualScale;
    };

    std::vector<double> physicalRoots;
    for (const double E1 : candidates) {
        if (isPhysical(E1)) physicalRoots.push_back(E1);
    }

    if (physicalRoots.size() == 1) return physicalRoots.front() - m1;
    if (physicalRoots.size() > 1) {
        std::cerr << "[InvertKin] Ambiguous physical roots; selecting higher beam energy."
                  << std::endl;
        return *std::max_element(physicalRoots.begin(), physicalRoots.end()) - m1;
    }

    std::cerr << "[InvertKin] No physical root: Ex=" << Ex
              << ", Talpha=" << Tb_recon << ", theta=" << theta_recon
              << " deg, a=" << a << ", b=" << b << ", c=" << c
              << ", disc=" << disc;
    for (const double E1 : candidates) {
        std::cerr << ", candidate Tbeam=" << E1 - m1
                  << " (unsquared lhs=" << K - 2.0 * C * E1
                  << ", rhs=" << (E1 > m1 ? 2.0 * std::sqrt(E1 * E1 - m1 * m1) *
                     std::sqrt(p3sq) * cosT : 0.0) << ")";
    }
    std::cerr << std::endl;
    return -1.0;
}


#endif