//_____________________________________________________________________________
///
/// \class ConvergenceMaster
///
/// ConvergencMaster - class guiding the sequence in which fits are attemmpted.
/// During the initial iterations many of the track hits are "masked out" and 
/// ignored by the fitter. As the iterations progress ConvergenceMaster keeps 
/// unmasking the hits until the longest possible part of the track (starting
/// at the vertex) is fit using all hits. The "masks" are stored in vectors
/// and organized in "binary levels" where a "next" level mask is obtained by
/// removing every other hit from a "current" level mask.
///
/// \author Sergei avva@fnal.gov
///

#include <vector>
#include <algorithm>
#include <cassert>
#include <numeric>

#include "MessageService/MsgService.h"

#include "CandFitTrackSA/DataFT.h"
#include "CandFitTrackSA/TrackContext.h"
#include "CandFitTrackSA/TracerSA.h"

#include "ConvergenceMaster.h"

CVSID("$Id: ConvergenceMaster.cxx,v 1.4 2006/12/03 01:41:37 gmieg Exp $");

///
/// ctor (does little - mask creation is in BuildMasks method)
///
ConvergenceMaster::ConvergenceMaster(Int_t nhitsinviewmin) :
    fNHitsInViewMin(nhitsinviewmin)
{
    TracerSA trace("ConvergenceMaster::ConvergenceMaster(Int_t)");
}

///
/// dtor
///
ConvergenceMaster::~ConvergenceMaster()
{
}


///
/// build the "mask levels" for a given track.  
///
void ConvergenceMaster::BuildMasks(const DataFT& data)
{
    TracerSA trace("ConvergenceMaster::BuildMasks(const DataFT& )");
    
    ViewMask_t uMask = data.GetUHitUse();
    ViewMaskList_t uMaskList;
    BuildViewMasks(uMask, uMaskList);    

    ViewMask_t vMask = data.GetVHitUse();
    ViewMaskList_t vMaskList;
    BuildViewMasks(vMask, vMaskList);    

    // make uMaskList, vMaskList equal size
    // by copying the last element of the shorter
    // list n times
    PadMaskLists(uMaskList, vMaskList);
    
    // make MaskStep objects - each MaskStep holds
    // masks for U and V and pointers to current
    // # of planes fit, max, min #planes, etc   
    BuildMaskSteps(uMaskList, vMaskList);
    
    SetInitialMask();
    
    PrintMasks();    
    return;
}


///
/// if U and V masks have different number of "levels" - pad the shorter
/// one with copies of the last element
///
void ConvergenceMaster::PadMaskLists( ViewMaskList_t& uMaskList, 
                                      ViewMaskList_t& vMaskList  )
{
    TracerSA trace("ConvergenceMaster::PadMaskLists(ViewMaskList_t&, ViewMaskList_t&)");
    UInt_t sizeU = uMaskList.size();
    UInt_t sizeV = vMaskList.size();
    
    // if size equal - nothing to do, return
    if ( sizeU == sizeV ) return;

    // pad one of the lists    
    if ( sizeU < sizeV ) {
        PadWithLastElement(uMaskList, sizeV-sizeU);
    } else {
        PadWithLastElement(vMaskList, sizeU-sizeV);
    }
    return;
}


///
/// extend vector by copying the last element n times
///
void ConvergenceMaster::PadWithLastElement(ViewMaskList_t& maskList, UInt_t n)
{
    TracerSA trace("ConvergenceMaster::PadWithLastElement(ViewMaskList_t&, UInt_t)");
    ViewMask_t last = *maskList.rbegin();
    
    for ( UInt_t i = 0; i < n; ++i ) {
        maskList.push_back(last);
    }
    
    return;
} 


///
/// set the "maximal" mask level (between 4 and 7 hits used in each view) 
/// as the initial mask 
///
void ConvergenceMaster::SetInitialMask() 
{
    TracerSA trace("ConvergenceMaster::SetInitialMask()");
    fMaskCur = fMaskList.begin();
    fMaskIsValid = kTRUE;
}


