#include "RCUCommandCoder.h"

//===========================================================================
RCUCommandCoder::RCUCommandCoder() {
}

//===========================================================================
uint64_t RCUCommandCoder::sendAltroInstruction(uint64_t AltroInstruction){
  // This is a write instruction consisting of two 22bit words to be put into the RCU Instruction Memory
  // The first word is the command and register adress, the second is the data
  return (AltroInstruction&0xFFFFFFFF) | ((uint64_t)instruction(eTarget(FEC_WR),AltroInstruction>>32)<<32);
}

//===========================================================================
uint32_t RCUCommandCoder::sendAltroCommand(uint32_t AltroInstruction){
  // This is a command consisting of one 22bit word to be put into the RCU Instruction Memory
  uint32_t retval = instruction(eTarget(FEC_CMD), AltroInstruction);
  return retval;
}

//===========================================================================
uint64_t RCUCommandCoder::sendBoardControllerInstruction(uint64_t BoardControllerInstruction){
  // This is a write instruction consisting of two 22bit words to be put into the RCU Instruction Memory
  // The first word is the command and register adress, the second is the data
  return (BoardControllerInstruction&0xFFFFFFFF) | ((uint64_t)instruction(eTarget(FEC_WR),
									  BoardControllerInstruction>>32)<<32);
}

