#include "UtsusemiOneTrignetStorage.hh"
//////////////////////////////////////////////////////////
UtsusemiOneCase::
UtsusemiOneCase(){
    TOF = 0;
    Cases.clear();
}
//////////////////////////////////////////////////////////
UtsusemiOneCase::
UtsusemiOneCase(UInt4 _tof, std::vector<UInt4>* _cases){
    TOF = _tof;
    Cases.clear();
    Cases.resize( _cases->size() );
    copy( _cases->begin(), _cases->end(), Cases.begin() );
}
//////////////////////////////////////////////////////////
UtsusemiOneCase::
UtsusemiOneCase( const UtsusemiOneCase& obj ){
    TOF = obj.TOF;
    Cases.clear();
    Cases.resize( obj.Cases.size() );
    copy( obj.Cases.begin(), obj.Cases.end(), Cases.begin() );
}
//////////////////////////////////////////////////////////
UtsusemiOneCase::
~UtsusemiOneCase(){

}
//////////////////////////////////////////////////////////
void UtsusemiOneCase::
SetCases( std::vector<UInt4> _cases ){
    Cases.clear();
    Cases.resize( _cases.size() );
    copy( _cases.begin(), _cases.end(), Cases.begin() );
}


//////////////////////////////////////////////////////////
UtsusemiOneTrignetStorage::
UtsusemiOneTrignetStorage( UInt4 _num_of_multh, UInt4 _dio, UInt4 _adc1, UInt4 _adc2, UInt4 _count ){

    Initialize(_num_of_multh, _dio, _adc1, _adc2, _count);
}

//////////////////////////////////////////////////////////
UtsusemiOneTrignetStorage::
~UtsusemiOneTrignetStorage(){

}

//////////////////////////////////////////////////////////
void UtsusemiOneTrignetStorage::
Initialize( UInt4 _num_of_multh, UInt4 _dio, UInt4 _adc1, UInt4 _adc2, UInt4 _count){
    _NumOfMulTh = _num_of_multh;
    _firstDIOcase = _dio;
    _firstADC1case = _adc1;
    _firstADC2case = _adc2;
    _firstCOUNTcase = _count;

    _DIOCaseTableMulTh = NULL;
    _ADC1CaseTableMulTh = NULL;
    _ADC2CaseTableMulTh = NULL;
    _COUNTCaseTableMulTh = NULL;
    _DIOCaseTable = NULL;
    _ADC1CaseTable = NULL;
    _ADC2CaseTable = NULL;
    _COUNTCaseTable = NULL;

    _FrameIndex = 0;
    _PulseIdTableMulTh.clear();
    _PulseIdTableMulThIndex.clear();
    _InstClockTableMulTh.clear();

    PulseIdTable.clear();
    InstClockTable.clear();

    _isUsedFilterCase = false;
    _isUsedCounterCase = false;

    _isDebug = false;
}

//////////////////////////////////////////////////////////
void UtsusemiOneTrignetStorage::
ClearTable( std::vector< std::vector< UtsusemiOneCase* >* >* _table ){
    if (_table!=NULL){
        for (UInt4 i=0; i<_table->size(); i++)
            if (_table->at(i)!=NULL){
                for (UInt4 j=0; j<_table->at(i)->size(); j++){
                    if (_table->at(i)->at(j)!=NULL){
                        delete _table->at(i)->at(j);
                    }
                }
                delete _table->at(i);
            }
        delete _table;
        //_table = NULL;
    }
}
//////////////////////////////////////////////////////////
void UtsusemiOneTrignetStorage::
ClearTableMulTh( std::vector< std::vector< std::vector< UtsusemiOneCase* >* >* >* _table ){
    if (_table!=NULL){
        for (UInt4 i=0; i<_table->size(); i++){
            if (_table->at(i)!=NULL){
                for (UInt4 j=0; j<_table->at(i)->size(); j++){
                    if (_table->at(i)->at(j)!=NULL){
                        for (UInt4 k=0; k<_table->at(i)->at(j)->size(); k++){
                            if (_table->at(i)->at(j)->at(k)!=NULL){
                                delete _table->at(i)->at(j)->at(k);
                            }
                        }
                        delete _table->at(i)->at(j);
                    }
                }
                delete _table->at(i);
            }
        }
        delete _table;
    }
}
//////////////////////////////////////////////////////////
void UtsusemiOneTrignetStorage::
ClearTableMulThAll(){
    if (_isDebug) std::cout << "## ClearTableMulThAll start" << std::endl;
    ClearTableMulTh( _DIOCaseTableMulTh );
    _DIOCaseTableMulTh=NULL;
    if (_isDebug) std::cout << "## ClearTableMulThAll Clear DIO fin" << std::endl;
    ClearTableMulTh( _ADC1CaseTableMulTh );
    _ADC1CaseTableMulTh=NULL;
    if (_isDebug) std::cout << "## ClearTableMulThAll Clear ADC1 fin" << std::endl;
    ClearTableMulTh( _ADC2CaseTableMulTh );
    _ADC2CaseTableMulTh=NULL;
    if (_isDebug) std::cout << "## ClearTableMulThAll Clear ADC2 fin" << std::endl;
    ClearTableMulTh( _COUNTCaseTableMulTh );
    _COUNTCaseTableMulTh=NULL;
    if (_isDebug) std::cout << "## ClearTableMulThAll Clear COUNT fin" << std::endl;
    _PulseIdTableMulTh.clear();
    _InstClockTableMulTh.clear();
}
//////////////////////////////////////////////////////////
void UtsusemiOneTrignetStorage::
Clear(){
    ClearTableMulThAll();
    ClearTable( _DIOCaseTable );
    _DIOCaseTable=NULL;
    ClearTable( _ADC1CaseTable );
    _ADC1CaseTable=NULL;
    ClearTable( _ADC2CaseTable );
    _ADC2CaseTable=NULL;
    ClearTable( _COUNTCaseTable );
    _COUNTCaseTable=NULL;
    PulseIdTable.clear();
    InstClockTable.clear();
}

