#include "BDTarget.h"
#include "BDProfMon.h"

#include <Conventions/Munits.h>
#include <RawData/RawBeamMonBlock.h>
#include <RawData/RawBeamData.h>

#include <MessageService/MsgService.h>
CVSID("$Id: BDTarget.cxx,v 1.17 2007/01/26 22:06:11 mdier Exp $");


#include <cmath>
#include <string>
#include <cmath>
using namespace std;

BDTarget::BDTarget()
    : BDProcessor()
{
    for (int ind=0; ind<4; ++ind) fBpms[ind] = 0;
    for (int ind=0; ind<2; ++ind) fPMs[ind] = 0;
    for (int ind=0; ind<3; ++ind) fTgt[ind] = 0;
}

BDTarget::~BDTarget()
{
}

static bool check_data(const RawBeamData* const* d, int n)
{
    for (int ind=0; ind<n; ++ind) {
	if (0 == d[ind]) return false;
	if (0 == d[ind]->GetDataLength()) return false;
    }
    return true;
}

static double get_dae_time(const RawBeamData* const* d, int n)
{
// Get the time from one of the devices
    double devtime =0;
    for (int idev=0; idev<n; ++idev){
        if (devtime==0) devtime = d[idev]->GetSeconds();
    }
    return devtime;
}

void BDTarget::SetSpill(const RawBeamMonHeaderBlock& /*rbmhb*/,
			const RawBeamMonBlock& rbmb)
{

    // Order matters!
    const char* bpms[] = { "E:VP121", "E:HP121", "E:VPTGT", "E:HPTGT",
			   "E:VITGT", "E:HITGT", 0 };
    for (int ind=0; bpms[ind]; ++ind) {
	fBpms[ind] = rbmb[bpms[ind]];
	if (!fBpms[ind])
	    MSG("BDU",Msg::kDebug) << "BPM \"" << bpms[ind] << "\" not found\n";
    }
    
    const char* pms[] = { "E:M121DS", "E:MTGTDS", 0 };
    for (int ind=0; pms[ind]; ++ind) fPMs[ind] = rbmb[pms[ind]];
    if (check_data(fPMs,2)) {
	fPM121.SetData(*fPMs[0]);
	fPMTGT.SetData(*fPMs[1]);
    }
    
    const char* tgt[] = { "I:NUTARZ", "I:NUTGUV", "I:NUTGDV", 0 };
    for (int ind=0; tgt[ind]; ++ind) fTgt[ind] = rbmb[tgt[ind]];
}

// Extrapolate from point 1 to 2 to point 3.  Returns the transverse
// position at point 3 given longitudinal one.
static double extrapolate_position(double t1, double z1, double t2, double z2, double z3)
{
    return t1+(t2-t1)*(z3-z1)/(z2-z1);
}

/* From Jim Hylen

Pseudo-ME target location is now defined as 100 cm upstream
of standard target LE location.
The beginning of the 95.2 cm long vertical fin target is
38 cm upstream of ACTRN1 for LE and
138 cm upstream of ACTRN1 for Semi-ME.
There will be small adjustments to this from the final
survey data.

*/

/* Plus beam sheet:
   http://www-numi.fnal.gov/numwork/tdh/TDH_V2_4.1_Appendix_C.txt
*/

static const double
    z_hp121 =    -72.2309*Munits::foot,	// HP121
    z_vp121 =    -71.3142*Munits::foot,	// VP121
    z_pm121 =    -70.5267*Munits::foot,	// PM121
    z_hptgt =    -33.1564*Munits::foot,	// HPTGT
    z_vptgt =    -32.2397*Munits::foot,	// VPTGT
    z_pmtgt =    -31.5980*Munits::foot,	// PMTGT
    z_target_le = 0.0*Munits::foot, // LE location
    actrn1  =     1.2467192*Munits::foot; // ACTRN1



int BDTarget::GetNbatches() const
{
    // return the shortest physical array length of all BPM positions
    // and intensities used.
    int n = fBpms[0]->GetDataLength();
    for (int ind=1; ind<6; ++ind) {
	int n2 = fBpms[ind]->GetDataLength();
	if (n2<n) n=n2;
    }
    return n;
}


void BDTarget::BpmProjection(vector<double> &xp, vector<double> &yp,
			     vector<double> &xi, vector<double> &yi) const
{
    xp.clear();
    yp.clear();
    xi.clear();
    yi.clear();

    if (!check_data(fBpms,6)) return;
    
    double devtime = get_dae_time(fBpms,6);
    double z_targ=0;
    bool is_in = false;
    if (! this->TargetIn(is_in,z_targ,devtime) || !is_in) return;

    // find smallest number of batches (should all be same)
    int n = this->GetNbatches();

    int start=0;
    if (n>1) ++start; // skip average but only if multiple batches

    for (int ind=start; ind<n; ++ind) {

	if (fBpms[4]->GetData()[ind] == 0 || fBpms[5]->GetData()[ind] == 0) {
	    yp.push_back(0.0);
	    xp.push_back(0.0);
	    yi.push_back(0.0);
	    xi.push_back(0.0);
	    continue;
	}

	yp.push_back
	    (extrapolate_position(fBpms[0]->GetData()[ind]*Munits::mm,z_vp121,
				  fBpms[2]->GetData()[ind]*Munits::mm,z_vptgt,
				  z_targ));
	xp.push_back
	    (extrapolate_position(fBpms[1]->GetData()[ind]*Munits::mm,z_hp121,
				  fBpms[3]->GetData()[ind]*Munits::mm,z_hptgt,
				  z_targ));
	yi.push_back(fBpms[4]->GetData()[ind]);
	xi.push_back(fBpms[5]->GetData()[ind]);
    }
    return;
}