///
/// make "mask steps" - each mask step contains mask vectors for U and V views
///
void ConvergenceMaster::BuildMaskSteps( const ViewMaskList_t& uMaskList, 
                                        const ViewMaskList_t& vMaskList )   
{
    TracerSA trace("ConvergenceMaster::BuildMaskSteps(const ViewMaskList_t&, const ViewMaskList_t&)");

    assert( uMaskList.size() == vMaskList.size() && 
            "U, V mask lists are not equal size!"    );
            
    ViewMaskListCItr itrU = uMaskList.begin();
    ViewMaskListCItr endU = uMaskList.end();
    ViewMaskListCItr itrV = vMaskList.begin();
    
    while ( itrU != endU ) {
        fMaskList.push_back(MaskStep( *itrU, *itrV, fNHitsInViewMin ) );
        ++itrU;
        ++itrV;
    }
    
    return;
}    


///
/// create "mask levels" for one view. Each level is created by sequentially masking
/// every other hit in the view. 
///
void ConvergenceMaster::BuildViewMasks(  const ViewMask_t&     maskIn, 
                                            ViewMaskList_t&   maskList   )   
{
    TracerSA trace("ConvergenceMaster::BuildViewMasks(const ViewMask_t&, ViewMaskList_t&)");
    ViewMask_t mask(maskIn);
    
    maskList.push_front(mask);
    
    while ( UnsetEverySecond(mask) ) {
        maskList.push_front(mask);
    }
    
    return;
}    


///
/// "mask" every other hit
///
Bool_t ConvergenceMaster::UnsetEverySecond( ViewMask_t& mask ) 
{ 
    TracerSA trace("ConvergenceMaster::UnsetEverySecond(vector<Int_t>&)");
    ViewMaskItr begin   = mask.begin();
    ViewMaskItr end     = mask.end();
    
    // if total number of hits is less than 7 - stop
    Int_t nhits = accumulate(begin, end, 0);
    MSG("FitTrackSA", Msg::kDebug) << "#hits = " << nhits << "\n";
    if ( nhits < (2*fNHitsInViewMin-1) ) return kFALSE;
    
    ViewMaskItr it = begin;
    while ( (it = find(it, end, 1)) != end ) {
        // skip this occurance of 1
        MSG("FitTrackSA", Msg::kDebug) << "found hit at " << it-begin << ", skipping\n";
        ++it;
        if ( it == end ) break;
        
        // find next 1, set it to 0
        it = find(it, end, 1);
        if ( it == end ) break;        
        MSG("FitTrackSA", Msg::kDebug) << "found hit at " << it-begin 
            << ", setting to 0\n";
        *it = 0;
        ++it;
        if ( it == end ) break;
    }
    return kTRUE;
}    


///
/// get current "mask level" for U view 
///
const ConvergenceMaster::ViewMask_t& ConvergenceMaster::GetMaskUCur() const
{
    TracerSA trace("ConvergenceMaster::GetMaskUCur()");
    return fMaskCur->GetMaskU();
}


///
/// get current "mask level" for V view 
///
const ConvergenceMaster::ViewMask_t& ConvergenceMaster::GetMaskVCur() const
{
    TracerSA trace("ConvergenceMaster::GetMaskVCur()");
    return fMaskCur->GetMaskV();
}


///
/// print masks (printing masks is useful to understand what is going on) 
///
void ConvergenceMaster::PrintMasks() const
{
    TracerSA trace("ConvergenceMaster::PrintMasks()");

    MaskListCItr beg = fMaskList.begin();
    MaskListCItr end = fMaskList.end();
      
    MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kDebug);
    (*mftsa) << "\nmasks:\n";

    for (MaskListCItr it = beg; it!=end; ++it) {
        (*mftsa) << "Level " << it-beg << "\n";
        it->PrintMasks();
        (*mftsa) << "\n";       
    }
    
    return;
}



