#include "RPMTDataConverter.hh"
//////////////////////////////////////////////////////////
RPMTDataConverter::RPMTDataConverter(){
    _EventSize = (UInt4)8;
    _HeaderT0Event      = (UChar)0x5b;
    _HeaderClockEvent   = (UChar)0x5c;
    _HeaderNeutronEvent = (UChar)0x5a;
    _MaxPHA = 1024;
    _numOfLines = 1024;
    _binWidth = 2;
    _size_Xch = 304;
    _size_Ych = 304;
    _XCHPSDNO = 0;
    _YCHPSDNO = 1;
    _XchShift = 0;
    _YchShift = 0;
    _LLD_X = 128;
    _LLD_Y = 1024;
    _HLD_X = 128;
    _HLD_Y = 1024;

    _XchOffset = (UInt4)(_numOfLines/_binWidth/2 - (Int4)(_size_Xch/2) - _XchShift);
    _YchOffset = (UInt4)(_numOfLines/_binWidth/2 - (Int4)(_size_Ych/2) - _YchShift);

    _DaqId = 15;
    _MessageTag = "RPMTDataConverter::";
    _st = new StringTools();

    if (UtsusemiEnvGetDebugMode()){
        std::cout << _MessageTag + " Default value" << std::endl;
        std::cout << "_size_Xch = " << _size_Xch << std::endl;
        std::cout << "_size_Ych = " << _size_Ych << std::endl;
        std::cout << "_XchShift = " << _XchShift << std::endl;
        std::cout << "_YchShift = " << _YchShift << std::endl;
        std::cout << "_XchOffset = "<<_XchOffset<<std::endl;
        std::cout << "_YchOffset = "<<_YchOffset<<std::endl;
        std::cout << "_LLD_X, _HLD_X = " << _LLD_X << "," << _HLD_X << std::endl;
        std::cout << "_LLD_Y, _HLD_Y = " << _LLD_Y << "," << _HLD_Y << std::endl;
    }
}

//////////////////////////////////////////////////////////
RPMTDataConverter::~RPMTDataConverter(){
    delete _st;
}

