#!/usr/bin/python
#Python imports
import sys, argparse, time, subprocess, serial, math
import PyGm2TrackerDAQ
from BU_tools.LogicBoard import *
from BU_tools.tdc_helpers import *

'''
Main function
'''

def CDR_check(ser,TDCs):
  cmd = "daq_reg_rd "

  #counts
  d_char_count = []
  k_char_count = []
  invalid_char_count = []
  misaligned_idle_char_count = []
  TDC_okay = []
  TDC_alignment = []

  #threshold of number of good TDC checks to say it is working
  TDC_okay_threshold = 25

  wait_time  = 0.1 # seconds
  wait_count = 2500000 * len(TDCs) * wait_time # expected number of k_chars per check
  wait_count = wait_count/10

  #create current and old values for these diagnostic counters
  for iTDC in TDCs:
    k_char_count.append( [0,0])
    d_char_count.append( [0,0])
    invalid_char_count.append( [0,0])
    misaligned_idle_char_count.append([0,0])
    TDC_okay.append(0)
    TDC_alignment.append(0)
  
  #keep running until we have a lock or we give up
  while (TDC_okay[0] < TDC_okay_threshold and 
         TDC_okay[1] < TDC_okay_threshold and 
         TDC_okay[2] < TDC_okay_threshold and 
         TDC_okay[3] < TDC_okay_threshold):
    for iTDC in TDCs:
      #Get the current TDC counters
      k_char_count[iTDC][1] = int(WriteCommand(ser,[cmd,(0x10*iTDC) + 0x52]),16)
      d_char_count[iTDC][1] = int(WriteCommand(ser,[cmd,(0x10*iTDC) + 0x58]),16)
      invalid_char_count[iTDC][1] = int(WriteCommand(ser,[cmd,(0x10*iTDC) + 0x56]),16)
      misaligned_idle_char_count[iTDC][1] = int(WriteCommand(ser,[cmd,(0x10*iTDC) + 0x57]),16)
      
      #compute differences
      k_char_diff = k_char_count[iTDC][1] -  k_char_count[iTDC][0]
      d_char_diff = d_char_count[iTDC][1] -  d_char_count[iTDC][0]
      invalid_char_diff = invalid_char_count[iTDC][1] -  invalid_char_count[iTDC][0]
      misaligned_idle_diff = misaligned_idle_char_count[iTDC][1] - misaligned_idle_char_count[iTDC][0]
      
      #setup for next iteration
      k_char_count[iTDC][0] = k_char_count[iTDC][1]
      d_char_count[iTDC][0] = d_char_count[iTDC][1]
      invalid_char_count[iTDC][0] = invalid_char_count[iTDC][1]
      misaligned_idle_char_count[iTDC][0] = misaligned_idle_char_count[iTDC][1]
      
      #check if the tdc is working
      if misaligned_idle_diff == 0 and invalid_char_diff == 0 and d_char_diff == 0 and k_char_diff > wait_count:
        TDC_okay[iTDC] = TDC_okay[iTDC] + 1
      else:
        print "Bad TDC %d (@%d okays) had %d new k-chars, %d data chars, %d invalid chars, and %d new misaligned idles\n" %(iTDC,TDC_okay[iTDC],k_char_diff,d_char_diff,invalid_char_diff,misaligned_idle_diff)
        #when not working, adjust the CDR
        TDC_okay[iTDC] = 0
        TDC_alignment[iTDC] = TDC_alignment[iTDC] + 1
        if TDC_alignment[iTDC] > 5:
          print "TDC %d can't lock on, giving up\n" % iTDC
          return
        else :
          print "Adjusting TDC %d to CDR alignment %d" %(iTDC,TDC_alignment[iTDC])
          WriteCommand(ser,["daq_reg_wr ",(0x10*iTDC + 0x50)," ",(0x10*TDC_alignment[iTDC])])
          time.sleep(wait_time)
          
  WriteCommand(ser,["daq_reg_wr 20 1"])
  print "TDC CDRs are aligned"
  return
    
    

def main():

  #=====================================================================
  #Setup system
  #=====================================================================

  #Open the serial port
  ser = serial.Serial("/dev/ttyUSB0",115200);

  #Enable TDCs
  TDCs = TDCBoard_TDC_nums(ser)
  CDR_check(ser,TDCs)


if __name__ == '__main__':
  main()