//////////////////////////////////////////////////////////
bool UtsusemiOneTrignetStorage::
AddFilterCaseSort( UInt4 _multh, UInt4 _sid2, UInt4 _tof, std::vector<UInt4> _cases ){
#ifdef DEBUG_UTSUSEMI
    std::cout << "AddFilterCaseSort >> multh,sid2,tof,case="<<_multh<<",  "<<_sid2<<", "<<_tof<<",  :";
    for (UInt4 i=0;i<_cases.size();i++) std::cout <<_cases[i]<< "," << std::endl;
    std::cout << std::endl;
#endif
    if (_multh>=_NumOfMulTh){
        return false;
    }
    UtsusemiOneCase *tmp = new UtsusemiOneCase( _tof, &_cases );

    if (_sid2==1){
        _DIOCaseTableMulTh->at(_multh)->back()->push_back(tmp);
    }else if ((_sid2==2)||(_sid2==4)){
        _ADC1CaseTableMulTh->at(_multh)->back()->push_back(tmp);
    }else if (_sid2==3){
        _ADC2CaseTableMulTh->at(_multh)->back()->push_back(tmp);
    }else{
        delete tmp;
        return false;
    }
    _isUsedFilterCase = true;
    return true;
}
//////////////////////////////////////////////////////////
bool UtsusemiOneTrignetStorage::
AddCounterCaseSort( UInt4 _multh, UInt4 _tof, UInt4 _case ){
    if (_multh>=_NumOfMulTh){
        return false;
    }
#ifdef DEBUG_UTSUSEMI
    std::cout << "AddCounterCaseSort >> tof,case="<<_tof<<","<<_case<<std::endl;
#endif
    std::vector<UInt4> *_cases= new std::vector<UInt4>(1, _case );
    UtsusemiOneCase *tmp = new UtsusemiOneCase( _tof, _cases );
    delete _cases;

    _COUNTCaseTableMulTh->at(_multh)->back()->push_back(tmp);
    _isUsedCounterCase = true;
    return true;
}
//////////////////////////////////////////////////////////
void UtsusemiOneTrignetStorage::
StartFrame(){
    Clear();//[inamura 151013]
    if (_DIOCaseTableMulTh==NULL) _DIOCaseTableMulTh = new std::vector< std::vector< std::vector< UtsusemiOneCase* >* >* >(_NumOfMulTh, NULL);
    if (_ADC1CaseTableMulTh==NULL) _ADC1CaseTableMulTh = new std::vector< std::vector< std::vector< UtsusemiOneCase* >* >* >(_NumOfMulTh, NULL);
    if (_ADC2CaseTableMulTh==NULL) _ADC2CaseTableMulTh = new std::vector< std::vector< std::vector< UtsusemiOneCase* >* >* >(_NumOfMulTh, NULL);
    if (_COUNTCaseTableMulTh==NULL) _COUNTCaseTableMulTh = new std::vector< std::vector< std::vector< UtsusemiOneCase* >* >* >(_NumOfMulTh, NULL);

    _PulseIdTableMulTh.resize(_NumOfMulTh);
    _InstClockTableMulTh.resize(_NumOfMulTh);

}
//////////////////////////////////////////////////////////
void UtsusemiOneTrignetStorage::
StartAFrame(UInt4 _multh){
    if (_multh>=_NumOfMulTh){
        std::cout << "## _multh is larger" << std::endl;
        return;
    }
    /*
    if (_DIOCaseTableMulTh==NULL) _DIOCaseTableMulTh = new std::vector< std::vector< std::vector< std::pair<UInt4,UInt4>* >* >* >(_NumOfMulTh, NULL);
    if (_ADC1CaseTableMulTh==NULL) _ADC1CaseTableMulTh = new std::vector< std::vector< std::vector< std::pair<UInt4,UInt4>* >* >* >(_NumOfMulTh, NULL);
    if (_ADC2CaseTableMulTh==NULL) _ADC2CaseTableMulTh = new std::vector< std::vector< std::vector< std::pair<UInt4,UInt4>* >* >* >(_NumOfMulTh, NULL);
    if (_COUNTCaseTableMulTh==NULL) _COUNTCaseTableMulTh = new std::vector< std::vector< std::vector< std::pair<UInt4,UInt4>* >* >* >(_NumOfMulTh, NULL);

    _PulseIdTableMulTh.resize(_NumOfMulTh);
    _InstClockTableMulTh.resize(_NumOfMulTh);
    */
    if (_DIOCaseTableMulTh->at(_multh)==NULL) _DIOCaseTableMulTh->at(_multh)=new std::vector< std::vector< UtsusemiOneCase* >* >;
    std::vector< UtsusemiOneCase* > *tmp_dio=new std::vector< UtsusemiOneCase* >;
    _DIOCaseTableMulTh->at(_multh)->push_back( tmp_dio );

    if (_ADC1CaseTableMulTh->at(_multh)==NULL) _ADC1CaseTableMulTh->at(_multh)=new std::vector< std::vector< UtsusemiOneCase* >* >();
    std::vector< UtsusemiOneCase* > *tmp_adc1=new std::vector< UtsusemiOneCase* >();
    _ADC1CaseTableMulTh->at(_multh)->push_back( tmp_adc1 );

    if (_ADC2CaseTableMulTh->at(_multh)==NULL) _ADC2CaseTableMulTh->at(_multh)=new std::vector< std::vector< UtsusemiOneCase* >* >();
    std::vector< UtsusemiOneCase* > *tmp_adc2=new std::vector< UtsusemiOneCase* >();
    _ADC2CaseTableMulTh->at(_multh)->push_back( tmp_adc2 );

    if (_COUNTCaseTableMulTh->at(_multh)==NULL) _COUNTCaseTableMulTh->at(_multh)=new std::vector< std::vector< UtsusemiOneCase* >* >();
    std::vector< UtsusemiOneCase* > *tmp_count=new std::vector< UtsusemiOneCase* >();
    _COUNTCaseTableMulTh->at(_multh)->push_back( tmp_count );

}
//////////////////////////////////////////////////////////
bool UtsusemiOneTrignetStorage::
AddPulseId( UInt4 _multh, UInt4 _pulseId, Double _instClock ){
    if (_multh>=_NumOfMulTh) return false;
#ifdef DEBUG_UTSUSEMI
    std::cout << "AddPulseId "<<_multh<<","<<_pulseId<<","<<_instClock<<std::endl;
#endif
    _PulseIdTableMulTh[_multh].push_back(_pulseId);
    _InstClockTableMulTh[_multh].push_back(_instClock);

    return true;
}
//////////////////////////////////////////////////////////
std::vector<UInt8> UtsusemiOneTrignetStorage::
SortPulseIdIndex(){
#ifdef DEBUG_UTSUSEMI
    std::cout << "## SortPulseIdIndex start" << std::endl;
#endif
    UInt4 num_of_index = 0;
    for (UInt4 i=0; i<_NumOfMulTh; i++) num_of_index += (UInt4)(_PulseIdTableMulTh[i].size());
    _PulseIdTableMulThIndex.clear();
    _PulseIdTableMulThIndex.resize(num_of_index,0);
    std::vector<UInt8> ret_pulseId(num_of_index,0);
    std::vector<UInt4> mi(_NumOfMulTh,0);
    std::vector<UInt4> mf(_NumOfMulTh,true);
    UInt4 ind = 0;
    bool isContinue=true;
    for (UInt4 i=0; i<_NumOfMulTh; i++)
        if (_PulseIdTableMulTh[i].empty()) mf[i]=false;
#ifdef DEBUG_UTSUSEMI
    std::cout << "## SortPulseIdIndex while loop" << std::endl;
#endif
    while(isContinue){
        UInt4 min_i = 0;
        UInt8 min_pulseId = 0;
        bool isFirstSet = true;
        for (UInt4 i=0; i<_NumOfMulTh; i++)
            if (mf[i]){
#ifdef DEBUG_UTSUSEMI
                //std::cout << "## multh,_PulseIdTableMulTh.size(),mi.size()="<<i<<","<<_PulseIdTableMulTh.size()<<","<<mi.size()<<std::endl;
                //std::cout << "## mi[i]/_PulseIdTableMulTh[i].size()="<<mi[i]<<"/"<< _PulseIdTableMulTh[i].size()<<std::endl;
#endif
                if (isFirstSet){
                    min_pulseId=_PulseIdTableMulTh[i][mi[i]];
                    isFirstSet = false;
                    min_i = i;
                }

                if (min_pulseId>=_PulseIdTableMulTh[i][mi[i]]){
                    min_pulseId = _PulseIdTableMulTh[i][mi[i]];
                    min_i = i;
                }
            }

        _PulseIdTableMulThIndex[ind]=min_i;
        ret_pulseId[ind] = min_pulseId;
        if (mi[min_i]==(_PulseIdTableMulTh[min_i].size()-1)) mf[min_i]=false;
        else mi[min_i]++;

        isContinue = false;
        for (UInt4 i=0; i<_NumOfMulTh; i++)
            if (mf[i]){
                isContinue = true;
                break;
            }

        if (isContinue){
            ind++;
            if (num_of_index==ind){
                UtsusemiError("UtsusemiOneTrignetStorage::SortPulseIdIndex > index becomes over : something wrong.");
                break;
            }
        }

    }
#ifdef DEBUG_UTSUSEMI
    for (UInt4 i=0; i<ret_pulseId.size(); i++)
        std::cout << "i,_PulseIdTableMulThIndex,pulseId="<<i<<","<<_PulseIdTableMulThIndex[i]<<","<<ret_pulseId[i]<<std::endl;
#endif
    return ret_pulseId;
}
//////////////////////////////////////////////////////////
void UtsusemiOneTrignetStorage::
_MergeTableOnMulTh( std::vector< std::vector< std::vector< UtsusemiOneCase* >* >* >* _table1, std::vector< std::vector< UtsusemiOneCase* >* >* _table2, UInt4 &_firstCase ){
    if (_table1==NULL) {
        UtsusemiError( "UtsusemiOneTrignetStorage::_MergeTableOnMulTh > table1 is empty." );
        return;
    }
    if (_table2==NULL) {
        UtsusemiError( "UtsusemiOneTrignetStorage::_MergeTableOnMulTh > table2 is empty." );
        return;
    }
    if (_PulseIdTableMulThIndex.empty()){
        UtsusemiError( "UtsusemiOneTrignetStorage::_MergeTableOnMulTh > _PulseIdTableMulThIndex is empty." );
        return;
    }


    std::vector<UInt4> mi(_NumOfMulTh,0);
    _table2->resize( _PulseIdTableMulThIndex.size(), NULL );
    //UInt4 pre_case = 0;
    std::vector<UInt4> pre_case;

    for (UInt4 i=0; i<_PulseIdTableMulThIndex.size(); i++){
        std::vector< UtsusemiOneCase* >* tmp = _table1->at( _PulseIdTableMulThIndex[i] )->at( mi[_PulseIdTableMulThIndex[i]] );
        bool isFirst = true;

        _table2->at(i) = new std::vector< UtsusemiOneCase* >;
        for (UInt4 j=0; j<tmp->size(); j++){
#ifdef DEBUG_UTSUSEMI
            //std::cout << "_MergeTableOnMulTh, tmp.first,tmp.second="<< tmp->at(j)->at(0) << "," << tmp->at(j)->at(1);
#endif
            UtsusemiOneCase* tmp_pair = new UtsusemiOneCase( *(tmp->at(j)) );

            if (isFirst){
                _table2->at(i)->push_back(tmp_pair);
                pre_case.clear();
                pre_case.resize( tmp_pair->Cases.size() );
                sort( tmp_pair->Cases.begin(),tmp_pair->Cases.end() );
                copy( tmp_pair->Cases.begin(), tmp_pair->Cases.end(), pre_case.begin() );
#ifdef DEBUG_UTSUSEMI
                std::cout << "----------> Added ";
#endif
                isFirst = false;
            }else{
                if (equal(tmp_pair->Cases.begin(), tmp_pair->Cases.end(), pre_case.begin())){
                    delete tmp_pair;
                }else{
                    _table2->at(i)->push_back(tmp_pair);
                    pre_case.clear();
                    pre_case.resize( tmp_pair->Cases.size() );
                    sort( tmp_pair->Cases.begin(),tmp_pair->Cases.end() );
                    copy( tmp_pair->Cases.begin(), tmp_pair->Cases.end(), pre_case.begin() );

#ifdef DEBUG_UTSUSEMI
                    std::cout << "----------> Added ";
#endif
                }
            }
#ifdef DEBUG_UTSUSEMI
            std::cout << std::endl;
#endif
        }
        mi[_PulseIdTableMulThIndex[i]]++;
    }
}
//////////////////////////////////////////////////////////
bool UtsusemiOneTrignetStorage::
MergeOnMulTh(){
    PulseIdTable.clear();
    PulseIdTable = SortPulseIdIndex();

    InstClockTable.clear();
    InstClockTable.resize(_PulseIdTableMulThIndex.size(),0.0);
    std::vector<UInt4> ind_instclk(_NumOfMulTh,0);
    for (UInt4 i=0; i<_PulseIdTableMulThIndex.size(); i++){
        UInt4 ind = _PulseIdTableMulThIndex[i];
        InstClockTable[i]=_InstClockTableMulTh[ ind ][ ind_instclk[ ind ] ];
        ind_instclk[ ind ]++;
    }

    if (_isDebug) std::cout << "----Merge DIO" << std::endl;
    if (_DIOCaseTable==NULL) _DIOCaseTable = new std::vector< std::vector< UtsusemiOneCase* >* >;
    //std::cout << "----------------- _DIOCaseTable->size()=" << _DIOCaseTable->size() << std::endl;
    _MergeTableOnMulTh( _DIOCaseTableMulTh, _DIOCaseTable, _firstDIOcase );
    if (_isDebug) std::cout << "----Merge ADC1" << std::endl;
    if (_ADC1CaseTable==NULL) _ADC1CaseTable = new std::vector< std::vector< UtsusemiOneCase* >* >;
    _MergeTableOnMulTh( _ADC1CaseTableMulTh, _ADC1CaseTable, _firstADC1case );
    if (_isDebug) std::cout << "----Merge ADC2" << std::endl;
    if (_ADC2CaseTable==NULL) _ADC2CaseTable = new std::vector< std::vector< UtsusemiOneCase* >* >;
    _MergeTableOnMulTh( _ADC2CaseTableMulTh, _ADC2CaseTable, _firstADC2case );
    if (_isDebug) std::cout << "----Merge COUNT" << std::endl;
    if (_COUNTCaseTable==NULL) _COUNTCaseTable = new std::vector< std::vector< UtsusemiOneCase* >* >;
    _MergeTableOnMulTh( _COUNTCaseTableMulTh, _COUNTCaseTable, _firstCOUNTcase );

    if (_isDebug) std::cout << "----ClearTableMulThAll" << std::endl;

    ClearTableMulThAll();

#ifdef DEBUG_UTSUSEMI
        std::cout << "### After AddPulse " << std::endl;
        std::cout << "_DIOCaseTable->size()=" << _DIOCaseTable->size() << std::endl;
        std::cout << "_ADC1CaseTable->size()=" << _ADC1CaseTable->size() << std::endl;
        std::cout << "_ADC2CseTable->size()=" << _ADC2CaseTable->size() << std::endl;
        std::cout << "_COUNTCaseTable->size()=" << _COUNTCaseTable->size() << std::endl;

        for (UInt4 i=0; i<(_DIOCaseTable->size()); i++){
            std::cout << "-----------------------------------------------"<<std::endl;
            std::cout << "frame no=" << i << std::endl;
            std::cout << "_DIOCaseTable case=";
            if ((_DIOCaseTable!=NULL)&&(_DIOCaseTable->at(i)!=NULL))
                for (UInt4 j=0; j<(_DIOCaseTable->at(i)->size()); j++){
                    std::cout << ",[" << _DIOCaseTable->at(i)->at(j)->TOF << ",";
                    for (UInt4 k=0; k<(_DIOCaseTable->at(i)->at(j)->Cases.size()); k++)
                        std::cout << _DIOCaseTable->at(i)->at(j)->Cases[k] << ",";
                    std::cout << "] " << std::endl;
                }
            std::cout << std::endl;
            std::cout << "_ADC1CaseTable case=";
            if ((_ADC1CaseTable!=NULL)&&(_ADC1CaseTable->at(i)!=NULL))
                for (UInt4 j=0; j<(_ADC1CaseTable->at(i)->size()); j++){
                    std::cout << ",[" << _ADC1CaseTable->at(i)->at(j)->TOF << ",";
                    for (UInt4 k=0; k<(_ADC1CaseTable->at(i)->at(j)->Cases.size()); k++)
                        std::cout << _ADC1CaseTable->at(i)->at(j)->Cases[k] << ",";
                    std::cout << "] " << std::endl;
                }
            std::cout << std::endl;
            std::cout << "_ADC2CaseTable case=";
            if ((_ADC2CaseTable!=NULL)&&(_ADC2CaseTable->at(i)!=NULL))
                for (UInt4 j=0; j<(_ADC2CaseTable->at(i)->size()); j++){
                    std::cout << ",[" << _ADC2CaseTable->at(i)->at(j)->TOF << ",";
                    for (UInt4 k=0; k<(_ADC2CaseTable->at(i)->at(j)->Cases.size()); k++)
                        std::cout << _ADC2CaseTable->at(i)->at(j)->Cases[k] << ",";
                    std::cout << "] " << std::endl;
                }
            std::cout << std::endl;
            std::cout << "_COUNTCaseTable case=";
            if ((_COUNTCaseTable!=NULL)&&(_COUNTCaseTable->at(i)!=NULL))
                for (UInt4 j=0; j<(_COUNTCaseTable->at(i)->size()); j++){
                    std::cout << ",[" << _COUNTCaseTable->at(i)->at(j)->TOF << ",";
                    for (UInt4 k=0; k<(_COUNTCaseTable->at(i)->at(j)->Cases.size()); k++)
                        std::cout << _COUNTCaseTable->at(i)->at(j)->Cases[k] << ",";
                    std::cout << "] " << std::endl;
                }
            std::cout << std::endl;
            std::cout << "----" << std::endl;
            std::cout << "PulseIdTable.size(), InstClockTable.size()="<<PulseIdTable.size()<<","<< InstClockTable.size()<<std::endl;
        }
        std::cout << "fin" << std::endl;
#endif

    return true;
}