int BDTarget::ProfileProjection(double &x, double &y, double &xrms, double &yrms) const
{
    if (!check_data(fPMs,2)) return 0;    

    double devtime = get_dae_time(fPMs,2);

    double z_targ=0;
    bool is_in = false;
    if (! this->TargetIn(is_in,z_targ,devtime) || !is_in) return 0;

    double x121=-999,y121=-999,xrms121=-999,yrms121=-999;
    //    fPM121.GetStats(x121,y121,xrms121,yrms121);
    fPM121.GetGaussFit(x121,y121,xrms121,yrms121);
    
    double xtgt=0,ytgt=0,xrmstgt=0,yrmstgt=0;
    //    fPMTGT.GetStats(xtgt,ytgt,xrmstgt,yrmstgt);
    fPMTGT.GetGaussFit(xtgt,ytgt,xrmstgt,yrmstgt);

    if (fabs(x121) < 11*Munits::mm &&
	fabs(xtgt) < 11*Munits::mm &&
	fabs(y121) < 11*Munits::mm &&
	fabs(ytgt) < 11*Munits::mm) {
      x = extrapolate_position(x121,z_pm121,xtgt,z_pmtgt,z_targ);
      y = extrapolate_position(y121,z_pm121,ytgt,z_pmtgt,z_targ);
    }
    
    xrms = xrmstgt;
    yrms = yrmstgt;

    return 1;
}

BDTarget::BeamType BDTarget::TargetIn(bool& is_in, double& location, double spilltime) const
{        
        /* From Jim Hylen

        Look at I:NUTGUV and I:NUTGDV
        If they are around 1531 and 1441, target is "in"
        If they are between 8800 and 8900, target is "out"
        Unit is .001"
    
        For I:NUTARZ, LE ~ 0, Semi-ME ~ 39373, Semi-HE ~98408
        Calibration appears to have been drifting by a few
        tenths of a percent.
        */

        // note on units: mils are used here implicitly as this is what
        // the raw data is in.  location is returned in Munits

    is_in = false;
    location = -9999/Munits::mil;

    if (spilltime==0){
        // use the device readout to determine the target position
        if (!check_data(fTgt,3)) return kUnknown;
        location = fTgt[0]->GetData()[0];
        double up = fTgt[1]->GetData()[0];
        double down = fTgt[2]->GetData()[0];
        
        // Check insert.
        if (up < 2000 && down < 2000) is_in = true;
        
    }
    else {
        // use the measured average over a period of time in order to
        // avoid throwing away spills where the target was actually in
        // but the device failed to read out 

        // default assume target is in
        is_in = true;
        // target positions        
        // FIXME: this is hardcoded for the time being, it would be
        // good if this goes into a database
        if (spilltime <= 1109540100) location = 39465;      // 100 cm
        else if (spilltime <= 1109899600) location = 98070; // 250 cm
        else if (spilltime <= 1110280000) location = 39465; // 100 cm
        else if (spilltime <= 1112010000) location = 815.4; //   0 cm
        else if (spilltime <= 1112574300) location = 39350; // 100 cm
        else if (spilltime <= 1114040000) is_in = false;
        else if (spilltime <= 1114062000) location = 39372; // 100 cm
        else if (spilltime <= 1114640000) is_in = false;
        else if (spilltime <= 1115935000) location = 39363; // 100 cm
        else if (spilltime <= 1116615000) location = 98578; // 250 cm
        else if (spilltime <= 1149202720) location = 3935;  // 10 cm
        else if (spilltime <= 1150047812) location = 57313; // 150 cm
        else if (spilltime <= 1155564632) location = 96227; // 250 cm
        else location = 3937;                               // 10 cm
    }
    
    location *= Munits::mil;
    BeamType bt = kUnknown;
    const float dist_cut = 0.35*Munits::meter;
    const float le_pos   = 0.0*Munits::meter;
    const float pme_pos  = 1.25*Munits::meter;
    const float phe_pos  = 2.5*Munits::meter;
    if (fabs(location-le_pos) < dist_cut)       bt = kLE;
    else if (fabs(location-pme_pos) < dist_cut) bt = kPsME;
    else if (fabs(location-phe_pos) < dist_cut) bt = kPsHE;
    else bt = kUnknown;

    return bt;
}