///
/// Depending on the result of previous iteration, choose the next mask level,
/// track length to try. 
///
Bool_t ConvergenceMaster::NextStep()
{
    TracerSA trace("ConvergenceMaster::NextStep()");
    // possibilities
    // - cur step converged and not at max and prev < cur -> increment
    // - cur step converged and ( at max or prev > cur ) -> go to next mask
    // - cur step diverged and not at min and prev > cur -> decrement
    // - cur step diverged and ( at min or prev < cur ) -> go to next mask
    
    fMaskCur->Print();
        
    if (    fMaskCur->GetConvergedCur()     && 
            fMaskCur->GetPlaneCountCur() < fMaskCur->GetPlaneCountMax() && 
            fMaskCur->GetPlaneCountCur() >= fMaskCur->GetPlaneCountLast()   ) {
            
        return IncrementPlaneCount();
    
    } else if ( fMaskCur->GetConvergedCur()        && 
                (fMaskCur->GetPlaneCountCur() == fMaskCur->GetPlaneCountMax()||  
                 fMaskCur->GetPlaneCountCur() <=  fMaskCur->GetPlaneCountLast()) 
                                                                            ) {
        
        return NextMask(fMaskCur->GetPlaneCountCur());
    
    } else if ( ! fMaskCur->GetConvergedCur() && 
                fMaskCur->GetPlaneCountCur() > fMaskCur->GetPlaneCountMin()&&
                fMaskCur->GetPlaneCountCur() <= fMaskCur->GetPlaneCountLast()
                                                                             ){
        
        return DecrementPlaneCount();
    
    } else if ( ! fMaskCur->GetConvergedCur() && 
                ( fMaskCur->GetPlaneCountCur() == fMaskCur->GetPlaneCountMin() ||  
                  fMaskCur->GetPlaneCountCur() >=fMaskCur->GetPlaneCountLast())
                                                                            ){
        return NextMask(fMaskCur->GetPlaneCountCur());
    }
    
    assert ( ! "Should not get here!");

    return kTRUE;
}


///
/// increase length of the track segment to fit
///
Bool_t ConvergenceMaster::IncrementPlaneCount()
{
    TracerSA trace("ConvergenceMaster::IncrementPlaneCount()");
    return fMaskCur->IncrementPlaneCount();            
}


///
/// decrease length of the track segment to fit
///
Bool_t ConvergenceMaster::DecrementPlaneCount()
{
    TracerSA trace("ConvergenceMaster::DecrementPlaneCount()");
    return fMaskCur->DecrementPlaneCount();            
}


///
/// switch to the next mask level
///
Bool_t ConvergenceMaster::NextMask(Count_t planeCount)
{
    TracerSA trace("ConvergenceMaster::NextMask()");
    ++fMaskCur;
    
    if ( fMaskCur == fMaskList.end() ) return kFALSE;
    
    fMaskCur->SetPlaneCountCur(planeCount);            
    fMaskCur->SetPlaneCountLast(planeCount);            
    
    fMaskIsValid = kFALSE;
    return kTRUE;
}


///
/// return current track segment length 
///
ConvergenceMaster::Count_t ConvergenceMaster::GetNPlanesCur() const
{
    TracerSA trace("ConvergenceMaster::GetNPlanesCur()");
    return fMaskCur->GetPlaneCountCur();
} 
   

///
/// return minimum (for this mask level) track segment length 
///
ConvergenceMaster::Count_t ConvergenceMaster::GetNPlanesMin() const
{
    TracerSA trace("ConvergenceMaster::GetNPlanesMin()");
    return fMaskCur->GetPlaneCountMin();
}


///
/// return maximum (for this mask level) track segment length 
///
ConvergenceMaster::Count_t ConvergenceMaster::GetNPlanesMax() const
{
    TracerSA trace("ConvergenceMaster::GetNPlanesMax()");
    return fMaskCur->GetPlaneCountMax();
}


///
/// ConvergenceMaster::MaskStep ctor
///
ConvergenceMaster::MaskStep::MaskStep(  const ViewMask_t& umask, 
                                        const ViewMask_t& vmask,
                                        Int_t nhitsinviewmin     ) :
    fMaskU(umask), fMaskV(vmask)
{
    TracerSA trace("ConvergenceMaster::MaskStep::MaskStep(...,Int_t)");
    // make sum of U and V masks
    fMaskUVSum.resize(fMaskU.size());
    for (UInt_t i = 0; i < fMaskUVSum.size(); ++i) {
        fMaskUVSum[i] = fMaskU[i] + fMaskV[i];
    }
    
    Count_t uMin = FindPlaneCountMin(umask, nhitsinviewmin);
    Count_t vMin = FindPlaneCountMin(vmask, nhitsinviewmin);    
    fPlaneCountMin = max(uMin, vMin);
    
    Count_t uMax = FindPlaneCountMax(umask);
    Count_t vMax = FindPlaneCountMax(vmask);
    fPlaneCountMax = max(uMax, vMax);
    
    fPlaneCountCur  = fPlaneCountMax;
    fConvergedCur   = kTRUE;
    fPlaneCountLast = fPlaneCountCur;
}


