#ifndef HodTrack_HH
#define HodTrack_HH

#include <string>
#include <iostream>

#include "UtlPrintHex.hh"

class HodTrack {

public:
  HodTrack(const BmlHodEventData &hd) {
    _data[0]=hd.data(BmlHodEventData::x1);
    _data[1]=hd.data(BmlHodEventData::y1);
    _data[2]=hd.data(BmlHodEventData::x2);
    _data[3]=hd.data(BmlHodEventData::y2);
  }

  unsigned short data(unsigned i) const {
    return _data[i];
  }


    bool track(double *r) {
        r[0]=-1.0;
        r[1]=-1.0;

        double xy[2][2];

        for(unsigned i(0);i<4;i++) {
	  //bool reply(false);
            xy[i/2][i%2]=-1.0;

            for(unsigned j(0);j<16;j++) {
                if(_data[i]==(1<<j)) {
		  //reply=true;
                    xy[i/2][i%2]=j*_dPlane;
                }
            }
            {for(unsigned j(0);j<15;j++) {
                if(_data[i]==(3<<j)) {
		  //reply=true;
                    xy[i/2][i%2]=(j+0.5)*_dPlane;
                }
            }}

	    //std::cout << "Here1 " << r[0] << " " << r[1] << std::endl;
            //if(!reply) return false;
	    if(xy[i/2][i%2]<0.0) return false;
        }

	//std::cout << "Here " << r[0] << " " << r[1] << std::endl;

        r[0]=(xy[0][0]*_zPlane2-xy[1][0]*_zPlane0)/
        (_zPlane2-_zPlane0);
        r[1]=(xy[0][1]*_zPlane3-xy[1][1]*_zPlane1)/
        (_zPlane3-_zPlane1);

        return true;
    }

    unsigned numberOfBits(unsigned p) {
        unsigned n(0);
        for(unsigned i(0);i<16;i++) {
            if((_data[i]&(1<<i))!=0) n++;
        }
        return n;
    }

    std::ostream& print(std::ostream &o, std::string s="") {
        o << s << "HodTrack::print()" << std::endl;

        o << s << " Scintillator spacing = " << _dPlane << " cm" << std::endl;
        o << s << " Scintillator z heights"
        << ", X1 " << _zPlane0
        << " cm, Y1 " << _zPlane1
        << " cm, X2 " << _zPlane2
        << " cm, Y2 " << _zPlane3 << " cm" << std::endl;

        o << s << " Plane X1 = " << printHex(_data[0]) << std::endl;
        o << s << " Plane Y1 = " << printHex(_data[1]) << std::endl;
        o << s << " Plane X2 = " << printHex(_data[2]) << std::endl;
        o << s << " Plane Y2 = " << printHex(_data[3]) << std::endl;

        double r[2];
        if(track(r)) {
           o << s << " Track interpolation, X0 = " 
             << r[0] << " cm, Y0 = " << r[1] << " cm" << std::endl;
        } else {
           o << s << " No track interpolation" << std::endl;
        }
		return o;
    }


private:
    unsigned short _data[4];

  //static const double _zPlane[4];
    static const double _zPlane0;
    static const double _zPlane1;
    static const double _zPlane2;
    static const double _zPlane3;
    static const double _dPlane;
};

//const double HodTrack::_zPlane[4]={82.0,77.0,-60.0,-57.0};
const double HodTrack::_zPlane0=82.0;
const double HodTrack::_zPlane1=77.0;
const double HodTrack::_zPlane2=-60.0;
const double HodTrack::_zPlane3=-57.0;
const double HodTrack::_dPlane=2.72;

#endif
