#include "PhiCalculation.hh"
//////////////////////////////////////////////////////////
PhiCalculation::
PhiCalculation()
{
    Initialize();
}
//////////////////////////////////////////////////////////
PhiCalculation::
PhiCalculation(ElementContainerMatrix *ecm)
{
    Initialize();
    SetTarget(ecm);
}
//////////////////////////////////////////////////////////
PhiCalculation::
PhiCalculation(ElementContainerMatrix *ecm, double d_px, double d_py, double d_pz)
{
    Initialize();
    SetPositionDeviationConstant(d_px, d_py, d_pz);
    SetTarget(ecm);
}
//////////////////////////////////////////////////////////
PhiCalculation::
~PhiCalculation()
{
    delete st;
}
//////////////////////////////////////////////////////////
void PhiCalculation::
Initialize(){
    SetPositionDeviationConstant(0.0,0.0,0.0);
    commentHead = "PhiCalculation >> ";
    st = new StringTools();
}
//////////////////////////////////////////////////////////
void PhiCalculation::
CalcPhi(){
    ElementContainerMatrix *ecm = Put();
    ElementContainerArray *eca;
    ElementContainer *ec;
    HeaderBase *h;
   
    UInt4 num_of_psd, num_of_pixel;
    Double L2,err,LL;
    vector<double> pposi, phi_pol, phi_azim;
    vector<string> process;

    if ((delta_px==0.0)&&(delta_py==0.0)&&(delta_pz==0.0)){
        if (ecm->PutHeaderPointer()->CheckKey( UTSUSEMI_KEY_HEAD_TYPICALPIXELSIZE )==0){
            string msg = commentHead+"There is no information of pixel size in given data.";
            UtsusemiError(msg);
            return;
        }else{
            vector<Double> ps = ecm->PutHeaderPointer()->PutDoubleVector(UTSUSEMI_KEY_HEAD_TYPICALPIXELSIZE);
            delta_px = ps[0]/2.0;
            delta_py = ps[1]/2.0;
            delta_pz = ps[2]/2.0;
        }
    }
    num_of_psd = ecm->PutTableSize();
    for (UInt4 psd=0;psd<num_of_psd;psd++){
        eca = ecm->PutPointer(psd);
        num_of_pixel = eca->PutTableSize();
        for (UInt4 pixel=0;pixel<num_of_pixel;pixel++){
            pposi.clear();
            phi_pol.clear();
            phi_azim.clear();
            phi_pol.resize(2,0.0);
            phi_azim.resize(2,0.0);
            ec = eca->PutPointer(pixel);
            if (ec->PutHeaderPointer()->CheckKey(UTSUSEMI_KEY_HEAD_PIXELPOSITION)==0){
                string msg = commentHead+"There is no PixelPosition in ElementContainer of Pixel-ID= ";
                msg+=st->Int4ToString(ec->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_PIXELID))+".";
                UtsusemiError( msg );
                continue;
            }
            pposi = ec->PutHeaderPointer()->PutDoubleVector(UTSUSEMI_KEY_HEAD_PIXELPOSITION);
            L2 = sqrt( pposi[0]*pposi[0] + pposi[1]*pposi[1] + pposi[2]*pposi[2] );
            // Bugfix 100427
            phi_pol[0] = 180.0/M_PI*acos( pposi[2]/L2 );
            err = sqrt( (delta_pz*delta_pz + (pposi[2]*pposi[2]/(L2*L2))*(delta_px*delta_px+delta_py*delta_py+delta_pz*delta_pz) ))/L2/sqrt( pow( sin( phi_pol[0]/180.0*M_PI ),2.0 ) );
            phi_pol[1] = err/M_PI*180.0;
            LL = pposi[0]*pposi[0]+pposi[1]*pposi[1];
            if (pposi[1]==0.0){
                if (pposi[0]>=0) phi_azim[0] = 0.0;
                else phi_azim[0] = 180.0;
            }else{
                phi_azim[0] = 180.0/M_PI*acos( pposi[0]/sqrt(LL) )*pposi[1]/sqrt( pposi[1]*pposi[1] );
            }
            //err = ( pow((1.0-pposi[0]*pposi[0]/LL),2.0) * pow(delta_px,2.0) + pow((2.0*pposi[0]*pposi[1]/LL),2.0)*pow(delta_py,2.0))/LL/sqrt( pow( sin( phi_azim[0] ),2.0 ) );
            err = sqrt( ( pow( (pposi[1]*pposi[1]*delta_px),2.0 ) + pow( (2*pposi[0]*pposi[1]*delta_py),2.0 ) ) / pow( sin( phi_azim[0]*M_PI/180.0 ),2.0 ) / pow( (pposi[0]*pposi[0] + pposi[1]*pposi[1]),3.0 ));
            phi_azim[1] = err*180.0/M_PI;
            
            h = ec->PutHeaderPointer();
            if (h->CheckKey(UTSUSEMI_KEY_HEAD_PIXELPOLARANGLES)==0){
                h->Add(UTSUSEMI_KEY_HEAD_PIXELPOLARANGLES,phi_pol);
            }else{
                h->OverWrite(UTSUSEMI_KEY_HEAD_PIXELPOLARANGLES,phi_pol);
            }
            if (h->CheckKey(UTSUSEMI_KEY_HEAD_PIXELAZIMANGLES)==0){
                h->Add(UTSUSEMI_KEY_HEAD_PIXELAZIMANGLES,phi_azim);
            }else{
                h->OverWrite(UTSUSEMI_KEY_HEAD_PIXELAZIMANGLES,phi_azim);
            }
        }
    }

    h = ecm->PutHeaderPointer();     
    if (h->CheckKey(UTSUSEMI_KEY_HEAD_DATAPROCESSED)!=1) return;
    process = h->PutStringVector(UTSUSEMI_KEY_HEAD_DATAPROCESSED);
    process.push_back("Phi Calculation");
    h->OverWrite(UTSUSEMI_KEY_HEAD_DATAPROCESSED,process);

}


//////////////////////////////////////////////////////////
void PhiCalculation::
SetPositionDeviationConstant(double d_px, double d_py, double d_pz){
    delta_px = d_px/2.0;
    delta_py = d_py/2.0;
    delta_pz = d_pz/2.0;
}
