#include <cmath>
#include <algorithm>
#include <vector>
#include <RAT/MTCA.hh>
#include <RAT/BitManip.hh>
#include <RAT/Log.hh>
#include <RAT/TriggerSum.hh>
#include <RAT/AnalogSignal.hh>
#include <RAT/DS/DataNotFound.hh>
#include <G4UImessenger.hh>

namespace RAT {

MTCA::MTCA(unsigned NThresholds)
{
   DBLinkPtr fLDAQ = DB::Get()->GetLink("DAQ");

   fNThresholds = NThresholds;
   fThresholds.resize(fNThresholds);
   fThreshCrossingTime.resize(fNThresholds);
   fRawTrigger = 0;

   fComparator.resize(fNThresholds);

   fRawTrigPulseWidth = 0.0;

   fGTprime = false;
   fLockoutStar = false;
   fNRetrig = 0;
   fNRetrigCount = 0;
   fEndTrigSum = false;

   fGains.resize(3);
   fGains[LO] = fLDAQ->GetD("lo_gain");
   fGains[MED] = fLDAQ->GetD("med_gain");
   fGains[HIGH] = fLDAQ->GetD("hi_gain");
}

void MTCA::SetThreshold(float thresh, unsigned ithresh)
{
  if(ithresh < fNThresholds)
    fThresholds.at(ithresh)=thresh;
  else
    Log::Die("MTCA::SetThreshold: Invalid threshold number");
}

unsigned int MTCA::Discriminate(double T, TriggerSum *sum)
{
  BitManip bits;

  // If we're outside the extent of sum, just retrigger on LO*
  if(fEndTrigSum){
    for (unsigned ithresh=0; ithresh<fNThresholds; ithresh++){
      if(fNRetrigCount>0 && fLockoutStar){
        // Set bit in trigger word to issue a raw trig
        fRawTrigger = bits.SetBit(fRawTrigger, ithresh);
        // Decrement counter
        if(ithresh==(fNThresholds-1))fNRetrigCount -= 1;
      }
    }
    // Reset LO* to off (since it should only be enabled for a single T step)
    fLockoutStar = false;
    // Reset fEndTrigSum for next event (will be set again each step)
    fEndTrigSum = false;
    return fRawTrigger;
  }
  double height = sum->GetHeight(T);
  for (unsigned ithresh=0; ithresh<fNThresholds; ithresh++){
    if (T<sum->fPulse[0]->GetStartTime()){
      fThreshCrossingTime.at(ithresh) = DS::INVALID;
      fComparator.at(ithresh)=false;
      fGTprime = 0;
    }
    if (T-fThreshCrossingTime.at(ithresh)>fRawTrigPulseWidth){
    //past any previous raw trigger pulse or first crossing in this event

      if (height>=fThresholds.at(ithresh)){
        if (!fComparator.at(ithresh) || fGTprime){
          // Set bit in trigger word
          fRawTrigger = bits.SetBit(fRawTrigger, ithresh);
          fThreshCrossingTime.at(ithresh)=T;
          fComparator.at(ithresh)=true;
          if(fGTprime){fGTprime = 0;}
          // We just issued a raw trig, so reset the counter for retriggers
          fNRetrigCount = fNRetrig;
        }
        else{ //above thresh, but high for longer than raw trig width
          // clear bit `ithresh'
          fRawTrigger = bits.ClearBit(fRawTrigger, ithresh);
        }
      }
      else { //below thresh, and beyond raw trigger pulse width
        // Are we at the end of LO, and retriggering?  Shoot:
        if(fNRetrigCount>0 && fLockoutStar){
          // Set bit in trigger word to issue a raw trig
          fRawTrigger = bits.SetBit(fRawTrigger, ithresh);
          fThreshCrossingTime.at(ithresh)=T;
          fComparator.at(ithresh)=true;
          // Decrement counter
          if(ithresh==(fNThresholds-1))fNRetrigCount -= 1;
        }
        // If not, reset everything
        else{
          fComparator.at(ithresh)=false;
          // If this trig type is still set, turn it off (i.e. clear the bit)
          fRawTrigger = bits.ClearBit(fRawTrigger, ithresh);
        }
      }
    }
  }
  // Reset LO* to off (since it should only be enabled for a single T step)
  fLockoutStar = false;
  return fRawTrigger;
}

AnalogSignal* MTCA::ScaleTrigSum(TriggerSum *sum, Gain gainPath)
{
  ScaledSum *thisSum = new ScaledSum(sum,GetGain(gainPath));
  return thisSum;
}

double MTCA::ScaledSum::GetHeight(double time)
{
  return original->GetHeight(time)*scaleFactor;
}

} // namespace RAT