///
/// ConvergenceMaster::MaskStep dtor
///
ConvergenceMaster::MaskStep::~MaskStep()
{}


///
/// calculate minimum (for this mask level) track segment length 
///
ConvergenceMaster::Count_t 
ConvergenceMaster::MaskStep::FindPlaneCountMin( const ViewMask_t& mask, 
                                                      Count_t nhitsmin  )
{
    TracerSA trace("ConvergenceMaster::MaskStep::FindPlaneCountMin(ViewMask_t&, Count_t)");
    ViewMaskCItr beg = mask.begin();
    ViewMaskCItr end = mask.end();
    ViewMaskCItr it = beg;
    
    for ( Count_t i = 0; i < nhitsmin; ++i) {
        it = find(it, end, 1);
        ++it;
    }
    
    return it - beg;
}


///
/// calculate maximum (for this mask level) track segment length 
///
ConvergenceMaster::Count_t 
ConvergenceMaster::MaskStep::FindPlaneCountMax( const ViewMask_t& mask )
{
    TracerSA trace("ConvergenceMaster::MaskStep::FindPlaneCountMax(ViewMask_t&)");
    ViewMaskCRItr beg = mask.rbegin();
    ViewMaskCRItr end = mask.rend();
    
    ViewMaskCRItr it  = find(beg, end, 1);
    Count_t size = mask.size();    
    
    return (size - (it - beg));
}


///
/// print MaskStep (excluding mask vectors)
///
void ConvergenceMaster::MaskStep::Print() const
{
    TracerSA trace("ConvergenceMaster::MaskStep::Print()");

    MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kDebug);
    (*mftsa) << "min = " << fPlaneCountMin 
             << "; max = " << fPlaneCountMax 
             << "; cur = " << fPlaneCountCur 
             << "; last = " << fPlaneCountLast 
             << "; conv = " << fConvergedCur << "\n";
}


///
/// print MaskStep mask vectors
///
void ConvergenceMaster::MaskStep::PrintMasks() const
{
    TracerSA trace("ConvergenceMaster::MaskStep::PrintMask()");

    MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kDebug);
    (*mftsa) << "min = " << fPlaneCountMin 
             << "; max = " << fPlaneCountMax << "\n";

    (*mftsa) << "U: ";
    ViewMaskCItr beg = fMaskU.begin();
    ViewMaskCItr end = fMaskU.end();      
    for (ViewMaskCItr it = beg; it!=end; ++it) {
        (*mftsa) << *it << " ";
    }
    (*mftsa) << "\n";       
    
    (*mftsa) << "V: ";
    beg = fMaskV.begin();
    end = fMaskV.end();      
    for (ViewMaskCItr it = beg; it!=end; ++it) {
        (*mftsa) << *it << " ";
    }
    (*mftsa) << "\n";       
    
    return;
}


///
/// increase length of the track segment to fit
///
Bool_t ConvergenceMaster::MaskStep::IncrementPlaneCount()
{
    TracerSA trace("ConvergenceMaster::MaskStep::IncrementPlaneCount()");
    assert( fPlaneCountCur < fPlaneCountMax && 
            "Can't increment plane count, already at max!" );

    Print();
        
    ViewMaskCItr beg = fMaskUVSum.begin();
    ViewMaskCItr end = fMaskUVSum.end();
    
    ViewMaskCItr it = beg + fPlaneCountCur;
    
    it = find(it, end, 1);
    ++it;
    
    fPlaneCountLast = fPlaneCountCur;
    fPlaneCountCur = it - beg;
    
    Print();
        
    return kTRUE;
}


///
/// decrease length of the track segment to fit
///
Bool_t ConvergenceMaster::MaskStep::DecrementPlaneCount()
{
    TracerSA trace("ConvergenceMaster::MaskStep::DecrementPlaneCount()");
    assert( fPlaneCountCur > fPlaneCountMin && 
            "Can't decrement plane count, already at min!" );
    
    Print();
        
    ViewMaskCRItr beg = fMaskUVSum.rbegin();
    ViewMaskCRItr end = fMaskUVSum.rend();
    
    ViewMaskCRItr it = end - fPlaneCountCur + 1;
    
    it = find(it, end, 1);
    
    fPlaneCountLast = fPlaneCountCur;
    fPlaneCountCur = end - it;
    
    Print();
        
    return kTRUE;
}