#include "CalcSphericalCoordinates.hh"
//////////////////////////////////////////////////////////
CalcSphericalCoordinates::
CalcSphericalCoordinates()
{
    Initialize();
}
//////////////////////////////////////////////////////////
CalcSphericalCoordinates::
CalcSphericalCoordinates(ElementContainerMatrix *ecm)
{
    Initialize();
    SetTarget(ecm);
}
//////////////////////////////////////////////////////////
CalcSphericalCoordinates::
CalcSphericalCoordinates(ElementContainerMatrix *ecm, double d_px, double d_py, double d_pz)
{
    Initialize();
    SetPositionDeviationConstant(d_px, d_py, d_pz);
    SetTarget(ecm);
}
//////////////////////////////////////////////////////////
CalcSphericalCoordinates::
~CalcSphericalCoordinates()
{
}
//////////////////////////////////////////////////////////
void CalcSphericalCoordinates::
Initialize(){
    SetPositionDeviationConstant(10.0, 12.5, 10.0);
    HeadNameHorzAngle = "PixelDirectAngle";
    HeadNameElevAngle = "PixelElevatAngle";
    commentHead = "CalcSphericalCoordinates >> ";

}
//////////////////////////////////////////////////////////
void CalcSphericalCoordinates::
CalcAngles(){
    ElementContainerMatrix *ecm = Put();
    ElementContainerArray *eca;
    ElementContainer *ec;
    HeaderBase *h;
   
    UInt4 num_of_psd, num_of_pixel;
    Double px,py,pz,SL2,L2h,phi,err;
    vector<double> pposi, phi_h, phi_l;
    vector<string> process;


    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_h.clear();
            phi_l.clear();
            ec = eca->PutPointer(pixel);
            if (ec->PutHeaderPointer()->CheckKey(UTSUSEMI_KEY_HEAD_PIXELPOSITION)==0){
                StringTools st = StringTools();
                string msg = commentHead;
                msg += "There is no PixelPosition in ElementContainer of Pixel-ID= ";
                msg += st.Int4ToString( ec->PutHeaderPointer()->PutInt4(UTSUSEMI_KEY_HEAD_PIXELID) )+".";
                UtsusemiError( msg );
                continue;
            }
            
            // Get Pixel Position
            pposi = ec->PutHeaderPointer()->PutDoubleVector(UTSUSEMI_KEY_HEAD_PIXELPOSITION);
            px = pposi[0];
            py = pposi[1];
            pz = pposi[2];
            
            // Prepare some expressions
            SL2 = px*px + py*py + pz*pz;  // the square of L2
            L2h = sqrt( px*px + pz*pz );  // the length for projection of L2 on horizontal plane

            // Calculate for direction angle on horizontal plane
            phi = (px/fabs(px))*acos(pz/L2h);
            phi_h.push_back( phi*180.0/M_PI );
            err = sqrt( (1.0/SL2/(sin(phi)*sin(phi))) * ( pow( ((px*py/SL2)*delta_px),2.0) + pow( ( (1.0-(pz*pz/SL2))*delta_pz ),2.0) ) );
            phi_h.push_back( err*180.0/M_PI );

            // Calculate for elevation angle from horizontal plane
            phi = atan(py/L2h);
            phi_l.push_back( phi*180.0/M_PI );
            err = sqrt( ( pow((L2h/sqrt(SL2)),4.0)/pow(SL2,3.0) ) * ( pow( (px*py*delta_px),2.0 ) + pow( (SL2*delta_py),2.0 ) + pow( (py*pz*delta_pz),2.0 ) ) );
            phi_l.push_back( err*180.0/M_PI );
            
            // Set angles and ambiguities as header information
            // If the key is not existed, make new key.
            h = ec->PutHeaderPointer();
            if (h->CheckKey(HeadNameHorzAngle)==0){
                h->Add(HeadNameHorzAngle,phi_h);
            }else{
                h->OverWrite(HeadNameHorzAngle,phi_h);
            }
            if (h->CheckKey(HeadNameElevAngle)==0){
                h->Add(HeadNameElevAngle,phi_l);
            }else{
                h->OverWrite(HeadNameElevAngle,phi_l);
            }
        }
    }
    
    // Add the information that this process has been done into header
    h = ecm->PutHeaderPointer();     
    process = h->PutStringVector(UTSUSEMI_KEY_HEAD_DATAPROCESSED);
    process.push_back("SphericalAngle Calculation");
    h->OverWrite(UTSUSEMI_KEY_HEAD_DATAPROCESSED,process);

}


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