#include <cmath>
#include <algorithm>
#include <vector>
#include <RAT/MTCD.hh>
#include <RAT/clock.hh>
#include <RAT/Log.hh>
#include <RAT/BitManip.hh>
#include <RAT/DU/TrigBits.hh>
#include <G4UImessenger.hh>


namespace RAT {


MTCD::MTCD()
{
    fRawTrigger = 0;
    fRawTriggerTime = 0.;
    fLatchPending = 0;
    fExtAsyTime = 0.;
    fClock50 = new clock;
    fClock50->SetFrequency(50000000.0); //50 MHz free-running clock
    fClock50->SetNbits(43); //43 bits on 50 MHz clock counter
    fClock50->SetRolloverInterval();

    fClock10 = new clock;
    fClock10->SetFrequency(10000000.0); //10 MHz GPS clock
    fClock10->SetNbits(53); //53 bits on 10 MHz clock counter

    fSYNC = false;
    fSYNC24 = false;

    fIsPGT = false;
    fIsPre = false;

    fPGTwidth = 0.0;
    fPGTtime = 0.0;
    fPulseGTHigh = false;
    fPGTFreq = 0;
    fPrescaleCount = 0;
    fLoLow = true;
    fPrescaleTime = -99.;
    fPrescaleHigh = false;
    fPrescaleWidth = 0.;
    fPrescaleDelay = 0.;
    fPulseGTMin = 20.;  // Default is one clock tick, since pGT must occur on a tick
    fPulseGTDefault = 100.; // Default is 10Hz

    fTrigHigh = 0;

}

MTCD::~MTCD()
{
    delete fClock50;
    delete fClock10;
}

void MTCD::SetPulseGTFreq()
{
   // Check frequency <= max (period >= min)
   // If not, reset to max (min)
   if(fPulseGT < (fPulseGTMin/1.E6)){
     warn << RED << " MTCD::SetPulseGTFreq: PulseGT period set outside valid range!"
          << CLR << newline;
     warn << RED << " You have tried to set the period to "
          << 1.E6*fPulseGT << "ns and the minimum is " << fPulseGTMin
          << "ns." << CLR <<newline;
     fPulseGT = fPulseGTDefault;
     warn << RED << " Resetting to " << 1./(fPulseGTDefault/1000) << " Hz." << CLR << newline;
   }

   // Check frequency is multiple of 50ns
   if(((fPulseGT / (50.E-6)) - (int)(fPulseGT / (50.E-6))) !=0.){
     warn << RED << " MTCD::SetPulseGTFreq: PulseGT period set to a non-integer # of 50MHz clock ticks! " << CLR << newline;
     fPulseGT = 50.E-6*(double)((int)(fPulseGT/50.E-6));
     warn << RED << " Resetting to " << fPulseGT
          << " (rounding down to next int value)." << CLR <<newline;
   }
   // Every nth clock tick, we want a pGT (n=fPGTFreq)
   fPGTFreq = (ULong64_t) (fPulseGT*fClock50->GetFrequency()/1000.);
}

bool MTCD::CheckGlobalTrigger(UInt_t analogword, UInt_t externalword, double T)
{
  //
  //Check for PulseGT by looking to see if clock ticks say we just passed
  //a PulseGT time
  //
  unsigned int internalword=0;
  if(fIsPGT){
    EventTime *now= new EventTime;
    now->SetStart();
    now->IncrementUT(fMCEventTime+T);

    ULong64_t Clock50Count = fClock50->GetCount(*now);

    // On 1st clock count coinciding w/ pGT, set time, & set pulse high
    if (Clock50Count % fPGTFreq == 0){
      if(!fPulseGTHigh)fPGTtime = T;
      fPulseGTHigh = true;
    }
    if(fPulseGTHigh && ((T-fPGTtime)>fPGTwidth)) fPulseGTHigh = false;

    if (fPulseGTHigh){
      internalword = 1024;
    }
    delete now;
  }

  //
  // Prescale: Set prescale bit high every 'fPrescale' counts of NHit100Lo
  //
  BitManip bits;

  if(fIsPre){
    // Count NHit100Lo crossings:
    // If N100LoBit is set and was low before, count
    if(fLoLow && bits.TestBit(analogword, DU::TrigBits::N100Low)){
      ++fPrescaleCount;
      fLoLow = false;
    }
    // If N100LoBit is not set, reset bool
    if(!(bits.TestBit(analogword, DU::TrigBits::N100Low)))fLoLow = true;

    // When count reaches prescale, initiate delay
    if(fPrescaleCount >= fPrescale){
      fPrescaleTime = T;
      fPrescaleCount = 0;
    }

    // After delay, initiate raw trigger,
    if(fPrescaleTime>0. && (((T-fPrescaleTime) > fPrescaleDelay) || abs((T-fPrescaleTime) - fPrescaleDelay) < 0.0001) ){
      fPrescaleHigh = true;
    }

    // After pulse width, reset
    if(fPrescaleTime>0. &&
       ((T-fPrescaleTime) > (fPrescaleDelay+fPrescaleWidth) || abs((T-fPrescaleTime) - (fPrescaleDelay+fPrescaleWidth)) < 0.0001 )){
      fPrescaleHigh = false;
      fPrescaleTime = -10.;
    }

    // Set prescale bit
    if(fPrescaleHigh){analogword = bits.SetBit(analogword, DU::TrigBits::Prescale);}
  }
  fTriggerWord = analogword | externalword | internalword;

  bool GlobalTrigger=0;
  if (!fRawTrigger){
    if (fTriggerWord & fTriggerMask){
      fRawTriggerTime = T;
      fRawTrigger = 1;
      fLatchPending = 1;
    }
  }
   else{
     if (((T-fRawTriggerTime)>fLatchDelay || abs((T-fRawTriggerTime)-fLatchDelay)<0.0001 ) & fLatchPending){
       fLatchPending = 0;
     }
     if (!fLatchPending){
       if ((externalword & 32768) && (fTriggerMask & 32768)) { //ExtAsy so no latching with 50 MHz clock
         fGlobalTriggerTime = fExtAsyTime;
         fRawTrigger = 0;
         GlobalTrigger = 1;
       }
       else{
         //Now get the global trigger time, latching it with the 50 MHz
         //clock.  we don't have to subtract latch delay because
         //fRawTriggerTime is the time before the delay. If we used
         //T here instead of fRawTriggerTime, we would have to subtract.
         fGlobalTriggerTime = fClock50->GetNextTickTime(fMCEventTime+fRawTriggerTime)-fMCEventTime;
       }
       fRawTrigger = 0;
       GlobalTrigger = 1;
       // Reset trig signal statuses for missed trig search
       fTrigHigh = fTriggerWord;
     }
   }

   return GlobalTrigger;
}


bool MTCD::CheckMissedTrigger(UInt_t analogword, UInt_t externalword, double T)
{
  //
  //Check for PulseGT by looking to see if clock ticks say we just passed
  //a PulseGT time
  //
  unsigned int internalword=0;

  if(fIsPGT){
    EventTime *now= new EventTime;
    now->SetStart();
    now->IncrementUT(fMCEventTime+T);

    ULong64_t Clock50Count = fClock50->GetCount(*now);

    // On 1st clock count coinciding w/ pGT, set time, & set pulse high
    if (Clock50Count % fPGTFreq == 0){
      if(!fPulseGTHigh)fPGTtime = T;
      fPulseGTHigh = true;
    }
    if(fPulseGTHigh && ((T-fPGTtime)>fPGTwidth)) fPulseGTHigh = false;

    if (fPulseGTHigh){
      internalword = 1024;
    }
    delete now;
  }

  //
  // Prescale: Set prescale bit high every 'fPrescale' counts of NHit100Lo
  //
  BitManip bits;

  if(fIsPre){
    // Count NHit100Lo crossings:
    // If N100LoBit is set and was low before, count
    if(fLoLow && bits.TestBit(analogword, DU::TrigBits::N100Low)){
      ++fPrescaleCount;
      fLoLow = false;
    }
    // If N100LoBit is not set, reset bool
    if(!(bits.TestBit(analogword, DU::TrigBits::N100Low)))fLoLow = true;

    // When count reaches prescale, initiate delay
    if(fPrescaleCount >= fPrescale){
      fPrescaleTime = T;
      fPrescaleCount = 0;
    }

    // After delay, initiate raw trigger
    if(fPrescaleTime>0. && (((T-fPrescaleTime) > fPrescaleDelay) || (abs((T-fPrescaleTime) - fPrescaleDelay) <0.0001)) ){
      fPrescaleHigh = true;
    }

    // After pulse width, reset
    if(fPrescaleTime>0. &&
       ( ((T-fPrescaleTime) > (fPrescaleDelay+fPrescaleWidth)) || (abs((T-fPrescaleTime) - (fPrescaleDelay+fPrescaleWidth)) < 0.0001)) ){
      fPrescaleHigh = false;
      fPrescaleTime = -10.;
    }

    // Set prescale bit
    if(fPrescaleHigh){analogword = bits.SetBit(analogword, DU::TrigBits::Prescale);}
  }
  unsigned int TrigWord = analogword | externalword | internalword;

  bool MissedTrigger=0;
  unsigned int potentials = 0;

  // Get a missed trigger iff:
  // >=1 bit that is now high (in TrigWord) was low (in fTrigHigh) i.e. new t/h crossing
  for(int itrig=0;itrig<27;++itrig){
    if(bits.TestBit(TrigWord,itrig) && !bits.TestBit(fTrigHigh,itrig)){
      potentials = bits.SetBit(potentials,itrig);
    }
  }
  // AND that bit is masked in

  if (potentials & fTriggerMask) MissedTrigger = 1;

  return MissedTrigger;
}


ULong64_t MTCD::GetClockCount50()
{
    ULong64_t count50 = fClock50->GetCount();
    return count50;
}

ULong64_t MTCD::GetClockCount50(EventTime GTrigTime)
{
    ULong64_t count50 = fClock50->GetCount(GTrigTime);
    return count50;
}

ULong64_t MTCD::GetClockCount10(EventTime GTrigTime)
{
    ULong64_t count10 = fClock10->GetCount(GTrigTime);
    return count10;
}

EventTime MTCD::GetClockTime10()
{
    EventTime thisTime10 = fClock10->GetUTime();
    return thisTime10;
}

void MTCD::SetGTCount(UInt_t count)
{
    // So that the first event is the ID we init with, we want to init to count-1
    // because we then increment for every event, before recording the ID
    // If the count was 0, init to 2^24-1

    if(count==0){
        fGTCount = (1<<24) - 1;
    }
    if(count>0){
        int iGTCount = (int) count - 1;
        int ibroll = 1 << 24;
        fGTCount = iGTCount%ibroll;
    }
}

void MTCD::IncrementGTCount()
{
    int iGTCount = fGTCount;

    // Add 1 to the event counter
    iGTCount += 1;

    // Check if we want a SYNC CLEAR
    // (i.e. we're at 2^16 - 1 and about to roll over bit 17)
    int ibtest = (1<<16);
    if(((iGTCount+1)%ibtest) == 0){
      fSYNC = true;
    }

    // Check if we're at 24 bits, and should roll over
    int ibroll = 1 << 24;
    if(((iGTCount+1)%ibroll) == 0){
      fSYNC24 = true;
      if(!fSYNC)printf("%s MTCD::IncrementGTCount: ERROR: We have SYNC24 but not SYNC at event %26i%s\n", BBLUE, iGTCount, CLR);
    }

    if(fSYNC){
      // We're getting a SYNC CLEAR, so we skip the next event ID
      iGTCount += 1;
    }

    fGTCount = iGTCount%ibroll;
    //if(!fSYNC24 && (fGTCount != iGTCount)){
      // We rolled over but didn't expect to
      //printf("\033[1;34m ERROR: We don't have SYNC24 at event %26i but we got event ID %26i \033[m \n",iGTCount,fGTCount);
    //}

}

void MTCD::SetTrigErrBits()
{
    //These are the bits that are set when counter test modes are NOT enabled
    UInt_t testCount = (1)+(1<<1)+(1<<2);

    //int iGTCount = (int) fGTCount;
    // Set bit 5 (7) when all bits 0-15 (23) are 1s by testing on SYNC(24)
    // (Add 32 (128) to testCount)

    if(fSYNC){
      testCount = 32+testCount;
      fSYNC = false;
    }

    if(fSYNC24){
      testCount = 128+testCount;
      fSYNC24 = false;
    }

    fErrBits = testCount;
}

} // namespace RAT