//////////////////////////////////////////////////////////
void RPMTDataConverter::_addPath( std::string *orgPath, std::string *addPath ){

    std::string::size_type ind = orgPath->find_last_of("/");
    if (ind==(orgPath->size()-1)){
        *orgPath = *orgPath+*addPath;
    }else{
        *orgPath = *orgPath+"/"+*addPath;
    }

}
//////////////////////////////////////////////////////////
std::vector<UInt4> RPMTDataConverter::DecodeOneEvent( const UChar *data, UInt4 LLD_X, UInt4 HLD_X, UInt4 LLD_Y, UInt4 HLD_Y ){
    std::vector<UInt4> ret;

    UInt4 tof_clock = (UInt4)( *(data+1) << 16 )
        + (UInt4)( *(data+2) << 8 )
        + (UInt4)( *(data+3) );
    UInt4 psd_No = (UInt4)( *(data+4) );
    //UInt4 k = (UInt4)( *(data+6) )/16;
    //UInt4 ph_l = (UInt4)( *(data+5) ) * 16 +  k ;
    //UInt4 ph_r = (UInt4)( *(data+7) )
    //    + 256 * ( (UInt4( *(data+6) ) )-(k*16) );
    UInt4 k=(UInt4)( *(data+6) >> 4 );
    UInt4 ph_l = (UInt4)( *(data+5) <<4 ) + k;
    UInt4 ph_r = (UInt4)( *(data+6) <<8 ) - (k<<12) + (UInt4)( *(data+7) );

    UInt4 ph = (ph_l+ph_r)*_MaxPHA/4096;
    if (psd_No==_XCHPSDNO){
        if ((ph>=LLD_X)&&(ph<=HLD_X)){
            ret.resize(5,0.0);
            ret[0] = tof_clock;
            ret[1] = _XCHPSDNO;
            ret[2] = ph_l;
            ret[3] = ph_r;
            ret[4] = _MaxPHA*ph_l/(ph_l+ph_r)/_binWidth;
            if (ret[4]>512){
                std::string msg = _MessageTag+"Xaxis : ph_l, ph_r, ret[4]= " + _st->UInt4ToString(ph_l);
                msg += ("," + _st->UInt4ToString(ph_r) + "," + _st->UInt4ToString(ret[4]));
                UtsusemiWarning(msg);
                //std::cout << "Xaxis : ph_l, ph_r, ret[4]= "<< ph_l << "," << ph_r << "," <<ret[4]<<std::endl;
            }
        }else{
            ret.clear();
        }
    }else if (psd_No==_YCHPSDNO){
        if ((ph>=LLD_Y)&&(ph<=HLD_Y)){
            ret.resize(5,0.0);
            ret[0] = tof_clock;
            ret[1] = _YCHPSDNO;
            ret[2] = ph_l;
            ret[3] = ph_r;
            ret[4] = _MaxPHA*ph_l/(ph_l+ph_r)/_binWidth;
            if (ret[4]>512){
                std::string msg = _MessageTag+"Xaxis : ph_l, ph_r, ret[4]= " + _st->UInt4ToString(ph_l);
                msg += ("," + _st->UInt4ToString(ph_r) + "," + _st->UInt4ToString(ret[4]));
                UtsusemiWarning(msg);
                //std::cout << "Yaxis : ph_l, ph_r, ret[4]= "<< ph_l << "," << ph_r << "," <<ret[4]<<std::endl;
            }
        }else{
            ret.clear();
        }
    }else{
        ret.clear();
    }
    //std::cout << "DecodeOneEvent fin." << std::endl;
    return ret;
}
//////////////////////////////////////////////////////////
void RPMTDataConverter::
EncodeNeutronEvent( UChar *data, UInt4 TOF_clock, UInt4 X_ch, UInt4 Y_ch ){
    UInt4 modNo = X_ch / 8 + 1;
    UInt4 psdNo = X_ch % 8;

    Double A=1024.0;
    Double B=1.0;
    Double C=1.0;
    // ph = [ A*ph_r/(ph_l + B*ph_r) ] - C
    Double K = (A/((double)(Y_ch)+C)) - B;
    Double PH = 1024.;
    UInt4 ph_l = (UInt4)((PH*K)/(1.0+K));
    //UInt4 ph_r = (UInt4)(PH/(1.0+K));
    UInt4 ph_r = PH-ph_l;


    //data[0]=0x5a;
    data[1]=(UChar)( TOF_clock>>16);
    data[2]=(UChar)( (TOF_clock>>8)&(0xff) );
    data[3]=(UChar)( TOF_clock&(0xff) );
    data[4]=(UChar)( (modNo&(0x1f)<<3) + (psdNo&(0x7)) );
    data[5]=(UChar)( (ph_l&(0xff0))>>4 );
    data[6]=(UChar)( ((ph_l&(0xf))<<4) + (ph_r>>8) );
    data[7]=(UChar)( ph_r&(0xff) );

    UInt4 k_r = (UInt4)( *(data+6) )/16;
    UInt4 ph_l_r = (UInt4)( *(data+5) ) * 16 +  k_r ;
    UInt4 ph_r_r = (UInt4)( *(data+7) )
        + 256 * ( (UInt4( *(data+6) ) )-(k_r*16) );

    if (ph_l!=ph_l_r){
        std::string msg = _MessageTag+"ph_l " + _st->UInt4ToString(ph_l);
        msg += (" != " + _st->UInt4ToString(ph_l_r));
        UtsusemiWarning(msg);
        //std::cout << "Error on ph_l "<< ph_l << " != " << ph_l_r << std::endl;
    }
    if (ph_r!=ph_r_r){
        std::string msg = _MessageTag+"ph_r " + _st->UInt4ToString(ph_r);
        msg += (" != " + _st->UInt4ToString(ph_r_r));
        UtsusemiWarning(msg);
        //std::cout << "Error on ph_r "<< ph_r << " != " << ph_r_r << std::endl;
    }
}
//////////////////////////////////////////////////////////
void RPMTDataConverter::
ReplaceModuleNo( UChar *data, UInt4 modNo ){
    data[2]=(char)( modNo );
}
//////////////////////////////////////////////////////////
void RPMTDataConverter::
SetBinWidth( UInt4 bins ){
    _binWidth = bins;
    _XchOffset = (UInt4)(_numOfLines/_binWidth/2 - (Int4)(_size_Xch/2) - _XchShift);
    _YchOffset = (UInt4)(_numOfLines/_binWidth/2 - (Int4)(_size_Ych/2) - _YchShift);
}
//////////////////////////////////////////////////////////
void RPMTDataConverter::
SetChannelSize( UInt4 x_size, UInt4 y_size ){
    if (x_size==0)
        _size_Xch = 304;
    else
        _size_Xch = x_size;
    if (y_size==0)
        _size_Ych = 304;
    else
        _size_Ych = y_size;

    _XchOffset = (UInt4)(_numOfLines/_binWidth/2 - (Int4)(_size_Xch/2) - _XchShift);
    _YchOffset = (UInt4)(_numOfLines/_binWidth/2 - (Int4)(_size_Ych/2) - _YchShift);
}
//////////////////////////////////////////////////////////
bool RPMTDataConverter::
SetChannelShift(Int4 xadd, Int4 yadd){
    UInt4 X = _numOfLines/_binWidth/2 - (UInt4)(_size_Xch/2);
    UInt4 Y = _numOfLines/_binWidth/2 - (UInt4)(_size_Ych/2);
    if ((X-xadd)<0){
        std::string msg = _MessageTag + "Given X channel shift is too small, must be more than -" + _st->UInt4ToString(X);
        UtsusemiError(msg);
        //std::cout << "Given X channel shift is too small, must be more than -" << X << std::endl;
        return false;
    }
    if ((Y-yadd)<0){
        std::string msg = _MessageTag + "Given Y channel shift is too small, must be more than -" + _st->UInt4ToString(Y);
        UtsusemiError(msg);
        //std::cout << "Given Y channel shift is too small, must be more than -" << Y << std::endl;
        return false;
    }
    _XchShift = xadd;
    _YchShift = yadd;
    _XchOffset = (UInt4)(_numOfLines/_binWidth/2 - (Int4)(_size_Xch/2) - _XchShift);
    _YchOffset = (UInt4)(_numOfLines/_binWidth/2 - (Int4)(_size_Ych/2) - _YchShift);
    return true;
}
//////////////////////////////////////////////////////////
bool RPMTDataConverter::
SetParamsFromAnaEnv(UInt4 runNo, UInt4 modeNo, std::string wfile, std::string dfile){
    UtsusemiAnaEnvironReader AE(runNo,false);
    bool isFullPath=true;
    if (AE._Status){
        std::vector<std::string> pfiles=AE.PutParamFiles( runNo, modeNo, isFullPath );
        if (pfiles.empty()){
            return false;
        }
        if (wfile=="") wfile=pfiles[0];
        if (dfile=="") dfile=pfiles[1];
        SASDetectorInfoEditorRPMT DR;
        if (DR.Read(dfile)){
            _numOfLines = DR.PutNumOfLines();
            _binWidth = DR.PutBinWidth();
            _size_Xch = DR.PutSizeXch();
            _size_Ych = DR.PutSizeYch();
            _XchShift = DR.PutXchShift();
            _YchShift = DR.PutYchShift();
            std::pair<UInt4,UInt4> tmp = DR.PutPHRangeX();
            _LLD_X = tmp.first;
            _HLD_X = tmp.second;
            tmp = DR.PutPHRangeY();
            _LLD_Y = tmp.first;
            _HLD_Y = tmp.second;
            _XchOffset = (UInt4)(_numOfLines/_binWidth/2 - (Int4)(_size_Xch/2) - _XchShift);
            _YchOffset = (UInt4)(_numOfLines/_binWidth/2 - (Int4)(_size_Ych/2) - _YchShift);
            if (UtsusemiEnvGetDebugMode()){
                std::cout << "new _numOfLines = "<<_numOfLines<<std::endl;
                std::cout << "new _binWidth = "<<_binWidth<<std::endl;
                std::cout << "new _size_Xch = "<<_size_Xch<<std::endl;
                std::cout << "new _size_Ych = "<<_size_Ych<<std::endl;
                std::cout << "new _XchShift = "<<_XchShift<<std::endl;
                std::cout << "new _YchShift = "<<_YchShift<<std::endl;
                std::cout << "new _LLD_X, _HLD_X = "<<_LLD_X<<","<<_HLD_X<<std::endl;
                std::cout << "new _LLD_Y, _HLD_Y = "<<_LLD_Y<<","<<_HLD_Y<<std::endl;
            }
        }
    }else{
        return false;
    }
    return true;
}
//////////////////////////////////////////////////////////
bool RPMTDataConverter::
Execute( std::vector<std::string> orgfiles, std::string outdir, UInt4 runNo, UInt4 LLD_X, UInt4 HLD_X, UInt4 LLD_Y, UInt4 HLD_Y, UInt4 LmtCoin ){
    if (LLD_X==0) LLD_X = _LLD_X;
    if (HLD_X==0) HLD_X = _HLD_X;
    if (LLD_Y==0) LLD_Y = _LLD_Y;
    if (HLD_Y==0) HLD_Y = _HLD_Y;

    UChar *data = new UChar[8];
    std::vector<UInt4> curr_evt;
    std::vector<UInt4> prev_evt(5,0);
    bool wasCoin = false;
    UInt4 num_of_coin = 0;
    UInt4 NumOfMods = _numOfLines/_binWidth/8;
    //FILE *fp_org;
    std::vector<FILE*> fo_list( orgfiles.size() );
    std::vector<FILE*> fp_list( NumOfMods );

    for (UInt4 i=0; i<fo_list.size();i++){
        if (NULL==(fo_list[i]=fopen(orgfiles[i].c_str(),"rb"))){
            UtsusemiError(_MessageTag + "Can not open original data file = " + orgfiles[i]);
            //std::cout << "Can not open original data file = "<<orgfiles[i]<<std::endl;
            return false;
        }
    }

    for (UInt4 i=0; i<NumOfMods;i++){
        char outfile_name_c[31];
        std::snprintf( outfile_name_c, sizeof(outfile_name_c), "SAS%06d_%02d_%03d_000.edb", runNo, _DaqId, i+1 );
        std::string outfile_name( outfile_name_c );
        std::string outfile_fullpath(outdir);
        _addPath( &outfile_fullpath, &outfile_name );
        if ( (fp_list[i]=fopen( outfile_fullpath.c_str(), "wb"))==NULL ){
            UtsusemiError(_MessageTag + "Can not open output data file = " + outfile_fullpath);
            //std::cout << "Can not open output data file = "<<outfile_fullpath<<std::endl;
            return false;
        }
    }

    if (UtsusemiEnvGetDebugMode()){
        std::cout << "_XchOffset="<<_XchOffset<<std::endl;
        std::cout << "_YchOffset="<<_YchOffset<<std::endl;
    }

    UInt4 i;
    UInt4 ind=0;
    UtsusemiMessage("RPMTDataConverter::Execute >> Read " + orgfiles[ind]);
    while(true){
        i=fread( data, 8, 1, fo_list[ind] );
        if (i!=1){
            ind++;
            if (ind==fo_list.size()) break;
            UtsusemiMessage(_MessageTag + "Next " + orgfiles[ind]);
            //std::cout << "Next " << orgfiles[ind] << std::endl;
            i=fread( data, 8, 1, fo_list[ind] );
            if (i!=1) break;
        }

        if (*data == _HeaderNeutronEvent){
            curr_evt = DecodeOneEvent( data, LLD_X, HLD_X, LLD_Y, HLD_Y );
            UInt4 X_ch, Y_ch;
            //Double xch1, ych1; //for rotation but ignored
            UInt4 TOF_clock = 0;
            if (!(curr_evt.empty())){
                if (wasCoin){
                    wasCoin = false;
                    for (UInt4 j=0; j<5; j++) prev_evt[j]=curr_evt[j];
                }else{
                    if (curr_evt[1]!=prev_evt[1]){
                        UInt4 diff_time = abs( Int4(curr_evt[0] - prev_evt[0]) );
                        if ((diff_time!=0)&&(diff_time<LmtCoin)){
                            if (curr_evt[1]==0){
                                X_ch = prev_evt[4];
                                Y_ch = curr_evt[4];
                                //for rotation but ignored
                                //xch1 = prev_evt[4];
                                //ych1 = curr_evt[4];
                            }else{
                                X_ch = curr_evt[4];
                                Y_ch = prev_evt[4];
                                //for rotation but ignored
                                //xch1 = curr_evt[4];
                                //ych1 = prev_evt[4];
                            }
                            TOF_clock=(UInt4)((curr_evt[0]+prev_evt[0])/2);

                            wasCoin = true;
                            num_of_coin++;
                        }
                    }
                    if (wasCoin){
                        // Output Event data
                        if ((X_ch>=_XchOffset)&&(Y_ch>=_YchOffset)){
                            X_ch -= _XchOffset;
                            Y_ch -= _YchOffset;
                            EncodeNeutronEvent( data, TOF_clock, X_ch, Y_ch );
                            UInt4 modNo = X_ch/8 + 1;
                            if (modNo<fp_list.size())
                                std::fwrite( data,1 ,8 , fp_list[modNo] );
                        }
                    }else
                        for (UInt4 j=0; j<5; j++) prev_evt[j]=curr_evt[j];
                }
            }
        }else if (*data == _HeaderT0Event){
            // Output T0Event Directly
            for (UInt4 j=0; j<NumOfMods; j++){
                ReplaceModuleNo( data, j+1 );
                std::fwrite( data,1 ,8 , fp_list[j] );
            }
        }else if (*data == _HeaderClockEvent){
            // Output ClockEvent Directly
            for (UInt4 j=0; j<NumOfMods; j++)
                std::fwrite( data,1 ,8 , fp_list[j] );
        }
    }
    //fclose(fp_org);
    for (UInt4 i=0; i<fo_list.size();i++) fclose( fo_list[i] );
    for (UInt4 j=0; j<NumOfMods; j++)
        fclose(fp_list[j]);

    return true;
}
//////////////////////////////////////////////////////////
bool RPMTDataConverter::
PutPulseHeight(ElementContainer* ecX, ElementContainer* ecY, std::vector<std::string> orgfiles, UInt4 width_bin, UInt4 max_bin, UInt4 LmtCoin){
    UChar *data = new UChar[8];
    std::vector<UInt4> curr_evt;
    std::vector<UInt4> prev_evt(5,0);
    bool wasCoin = false;
    UInt4 num_of_coin = 0;

    std::vector<FILE*> fo_list( orgfiles.size() );

    for (UInt4 i=0; i<fo_list.size();i++){
        if (NULL==(fo_list[i]=fopen(orgfiles[i].c_str(),"rb"))){
            UtsusemiError("Can not open original data file = "+orgfiles[i]);
            return false;
        }
    }

    UInt4 i;
    UInt4 ind=0;
    UtsusemiMessage( "Read " + orgfiles[ind] );
    UInt4 num_bin = int(max_bin/width_bin);
    std::vector<Double> ph_X_vect(num_bin,0.0);
    std::vector<Double> ph_Y_vect(num_bin,0.0);
    std::vector<Double> ph_bin(num_bin+1,0.0);
    for (UInt4 i=0; i<num_bin+1; i++)
        ph_bin[i] = -0.5*Double(width_bin) + Double(i*width_bin);
    while(true){
        i=fread( data, 8, 1, fo_list[ind] );
        if (i!=1){
            ind++;
            if (ind==fo_list.size()) break;
            UtsusemiMessage( "Next " + orgfiles[ind] );
            i=fread( data, 8, 1, fo_list[ind] );
            if (i!=1) break;
        }

        if (*data == _HeaderNeutronEvent){
            curr_evt = DecodeOneEvent( data, 0, 100000, 0, 100000 );
            UInt4 X_ch, Y_ch;
            UInt4 TOF_clock = 0;
            if (!(curr_evt.empty())){
                if (wasCoin){
                    wasCoin = false;
                    for (UInt4 j=0; j<5; j++) prev_evt[j]=curr_evt[j];
                }else{
                    Double ph_X=0.0;
                    Double ph_Y=0.0;
                    if (curr_evt[1]!=prev_evt[1]){
                        UInt4 diff_time = abs( Int4(curr_evt[0] - prev_evt[0]) );
                        if ((diff_time!=0)&&(diff_time<LmtCoin)){
                            if (curr_evt[1]==0){
                                ph_X = (prev_evt[2]+prev_evt[3])*_MaxPHA/4096;
                                ph_Y = (curr_evt[2]+curr_evt[3])*_MaxPHA/4096;
                            }else{
                                ph_Y = (prev_evt[2]+prev_evt[3])*_MaxPHA/4096;
                                ph_X = (curr_evt[2]+curr_evt[3])*_MaxPHA/4096;
                            }
                            wasCoin = true;
                            num_of_coin++;
                        }
                    }
                    if (wasCoin){
                        UInt4 i_x = ph_X/Double(width_bin);
                        UInt4 i_y = ph_Y/Double(width_bin);
                        ph_X_vect[i_x] += 1.0;
                        ph_Y_vect[i_y] += 1.0;
                    }else
                        for (UInt4 j=0; j<5; j++) prev_evt[j]=curr_evt[j];
                }
            }
        }
    }
    // Make Histogram
    ecX->Add("PulseHeight", ph_bin, "Channel");
    ecX->Add(UTSUSEMI_KEY_INTENSITY, ph_X_vect, UTSUSEMI_KEY_COUNTS_UNIT);
    std::vector<Double> errX( ph_X_vect.size(), 0.0 );
    for (UInt4 i=0;i<ph_X_vect.size(); i++)
        errX[i] = sqrt(ph_X_vect[i]);
    ecX->Add(UTSUSEMI_KEY_ERROR, errX, UTSUSEMI_KEY_COUNTS_UNIT);
    ecX->SetKeys("PulseHeight",UTSUSEMI_KEY_INTENSITY,UTSUSEMI_KEY_ERROR);

    ecY->Add("PulseHeight", ph_bin, "Channel");
    ecY->Add(UTSUSEMI_KEY_INTENSITY, ph_Y_vect, UTSUSEMI_KEY_COUNTS_UNIT);
    std::vector<Double> errY( ph_Y_vect.size(), 0.0 );
    for (UInt4 i=0;i<ph_Y_vect.size(); i++)
        errY[i] = sqrt(ph_Y_vect[i]);
    ecY->Add(UTSUSEMI_KEY_ERROR, errY, UTSUSEMI_KEY_COUNTS_UNIT);
    ecY->SetKeys("PulseHeight",UTSUSEMI_KEY_INTENSITY,UTSUSEMI_KEY_ERROR);

    return true;
}