//////////////////////////////////////////////////////////
bool UtsusemiOneTrignetStorage::
Merge( std::vector< std::vector<UInt4>* >* _mergedTable, std::vector<bool> isAndFilter, UInt4 _numOfCounterCase, UInt4 _preCase ){
#ifdef DEBUG_UTSUSEMI
    std::cout << "## Merge start" << std::endl;
#endif
    if (_mergedTable==NULL) return false;
    if (_DIOCaseTable==NULL) {
        UtsusemiError("UtsusemiOneTrignetStoragi::Merge > _DIOCaseTable=NULL");
    }
    UInt4 num_of_frame = (UInt4)(_DIOCaseTable->size());

    if (_mergedTable!=NULL){
        for (UInt4 i=0; i<(_mergedTable->size()); i++)
            if (_mergedTable->at(i)!=NULL) delete _mergedTable->at(i);
        _mergedTable->clear();
    }

    UInt4 pre_case = _preCase;

    UInt4 pre_filter = 0;
    UInt4 pre_counter = 0;
    UInt4 pre_merged_case = _preCase;

    UtsusemiOneCase empty_case;
    empty_case.TOF=0;
    empty_case.Cases.clear();
    std::vector< UtsusemiOneCase > last_case(3,empty_case),curr_case(3,empty_case);
    curr_case[0].Cases.push_back( _firstDIOcase );
    curr_case[1].Cases.push_back( _firstADC1case);
    curr_case[2].Cases.push_back( _firstADC2case);
    UInt4 max_tof = 2000000000;
    std::vector< std::vector< UtsusemiOneCase* >* > TableList;
    for (UInt4 frm=0; frm< num_of_frame; frm++){
#ifdef DEBUG_UTSUSEMI
        std::cout << "## Merge frame loop start frm/num_of_frame="<< frm <<"/"<< num_of_frame<< std::endl;
#endif

        std::vector< UtsusemiOneCase* > *tmp_table = new std::vector< UtsusemiOneCase* >();

        if (_isUsedFilterCase){
            // Current TOF must be reset at start of a frame
            curr_case[0].TOF = 0;
            curr_case[1].TOF = 0;
            curr_case[2].TOF = 0;

            // Merge Filter (DIO, ADC1 and ADC2)
            TableList.clear();
            if ((_DIOCaseTable->empty())||(_DIOCaseTable->at(frm)==NULL)||(_DIOCaseTable->at(frm)->empty())){
            }else {
#ifdef DEBUG_UTSUSEMI
                std::cout << "### DIO case tables are used ,size="<<_DIOCaseTable->at(frm)->size() <<std::endl;
#endif
                TableList.push_back( _DIOCaseTable->at(frm) );
            }
            if ((_ADC1CaseTable->empty())||(_ADC1CaseTable->at(frm)==NULL)||(_ADC1CaseTable->at(frm)->empty())){
            }else{
#ifdef DEBUG_UTSUSEMI
                std::cout << "### ADC1 case tables are used, size="<< _ADC1CaseTable->at(frm)->size() <<std::endl;
#endif
                TableList.push_back( _ADC1CaseTable->at(frm) );
            }
            if ((_ADC2CaseTable->empty())||(_ADC2CaseTable->at(frm)==NULL)||(_ADC2CaseTable->at(frm)->empty())){
            }else {
#ifdef DEBUG_UTSUSEMI
                std::cout << "### ADC2 case tables are used, size="<< _ADC1CaseTable->at(frm)->size() <<std::endl;
#endif
                TableList.push_back( _ADC2CaseTable->at(frm) );
            }
            UInt4 num_of_table = (UInt4)(TableList.size());
            std::vector<UInt4> i_TableList(num_of_table,0);
            std::vector<bool> f_TableList(num_of_table,true);


            UtsusemiOneCase *first_case_in_frame = new UtsusemiOneCase();
            first_case_in_frame->TOF = 0;
            first_case_in_frame->Cases.push_back(pre_case);
            tmp_table->push_back( first_case_in_frame );

            bool isContinue = true;
            if (num_of_table==0) isContinue = false;
            while( isContinue ){
                // Get latest case inf and Searching minimum tof
                //std::cout << "## Merge frame while step 0" << std::endl;
                UInt4 i_min = num_of_table+1;
                UInt4 tof_min = max_tof;
#ifdef DEBUG_UTSUSEMI
                    std::cout << "i_tableList= ";
                    for (UInt4 i=0;i<num_of_table;i++) std::cout << i_TableList[i] << ",";
                    std::cout << std::endl;
#endif
                for (UInt4 i=0; i<num_of_table; i++){
                    last_case[i].TOF = max_tof;
                    last_case[i].SetCases( TableList[i]->at( i_TableList[i] )->Cases );

                    if (f_TableList[i]){
                        last_case[i].TOF = TableList[i]->at( i_TableList[i] )->TOF;
                        if (tof_min>(last_case[i].TOF)){
                            i_min = i;
                            tof_min = last_case[i].TOF;
                        }
                    }
                }
                if (i_min==num_of_table+1){
                    UtsusemiError("UtsusemiOneTrignetStorage::Merge > Something Wrong searching minimum tof");
                    return false;
                }

                curr_case[i_min] = last_case[i_min];

#ifdef DEBUG_UTSUSEMI
                    std::cout << "## Merge frame while step 1 : i_min="<<i_min << std::endl;
                    std::cout << "last_case =[ " << last_case[0].TOF <<":";
                    for (UInt4 k=0; k< last_case[0].Cases.size(); k++) std::cout << last_case[0].Cases[k] << ",";
                    std::cout << "] [ " << last_case[1].TOF << ":";
                    for (UInt4 k=0; k< last_case[1].Cases.size(); k++) std::cout << last_case[1].Cases[k] << ",";
                    std::cout << "] [ " << last_case[2].TOF << ":";
                    for (UInt4 k=0; k< last_case[2].Cases.size(); k++) std::cout << last_case[2].Cases[k] << ",";
                    std::cout <<  "]  ==> ";
                    std::cout << " found min tof = " << last_case[i_min].TOF << std::endl;
                    std::cout << "curr_case =[ " << curr_case[0].TOF << ":";
                    for (UInt4 k=0; k< curr_case[0].Cases.size(); k++) std::cout << curr_case[0].Cases[k] << ",";
                    std::cout << "] [ " << curr_case[1].TOF << ":";
                    for (UInt4 k=0; k< curr_case[1].Cases.size(); k++) std::cout << curr_case[1].Cases[k] << ",";
                    std::cout << "] [ " << curr_case[2].TOF << ":";
                    for (UInt4 k=0; k< curr_case[2].Cases.size(); k++) std::cout << curr_case[2].Cases[k] << ",";
                    std::cout << "]  ==> ";
#endif



                // Decide final Case in filter
                UInt4 decided_case = 0;
                bool isOK = true;
                for (UInt4 i=0; i<num_of_table; i++){
                    for (UInt4 i2=0; i2<(curr_case[i].Cases.size()); i2++){
                        UInt4 target_case = curr_case[i].Cases[i2];
                        if (isAndFilter[ target_case  ]){
                            // AND : when all are same case
#ifdef DEBUG_UTSUSEMI
                            std::cout << " AND ==>";
#endif
                            isOK = true;
                            for (UInt4 j=0; j<num_of_table; j++)
                                if (binary_search( curr_case[j].Cases.begin(), curr_case[j].Cases.end(), target_case )){
                                }else isOK = false;
                            if (isOK){
                                decided_case = target_case;
                                break;
                            }
                        }else{
                            // OR :
#ifdef DEBUG_UTSUSEMI
                            std::cout << " OR ==>";
#endif
                            //if (curr_case[i].first != 0) decided_case = curr_case[i].first;

                            /*
                            //   if 0, 0
                            if (curr_case[i].first == 0){
                            decided_case = 0;
                            isOK = true;
                            break;
                            }
                            */
                            //      all ohters are AND
                            isOK = true;
                            for (UInt4 j=0; j<num_of_table; j++){
                                if (j!=i){
                                    for (UInt4 j2=0; j2<(curr_case[j].Cases.size()); j2++)
                                        if (!(isAndFilter[ curr_case[j].Cases[j2] ])) isOK = false;
                                }
                            }
                            if (isOK){
                                decided_case = target_case;
                                break;
                            }
                            //      others are OR and others are same case
                            isOK = true;
                            for (UInt4 j=0; j<num_of_table; j++)
                                if (j!=i)
                                    for (UInt4 j2=0; j2<(curr_case[j].Cases.size()); j2++)
                                        if (!(isAndFilter[ curr_case[j].Cases[j2] ]))
                                            if ( target_case != curr_case[j].Cases[j2] ) isOK = false;
                            if (isOK){
                                decided_case = target_case;
                                break;
                            }
                        }
                        if (isOK) break;
                    }
                }

#ifdef DEBUG_UTSUSEMI
                std::cout << " decided case from curr_cases ="<< decided_case;
#endif


                // Add final case and TOF of filter
                //if ((isOK)&&(pre_case!=decided_case)){
                if (pre_case!=decided_case){
#ifdef DEBUG_UTSUSEMI
                    std::cout << "  ----->  add to tmp_table ";
#endif
                    UtsusemiOneCase *tmp_case = new UtsusemiOneCase();
                    tmp_case->TOF = curr_case[i_min].TOF;
                    tmp_case->Cases.push_back( decided_case );
                    tmp_table->push_back( tmp_case );
                    pre_case = decided_case;
                }else{
                }
#ifdef DEBUG_UTSUSEMI
                std::cout << std::endl;
#endif

                // Increment index and go out from loop

                if (i_TableList[ i_min ]<(TableList[ i_min ]->size())-1) i_TableList[ i_min ]++;
                else f_TableList[ i_min ]=false;

                isContinue = false;
                for (UInt4 i=0; i<num_of_table; i++){
                    if (f_TableList[i]){
                        isContinue = true;
                        break;
                    }
                }
            }
        }else{
#ifdef DEBUG_UTSUSEMI
            std::cout << "## Filter is not used " << std::endl;
#endif
        }
#ifdef DEBUG_UTSUSEMI
            for (UInt4 i=0; i<tmp_table->size(); i++){
                std::cout << "tmp_table TOF,Cases = "<<tmp_table->at(i)->TOF <<":";
                for (UInt4 j=0;j<tmp_table->at(i)->Cases.size();j++) std::cout << tmp_table->at(i)->Cases[j] <<",";
                std::cout << std::endl;
            }
#endif
        // Merge Filter and Counter
#ifdef DEBUG_UTSUSEMI
        std::cout << "## Merge Filter and Counter : start " << std::endl;
#endif
        std::vector<UInt4> *tmp_merged_table = new std::vector<UInt4>();
        if ((!(_isUsedCounterCase))||(_COUNTCaseTable==NULL)||
            (_COUNTCaseTable->empty())||(_COUNTCaseTable->at(frm)==NULL)){
#ifdef DEBUG_UTSUSEMI
                if (!(_isUsedCounterCase)) std::cout << "## _isUsedCounterCase = false" << std::endl;
                else if (_COUNTCaseTable==NULL) std::cout << "## _COUNTCaseTable=NULL" << std::endl;
                else if (_COUNTCaseTable->empty()) std::cout << "## _COUNTCaseTable is empty." << std::endl;
                else std::cout << "## _COUNTCaseTable->at(frm)==NULL" << std::endl;
#endif
            if (_isUsedFilterCase){
#ifdef DEBUG_UTSUSEMI
                std::cout << "## Use Filter Case "<< std::endl;
#endif
                if (tmp_table->empty()){
                    tmp_merged_table->push_back( pre_merged_case );
                    tmp_merged_table->push_back(0);
                }else{
                    //tmp_merged_table->push_back( pre_counter + tmp_table->at(0)->first ); // head of cali list is first CaseNo
                    if (_numOfCounterCase==0) _numOfCounterCase=1;
                    for (UInt4 i=0; i<(tmp_table->size()); i++){
#ifdef DEBUG_UTSUSEMI
                        std::cout << "tmp_table->at("<<i<<")="<< tmp_table->at(i)->TOF <<":";
                        for (UInt4 j=0;j<tmp_table->at(i)->Cases.size();j++) std::cout <<tmp_table->at(i)->Cases[j]<< ",";
                        std::cout << std::endl;
#endif
                        tmp_merged_table->push_back( pre_counter + (tmp_table->at(i)->Cases[0])*_numOfCounterCase );
                        tmp_merged_table->push_back( tmp_table->at(i)->TOF );
                    }
                }
            }else{
                tmp_merged_table->push_back(pre_counter);
                tmp_merged_table->push_back(0);
            }
        }else{
            if (!(_isUsedFilterCase)||(tmp_table->empty())){
#ifdef DEBUG_UTSUSEMI
                std::cout << "## Only use Counter case in this frame" << std::endl;
#endif
                tmp_merged_table->push_back(pre_merged_case);
                tmp_merged_table->push_back(0);
                for (UInt4 i=0; i<_COUNTCaseTable->at(frm)->size(); i++){
                    tmp_merged_table->push_back( _COUNTCaseTable->at(frm)->at(i)->Cases[0] );
                    tmp_merged_table->push_back( _COUNTCaseTable->at(frm)->at(i)->TOF );
                }
                pre_merged_case = tmp_merged_table->at( tmp_merged_table->size()-2 );
            }else{
                //tmp_merged_table->push_back( tmp_table->at(0)->first ); // head of cali list is first CaseNo
                //tmp_merged_table->push_back( tmp_table->at(0)->second ); // head of cali list is first CaseNo
                //UInt4 f_ind = 1; // first case of filter has been already set.
                tmp_merged_table->push_back( pre_merged_case );
                tmp_merged_table->push_back( 0 );
                UInt4 f_ind = 0;
                UInt4 c_ind = 0;
                bool is_f_in = true;
                //if ((tmp_table->empty())||(tmp_table->size()==1)) is_f_in = false;
                bool is_c_in = true;
                UInt4 curr_fcase = pre_filter;
                UInt4 curr_ccase = pre_counter;
                UtsusemiOneCase *fcase, *ccase;
#ifdef DEBUG_UTSUSEMI
                std::cout << "## Merge Filter and Counter : while loop start." << std::endl;
                std::cout << "tmp_table->size()=" << tmp_table->size()<< std::endl;
#endif
                while( true ){
                    if (is_f_in) fcase = tmp_table->at(f_ind);
                    else{
                        fcase = tmp_table->back();
                        fcase->TOF = max_tof;
                    }
                    //std::cout << "## Merge Filter and Counter : while loop step 1" << std::endl;
                    if (is_c_in) ccase = _COUNTCaseTable->at(frm)->at(c_ind);
                    else{
                        ccase = _COUNTCaseTable->at(frm)->back();
                        ccase->TOF = max_tof;
                    }
                    //std::cout << "## Merge Filter and Counter : while loop step 2" << std::endl;
                    UInt4 merge_tof = 0;
                    if ((fcase->TOF)<(ccase->TOF)){
                        curr_fcase = fcase->Cases[0];
                        merge_tof = fcase->TOF;
                        f_ind++;
                    }else{
                        curr_ccase = ccase->Cases[0];
                        merge_tof = ccase->TOF;
                        c_ind++;
                    }

                    UInt4 merge_case = 0;
#ifdef DEBUG_UTSUSEMI
                        std::cout << "latest filter and counter case =["<< fcase->TOF << "," << fcase->Cases[0] << "] ";
                        std::cout << "["<< ccase->TOF << "," << ccase->Cases[0] << "]" << std::endl;
                        std::cout << "current filter and counter case =["<< curr_fcase << "," << curr_ccase << "] ";
                        std::cout << " merge tof = "<< merge_tof;
#endif
                    if (((is_f_in)&&(curr_fcase==0))||((is_c_in)&&(curr_ccase==0))){
                        merge_case = 0;
                    }else{
                        if (is_f_in){
                            if (is_c_in){
#ifdef DEBUG_UTSUSEMI
                                std::cout << "--> use both ";
#endif
                                merge_case = curr_ccase + ((curr_fcase)-1)*_numOfCounterCase;
                            }else{
#ifdef DEBUG_UTSUSEMI
                                std::cout << "--> use only filter ";
#endif
                                merge_case = pre_counter + (curr_fcase-1)*_numOfCounterCase;
                            }
                        }else{
                            if (is_c_in){
#ifdef DEBUG_UTSUSEMI
                                std::cout << "--> use only counter ";
#endif
                                merge_case = curr_ccase + (pre_filter-1)*_numOfCounterCase;
                            }else{
#ifdef DEBUG_UTSUSEMI
                                std::cout << "--> use prev case ";
#endif
                                merge_case = pre_merged_case;
                            }
                        }
                    }

#ifdef DEBUG_UTSUSEMI
                    std::cout << "-- > merge case= ["<< merge_case << "," << merge_tof << "]";
#endif
                    if ( f_ind>=(tmp_table->size()) ) is_f_in=false;
                    if ( c_ind>=(_COUNTCaseTable->at(frm)->size()) ) is_c_in=false;

                    if (pre_merged_case!=merge_case){
#ifdef DEBUG_UTSUSEMI
                        std::cout << "----> Append";
#endif
                        if (merge_tof==0) tmp_merged_table->at(0)=merge_case;
                        else{
                            tmp_merged_table->push_back( merge_case );
                            tmp_merged_table->push_back( merge_tof );
                        }
                    }
#ifdef DEBUG_UTSUSEMI
                    std::cout << std::endl;
#endif

                    pre_merged_case = merge_case;
                    pre_counter = curr_ccase;
                    pre_filter = curr_fcase;

                    if (!(is_f_in || is_c_in)) break;

                }
                //std::cout << "## Merge Filter and Counter : while loop fin." << std::endl;
            }
        }
        //std::cout << "## Merge Filter and Counter step 2" << std::endl;
#ifdef DEBUG_UTSUSEMI
        for (UInt4 i=0; i<tmp_merged_table->size(); i++)
            std::cout << "tmp_merged_table->at("<<i<<") = "<<tmp_merged_table->at(i)<<std::endl;
#endif
        //remove useless case
        std::vector<UInt4>* merged_table_in_frame = new std::vector<UInt4>;
        if (tmp_merged_table->empty()){
        }else{
            UInt4 tmp_merged_table_size = (UInt4)(tmp_merged_table->size());
            if (tmp_merged_table_size==1){
                merged_table_in_frame->push_back( tmp_merged_table->at(0) );
                merged_table_in_frame->push_back( 0 );
            }else{
                UInt4 tmp_pre_case = 0;
                bool is_first_case = true;
                for (UInt4 i=0; i<(tmp_merged_table_size); i+=2){
                    UInt4 tmp_case = tmp_merged_table->at(i);
                    UInt4 tmp_tof = tmp_merged_table->at(i+1);
                    if (is_first_case){
                        merged_table_in_frame->push_back( tmp_case );
                        merged_table_in_frame->push_back( tmp_tof );
                        tmp_pre_case = tmp_case;
                        is_first_case = false;
                    }else if (tmp_case!=tmp_pre_case){
                        merged_table_in_frame->push_back( tmp_case );
                        merged_table_in_frame->push_back( tmp_tof );
                        tmp_pre_case = tmp_case;
                    }
                }
            }
        }
#ifdef DEBUG_UTSUSEMI
        for (UInt4 i=0; i<merged_table_in_frame->size(); i++)
            std::cout << "merged_table_in_frame->at("<<i<<") = "<<merged_table_in_frame->at(i)<<std::endl;
#endif
        _mergedTable->push_back( merged_table_in_frame );
        //std::cout << "## Merge Filter and Counter fin" << std::endl;
        for (UInt4 i=0;i<(tmp_table->size());i++)
            if (tmp_table->at(i)!=NULL) delete tmp_table->at(i);
        delete tmp_table;

    }
#ifdef DEBUG_UTSUSEMI
        for (UInt4 i=0; i<_mergedTable->size(); i++){
            std::cout << "## mergedTable->at("<<i<<") = ";
            for (UInt4 j=0; j<_mergedTable->at(i)->size(); j++)
                std::cout <<_mergedTable->at(i)->at(j)<<",";
            std::cout << std::endl;
        }
        std::cout << "PulseIdTable.size(), InstClockTable.size()="<<PulseIdTable.size()<<","<< InstClockTable.size()<<std::endl;
#endif
    return true;
}

//////////////////////////////////////////////////////////
std::vector<UInt4> UtsusemiOneTrignetStorage::
PutMergedTable(UInt4 frm, std::vector<bool> isAndFilter, UInt4 _numOfCounterCase ){
    std::vector< std::vector<UInt4>* > *tmp = new std::vector< std::vector<UInt4>* >;
    bool r = Merge( tmp, isAndFilter, _numOfCounterCase );

    std::vector<UInt4> ret;

    if (frm<(tmp->size())){
        for (UInt4 i=0; i<(tmp->at(frm)->size()); i++) ret.push_back( tmp->at(frm)->at(i) );
    }

    for (UInt4 i=0; i<tmp->size(); i++) delete tmp->at(i);
    delete tmp;

    return ret;
}