//===========================================================================
uint32_t RCUCommandCoder::sendBoardControllerCommand(uint32_t BoardControllerInstruction){
  // This is a command consisting of one 22bit word to be put into the RCU Instruction Memory
  uint32_t retval = instruction(eTarget(FEC_CMD), BoardControllerInstruction);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::sendReadInstruction(uint32_t Instruction){
  uint32_t retval = instruction(eTarget(FEC_RD), Instruction);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::sendLoop(uint32_t numofLoops, uint32_t address){
  uint32_t command = (eRCUCommand(LOOP)<<16) | ((numofLoops&ci07BitOn)<<8) | (address&ci08BitOn);
  uint32_t retval = instruction(eTarget(IS_CMD), command);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::sendENDSEQ(){
  return sendEND();
}

//===========================================================================
uint32_t RCUCommandCoder::sendEND(){
  uint32_t retval = instruction(eTarget(IS_CMD), eRCUCommand(ENDSEQ)<<16);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::sendENDMEM(){
  uint32_t retval = instruction(eTarget(IS_CMD), eRCUCommand(ENDMEM)<<16);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::sendWAIT(uint32_t numofCycles){
  if ( numofCycles < 1 )
    cout << "RCUCommandCoder::sendWAIT: Wrong number of cycles: N_wait < 1!" << endl;
  uint32_t command = (eRCUCommand(WAIT)<<16) | (numofCycles&ci16BitOn);
  uint32_t retval = instruction(eTarget(IS_CMD), command);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeALTROIF(uint32_t err_check, uint32_t cstb_delay,
				      uint32_t clk_ratio, uint32_t nsamples_ev) {
  // Altro Interface Register
  // err_check   : 0 -> IMEM content is not checked. 1 (2, 3) -> check against TPC (PHOS, FMD) instr. set. 
  // cstb_delay  : 0x3 for TPC?
  // clk_ratio   : 0 (1, 2, 3) -> 20Mhz (10, 5, 2.5MHz)
  // n_samples_ev: number of time bins. Same as in ALTRO
  uint32_t retval = ( (err_check&ci02BitOn)<<16 ) | ( (cstb_delay&ci02BitOn)<<14 ) |
    ( (clk_ratio&ci04BitOn)<<10 ) | (nsamples_ev&ci10BitOn);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeALTROIF(uint32_t cstb_delay, uint32_t clk_ratio,
				      uint32_t nsamples_ev) {
  // Altro Interface Register
  uint32_t retval = codeALTROIF(1, cstb_delay, clk_ratio, nsamples_ev);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeTRGCONF(uint32_t trg_src, uint32_t trg_mode, uint32_t l2latency) {
  // Trigger Configuration Register
  // trg_src  : 1 -> software; 2 -> axilliary; 4 -> CTP-TTC (default); 6 -> Test mode with L1accept as axilliary trigger
  // trg_mode : 0 for TPC, 1 for PHOS (L0 from CTP is delivered as L1 to FEC)
  // l2latency: Only for softw TRG: 0x1fff 
  uint32_t retval = ( (trg_src&ci03BitOn)<<14 ) | ( (trg_mode&ci01BitOn)<<13 ) | (l2latency&ci13BitOn);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeTRGCONF(uint32_t trg_mode) {
  // Simplified function to get trigger configuration register for TPC
  uint32_t retval = codeTRGCONF(4, trg_mode, 0x1fff);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeRDOMOD(uint32_t empty_channel_skip, uint32_t dis_check_chadd_msmtch,
				     uint32_t dis_check_bl_msmtch, uint32_t dis_check_rdyrx,
				     uint32_t sparse_rdo, uint32_t exec_seq, uint32_t meb_mode) {
  // Readout Mode Register
  // empty_channel_skip:     1 -> RCU will skip the empty channel while performing Full Readout
  // dis_check_chadd_msmtch: 1 -> disable check of channel address mismatch (default is 0)
  // dis_check_bl_msmtch   : 1 -> disable check of block length mismatch (default is 0)
  // dis_check_rdyrx : Disable check ready-to-receive (open link) before data is to be received
  // sparse_rdo      : Readout optimisation. Read only channels (including overhead = adress words) with data
  // exec_seq        : 1 -> Execute sequence in IMEM on SOE, EOE. 1 for FMD. 0 for TPC.
  // meb_mode        : Number of multi event buffers (0x0 -> 4 buffers; 0x1 -> 8 buffers)
  uint32_t retval = ( (empty_channel_skip&ci01BitOn)<<6 ) | ( (dis_check_chadd_msmtch&ci01BitOn)<<5 )
    | ( (dis_check_bl_msmtch&ci01BitOn)<<4 ) | ( (dis_check_rdyrx&ci01BitOn)<<3 )
    | ( (sparse_rdo&ci01BitOn)<<2 ) | ( (exec_seq&ci01BitOn)<<1 ) | (meb_mode&ci01BitOn);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeRDOMOD(uint32_t empty_channel_skip, uint32_t sparse_rdo,
					 uint32_t meb_mode) {
  // Simplified function to get readout mode register for TPC
  uint32_t retval = codeRDOMOD(empty_channel_skip, 0, 0, 0, sparse_rdo, 0, meb_mode);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeALTROCFG1(uint32_t zs_cfg, uint32_t bc2_cfg, uint32_t bc1_cfg) {
  // code the ALTROCFG1 register. Holds a copy of the corresponding altro registers.
  uint32_t retval = ( (zs_cfg&ci08BitOn)<<12 ) | ( (bc2_cfg&ci07BitOn)<<5 ) | (bc1_cfg&ci05BitOn) ;
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeALTROCFG2(uint32_t pwsv, uint32_t flt_en, uint32_t nbuf,
					uint32_t ptrg) {
  // code the ALTROCFG2 register. Holds a copy of the corresponding altro registers.
  uint32_t retval = ( (pwsv&ci01BitOn)<<6 ) | ( (flt_en&ci01BitOn)<<5 ) | ( (nbuf&ci01BitOn)<<4 )
    | (ptrg&ci04BitOn);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeRCUID(uint32_t side, uint32_t sector, uint32_t rcu) {
  // code the RCUID
  uint32_t retval = ( (side&ci01BitOn)<<8 ) | ( (sector&ci05BitOn)<<3 ) | (rcu&ci03BitOn);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeTTC_CONTROL(uint32_t sbc_on, uint32_t dis_err_mask,
					  uint32_t en_roi_dec, uint32_t l0_support,
					  uint32_t l2a_fifo_mask, uint32_t l2r_fifo_mask,
					  uint32_t l2_tout_mask, uint32_t l1_msg_mask,
					  uint32_t bunch_cnt_ovflw, uint32_t run_active,
					  uint32_t busy, uint32_t cdh_version) {
  // code the TTC_COMMAND register. TO BE CHECKED
  uint32_t retval = ((cdh_version&ci04BitOn)<<20) | ((busy&ci01BitOn)<<18) | ((run_active&ci01BitOn)<<17)
    | ((bunch_cnt_ovflw&ci01BitOn)<<16) | ((l1_msg_mask&ci01BitOn)<<11) | ((l2_tout_mask&ci01BitOn)<<10)
    | ((l2r_fifo_mask&ci01BitOn)<<9) | ((l2a_fifo_mask&ci01BitOn)<<8) | ((l0_support&ci01BitOn)<<3)
    | ((en_roi_dec&ci01BitOn)<<2) | ((dis_err_mask&ci01BitOn)<<1) | (sbc_on&ci01BitOn); 
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeTTC_L1_LATENCY(uint32_t latency, uint32_t window) {
  // Code TTC_L1_LATENCY
  uint32_t retval = ((window&ci04BitOn)<<12) | (latency&ci12BitOn);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeTTC_L1_MSG_LATENCY(uint32_t min, uint32_t max) {
  // Code TTC_L1_MSG_LATENCY
  uint32_t retval = ((min&ci16BitOn)<<16) | (max&ci16BitOn);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeTTC_L1_MSG_LATENCY() {
  // Code TTC_L1_MSG_LATENCY
  uint32_t retval = codeTTC_L1_MSG_LATENCY(0x28, 0xf8);
  return retval;
}

//===========================================================================
uint32_t RCUCommandCoder::codeTTC_L2_LATENCY(uint32_t min, uint32_t max) {
  // Code TTC_L2_LATENCY
  uint32_t retval = ((min&ci16BitOn)<<16) | (max&ci16BitOn);
  return retval;
}


//
// Private functions
//

//===========================================================================
uint32_t RCUCommandCoder::instruction(eTarget Target, uint32_t instruction){
  // ALTRO bus instruction for RCU Fw V2
  uint32_t retval;
  retval = (eTarget(Target)<<20) | (instruction&ci20BitOn);
  return retval;
}