#include "ConvertToQspace.hh"
//////////////////////////////////////////////////////////
ConvertToQspace::
ConvertToQspace()
{
    Initialize();
}

//////////////////////////////////////////////////////////
ConvertToQspace::
ConvertToQspace(ElementContainerMatrix *ecm, Double vx, Double vy, Double vz )
{
    Initialize();
    SetTarget(ecm);
    SetIncidentBeamVector( vx, vy, vz );
}

//////////////////////////////////////////////////////////
ConvertToQspace::
~ConvertToQspace()
{
    delete UU;
    delete UCP;
}

//////////////////////////////////////////////////////////
void ConvertToQspace::
Initialize(){
    _MessageTag = "ConvertToQspace::";
    IncidentVect.clear();
    IncidentVect.resize(3,0.0);
    UU = new UtsusemiUnitConverter();
    UCP = new UtsusemiCheckDataProcess();
    _ProcessTag="ConvertToQspace";
    _grav_param_a = 0.0;
    _grav_param_b = 0.0;
}

//////////////////////////////////////////////////////////
void ConvertToQspace::
SetIncidentBeamVector( Double vx, Double vy, Double vz ){
    Double LL = sqrt( vx*vx + vy*vy + vz*vz );
    IncidentVect[0] = vx/LL;
    IncidentVect[1] = vy/LL;
    IncidentVect[2] = vz/LL;
}
//////////////////////////////////////////////////////////
bool ConvertToQspace::
SetIncidentBeamRPMT(Int4 DET, Int4 PIX){
    ElementContainerMatrix *ecm = Put();
    if (ecm==NULL){
        return false;
    }
    bool isFullPath=true;
    StringTools ST;
    std::string runNos = ecm->PutHeaderPointer()->PutString(UTSUSEMI_KEY_HEAD_RUNNUMBER);
    std::vector<std::string> runs_v = ST.SplitString(runNos, ",");
    if (runs_v.empty()){
        UtsusemiError("ConvertToQspace::SetIncientBeamRPMT >> Given Data is invalid (No runno).");
        return false;
    }
    UInt4 runNo = ST.StringToUInt4(runs_v[0]);
    UInt4 modeNo = 5; // for new RPMT in environ_ana.xml
    UtsusemiAnaEnvironReader AE(runNo,false);
    std::string dfile = "";
    if (AE._Status){
        std::vector<std::string> pfiles = AE.PutParamFiles(runNo, modeNo, isFullPath );
        if (pfiles.size()<2){
            return false;
        }
        dfile = pfiles[1];
    }

    SASDetectorInfoEditorRPMT DI;
    if (DI.Read(dfile)){
        Double centerX = Double(DI.PutSizeXch() / 2) - 0.5; // 304/2 -0.5 = 151.5
        Double centerY = Double(DI.PutSizeYch() / 2) - 0.5;
        Double unitX = DI.PutPixelSizeX();
        Double unitY = DI.PutPixelSizeY();

        if (DET >= (Int4)DI.PutSizeXch()){
            UtsusemiError("ConvertToQspace::SetIncientBeamRPMT >> Argument DET is too large.");
            return false;
        }
        if (PIX >= (Int4)DI.PutSizeYch()){
            UtsusemiError("ConvertToQspace::SetIncientBeamRPMT >> Argument PIX is too large.");
            return false;
        }
        Double Px = 0.0;
        Double Py = 0.0;
        if (DET >= 0) Px = (centerX - (Double)DET) * unitX; // DET->larger PX->smaller [mm]
        if (PIX >= 0) Py = ((Double)PIX - centerY) * unitY; // PIX->larger PY->larger [mm]
        Double Pz = DI.PutL2();
        SetIncidentBeamVector(Px, Py, Pz);
    }else{
        UtsusemiError("ConvertToQspace::SetIncientBeamRPMT >> DetectorInfo not found :"+dfile);
        return false;
    }
    return true;
}
//////////////////////////////////////////////////////////
bool ConvertToQspace::
SetIncidentBeamWithGravityRPMT( Double alpha, Double beta, Double px ){
    ElementContainerMatrix *ecm = Put();
    if (ecm==NULL){
        return false;
    }

    Int4 px_lo = (Int4)px;
    Int4 px_hi = px_lo+1;
    Int4 py_lo = (Int4)(alpha);
    Int4 py_hi = py_lo+1;


    std::vector<Double> p_lo;
    std::vector<Double> p_hi;

    SearchInHeader SIH( ecm );
    if ( SIH.SearchArray( "XPOS", px_lo ) ){
        std::vector< std::vector<UInt4> > ret = SIH.PutResults();
        if (!(ret.empty())){
            ElementContainerArray* eca = ecm->PutPointer(ret[0][0]);
            SearchInHeader SIH2( eca );
            if ( SIH2.Search( "YPOS", py_lo ) ){
                std::vector< std::vector<UInt4> > ret2 = SIH2.PutResults();
                std::cout << "YPOS=" << ret2[0][0] << "-" << ret2[0][1] << std::endl;
                if (!(ret2.empty()))
                    p_lo = eca->PutPointer( ret2[0][0] )->PutHeaderPointer()->PutDoubleVector("PixelPosition");
            }
        }
    }
    if ( SIH.SearchArray( "XPOS", px_hi ) ){
        std::vector< std::vector<UInt4> > ret = SIH.PutResults();
        if (!(ret.empty())){
            ElementContainerArray* eca = ecm->PutPointer(ret[0][0]);
            SearchInHeader SIH2( eca );
            if ( SIH2.Search( "YPOS", py_hi ) ){
                std::vector< std::vector<UInt4> > ret2 = SIH2.PutResults();
                std::cout << "YPOS=" << ret2[0][0] << "-" << ret2[0][1] << std::endl;
                if (!(ret2.empty()))
                    p_hi = eca->PutPointer( ret2[0][0] )->PutHeaderPointer()->PutDoubleVector("PixelPosition");
            }
        }
    }

    if ( (p_lo.empty()) || (p_hi.empty()) ){
        return false;
    }

    _grav_param_a = p_lo[1]+(p_hi[1]-p_lo[1])*(alpha-(Double)py_lo);
    _grav_param_b = beta*(p_hi[1]-p_lo[1]);

    IncidentVect[0] = -1.0*( p_lo[0] + (p_hi[0]-p_lo[0])*(px - (double)px_lo) );
    IncidentVect[1] = 0.0;
    IncidentVect[2] = (p_lo[2]+p_hi[2])/2.0;

    /*
    std::cout << "px "<<px_lo<<","<<px_hi<<std::endl;
    std::cout << "py "<<py_lo<<","<<py_hi<<std::endl;
    std::cout << "P_lo="<<p_lo[0]<<","<<p_lo[1]<<","<<p_lo[2]<<std::endl;
    std::cout << "P_hi="<<p_hi[0]<<","<<p_hi[1]<<","<<p_hi[2]<<std::endl;
    std::cout << "_grav_param_a="<<_grav_param_a<<std::endl;
    std::cout << "_grav_param_b="<<_grav_param_b<<std::endl;
    std::cout << "IncidentVect = "<<IncidentVect[0]<<","<<IncidentVect[1]<<","<<IncidentVect[2]<<std::endl;
    */
    return true;

}
//////////////////////////////////////////////////////////
bool ConvertToQspace::
SetIncidentBeamWithGravityRPMT( Double beta, Double lambda, Double px, Double py ){
    //alpha + beta*lambdda*lambda = py
    Double alpha = py -  beta*lambda*lambda;
    return SetIncidentBeamWithGravity( alpha, beta, px );

}
//////////////////////////////////////////////////////////
bool ConvertToQspace::
SetIncidentBeamWithGravityRPMTDetectInfo( UInt4 runNo, UInt4 modeNo, std::string dfile,  Double beta, Double l2 ){
    ElementContainerMatrix *ecm = Put();
    if (ecm==NULL){
        return false;
    }
    bool isFullPath=true;
    if (dfile.empty()){
        UtsusemiAnaEnvironReader AE(runNo,false);
        if (AE._Status){
            std::vector<std::string> pfiles = AE.PutParamFiles( runNo, modeNo, isFullPath );
            dfile = pfiles[1];
        }else{
            return false;
        }
    }
    SASDetectorInfoEditorRPMT DR;
    if (DR.Read(dfile)){
        _grav_param_a = 0.0;
        _grav_param_b = DR.PutIncidentGravityParam();
        IncidentVect[0] = 0.0;
        IncidentVect[1] = 0.0;
        IncidentVect[2] = DR.PutL2();
    }else{
        return false;
    }
    if (beta!=0.0)
        _grav_param_b = beta;
    if (l2!=0.0)
        IncidentVect[2] = l2;

    return true;
}
//////////////////////////////////////////////////////////
bool ConvertToQspace::
SetIncidentBeamWithGravity( Double alpha, Double beta, Double px ){
    ElementContainerMatrix *ecm = Put();
    if (ecm==NULL){
        return false;
    }

    Int4 px_lo = (Int4)px;
    Int4 px_hi = px_lo+1;
    Int4 py_lo = (Int4)(alpha);
    Int4 py_hi = py_lo+1;

    std::vector<Double> p_lo = (*ecm)(px_lo,py_lo)->PutHeaderPointer()->PutDoubleVector("PixelPosition");
    std::vector<Double> p_hi = (*ecm)(px_hi,py_hi)->PutHeaderPointer()->PutDoubleVector("PixelPosition");

    if ( (p_lo.empty()) || (p_hi.empty()) ){
        return false;
    }

    _grav_param_a = p_lo[1]+(p_hi[1]-p_lo[1])*(alpha-(Double)py_lo);
    _grav_param_b = beta*(p_hi[1]-p_lo[1]);

    IncidentVect[0] = p_lo[0] + (p_hi[0]-p_lo[0])*(px - (double)px_lo);
    IncidentVect[1] = 0.0;
    IncidentVect[2] = (p_lo[2]+p_hi[2])/2.0;

    /*
    std::cout << "px "<<px_lo<<","<<px_hi<<std::endl;
    std::cout << "py "<<py_lo<<","<<py_hi<<std::endl;
    std::cout << "P_lo="<<p_lo[0]<<","<<p_lo[1]<<","<<p_lo[2]<<std::endl;
    std::cout << "P_hi="<<p_hi[0]<<","<<p_hi[1]<<","<<p_hi[2]<<std::endl;
    std::cout << "_grav_param_a="<<_grav_param_a<<std::endl;
    std::cout << "_grav_param_b="<<_grav_param_b<<std::endl;
    std::cout << "IncidentVect = "<<IncidentVect[0]<<","<<IncidentVect[1]<<","<<IncidentVect[2]<<std::endl;
    */

    return true;
}
//////////////////////////////////////////////////////////
bool ConvertToQspace::
Execute(){
    StringTools ST;
    UtsusemiMessage(_MessageTag + "Gravity parameter is set to " + ST.DoubleToString(_grav_param_b));
    ElementContainerMatrix *ecm = Put();

    bool ret = false;

    if (UCP->CheckProcess( ecm, _ProcessTag )){
        UCP->RemoveProcess( ecm, _ProcessTag, true );
        //std::cout << _MessageTag + "Execute : already done  ConvertToQSpace" << std::endl;
        //return true;
    }
    if (!(UCP->CheckProcess( ecm, "ConvertToPoint"))){
        std::cout << _MessageTag + "Execute : Data has never been done ConvertToPoint" << std::endl;
        return ret;
    }

#ifdef MULTH
    //omp_set_num_threads( MULTH );
    omp_set_num_threads( 1 );
#endif

    for (UInt4 i=0; i<ecm->PutSize(); i++){
        ElementContainerArray* eca = ecm->PutPointer(i);
#pragma omp parallel for
#if (_OPENMP >= 200805)  // OpenMP 3.0 and later
        for (UInt4 j=0; j<eca->PutSize(); j++){
#else
        for (Int4 j=0; j<eca->PutSize(); j++){
#endif
            ElementContainer* ec = eca->PutPointer(j);
            HeaderBase* h_ec = ec->PutHeaderPointer();
            std::vector<Double> lambda_hist = ec->PutX();
            //std::vector<Double> intens_hist = ec->PutY();
            //std::vector<Double> error_hist = ec->PutE();
            std::string X_key = ec->PutXKey();
            std::string Y_key = ec->PutYKey();
            std::string E_key = ec->PutEKey();

            //std::vector<Double> lambda = ec->Put("Lamb");
            std::vector<Double> lambda = ec->Put("Lambda"); //[inamura 140314]
            UInt4 lambda_size = lambda.size();
            std::vector<Double> k_val( lambda_size, 0.0 );
            std::vector<Double>::iterator it_lamb = lambda.begin();
            std::vector<Double>::iterator it_kval = k_val.begin();
            while( it_lamb!=lambda.end() ){
                (*it_kval) = (UU->VtoLambda())/(*it_lamb)*1000.0 * (UU->Vmm_msToK());
                it_lamb++;
                it_kval++;
            }

            std::vector<Double> p_posi = h_ec->PutDoubleVector("PixelPosition");
            Double LL = sqrt( p_posi[0]*p_posi[0] + p_posi[1]*p_posi[1] + p_posi[2]*p_posi[2] );
            Double px = p_posi[0]/LL;
            Double py = p_posi[1]/LL;
            Double pz = p_posi[2]/LL;

            std::vector<Double> Qx( lambda_size, 0.0 );
            std::vector<Double> Qy( lambda_size, 0.0 );
            std::vector<Double> Qz( lambda_size, 0.0 );

            Double ivx = IncidentVect[0];
            Double ivy = IncidentVect[1];
            Double ivz = IncidentVect[2];
            if (_grav_param_b!=0.0){
                ivx = 0.0;
                ivy = 0.0;
                ivz = 1.0;
            }
            for (UInt4 k=0; k<lambda_size; k++){
                //intens_hist[i] = intens_hist[i]/(lambda_hist[i+1]-lambda_hist[i]);
                //error_hist[i] = error_hist[i]/(lambda_hist[i+1]-lambda_hist[i]);
                if (_grav_param_b!=0.0){
                    /*
                    ivx = IncidentVect[0];
                    ivy = _grav_param_a + _grav_param_b*( lambda[k]*lambda[k] );
                    ivz = IncidentVect[2];
                    Double ll = sqrt(ivx*ivx+ivy*ivy+ivz*ivz);
                    ivx /= ll;
                    ivy /= ll;
                    ivz /= ll;
                    */
                    Double d = _grav_param_a - _grav_param_b*( lambda[k]*lambda[k] );
                    LL = sqrt( p_posi[0]*p_posi[0] + (p_posi[1] - d) * (p_posi[1] - d) + p_posi[2]*p_posi[2] );
                    px = p_posi[0]/LL;
                    py = (p_posi[1] - d)/LL;
                    pz = p_posi[2]/LL;
                }
                //Qx[k] = k_val[k] * ( px - ivx );
                //Qy[k] = k_val[k] * ( py - ivy );
                //Qz[k] = k_val[k] * ( pz - ivz );
                // ki - kf
                Qx[k] = k_val[k] * ( ivx - px );
                Qy[k] = k_val[k] * ( ivy - py );
                Qz[k] = k_val[k] * ( ivz - pz );
            }
            //ec->Replace( Y_key, intens_hist );
            //ec->Replace( E_key, error_hist );
            if (ec->CheckKey("Qx")==1){
                ec->Replace( "Qx", Qx );
                ec->Replace( "Qy", Qy );
                ec->Replace( "Qz", Qz );
            }else{
                ec->Add( "Qx", Qx, "1/Ang" );
                ec->Add( "Qy", Qy, "1/Ang" );
                ec->Add( "Qz", Qz, "1/Ang" );
            }
            //ec->SetKeys( X_key, Y_key, E_key ); //[inamura 140314]
        }
    }

    UCP->AddProcess( ecm, _ProcessTag );

    return true;
}
