#include "TPCCommandCoderHardcoded.h"

#define CoCoVersion "TPCCommandCoderHardcoded"

/*
 * A Command Coder with hardcoded values. Mainly for test purposes.
 */

//===========================================================================
TPCCommandCoderHardcoded::TPCCommandCoderHardcoded() {

  frcu       = new RCUCommandCoder();
  faltro     = new AltroCommandCoder();
  fbc        = new BoardControllerCommandCoder();
  fDataBlock = new RcuContainer();
  frcuce     = new RCUControlEngine();
  fAFL       = new Afl();

  version = CoCoVersion;

  fNfecs     = 1;
  fNaltros   = 8;
  fNchannels = 16;

}

//===========================================================================
TPCCommandCoderHardcoded::~TPCCommandCoderHardcoded() {

  delete frcu;
  frcu = 0;
  delete faltro;
  faltro = 0;
  delete fbc;
  fbc = 0;
  delete frcuce;
  frcuce = 0;
  delete fDataBlock;
  fDataBlock = 0;
  delete fAFL;
  fAFL = 0;
}

//===========================================================================
void TPCCommandCoderHardcoded::reset() {
  fDataBlock->reset();
}

//===========================================================================
void TPCCommandCoderHardcoded::setReadResult(bool read, int32_t readsize) {
  fReadResult = read;
  if (read) fDataBlock->setReadResult(readsize);
};

//===========================================================================
uint32_t* TPCCommandCoderHardcoded::getDataBlock() {
  return fDataBlock->getDataBlock();
}

//===========================================================================
int32_t TPCCommandCoderHardcoded::createDataBlock(char* target, int32_t tag){
  //
  // Create the Data Block
  //

  // Get side, sector and partition from target name (e.g. TPC-FEE_1_12_1)                                                 
  string strtarget = target;

  uint32_t pos1_ = strtarget.find( "_", 0         );
  uint32_t pos2_ = strtarget.find( "_", (pos1_+1) );
  uint32_t pos3_ = strtarget.find( "_", (pos2_+2) );

  //int side   = strtonum(strtarget.substr((pos1_+1), (pos2_-pos1_-1 )));
  //int32_t sector = strtonum(strtarget.substr((pos2_+1), (pos3_-pos2_-1 )));
  int32_t rcu    = strtonum(strtarget.substr((pos3_+1), (strtarget.size()-pos3_-1)));

  int32_t branch = 0;
  int32_t fec = 0;

  fAFL->reset();

  fDataBlock->setHardwareAddress(0xabc);       // dummy
  fDataBlock->setExternalConfiguration(false); // DCS only

  if ( !fReadResult ) { // WRITE

    // Reset
    fDataBlock->fillRcu((uint32_t)frcuce->sendGLB_RESET());

    // Fill and send AFL
    for (int32_t ffec=0; ffec<fNfecs; ffec++) {
      if (rcu==1) {
	if (ffec<13) fAFL->fill(0, ffec,    true);
	else         fAFL->fill(1, ffec-13, true);
      } else if (rcu >2) {
	if (ffec<10) fAFL->fill(0, ffec,    true);
	else         fAFL->fill(1, ffec-10, true);
      } else { // rcu 0 || rcu 2
	if (ffec<9 ) fAFL->fill(0, ffec,    true);
	else         fAFL->fill(1, ffec-9,  true);
      }
    }

    if (fNfecs>0) {
      fDataBlock->fillRcu((uint32_t)frcuce->sendRCU_WRITE_AFL(), false);
      fDataBlock->fillRcu(fAFL->get());  // payload
    }

    // Reset
    fDataBlock->fillRcu((uint32_t)frcuce->sendRCU_RESET());
    fDataBlock->fillRcu((uint32_t)frcuce->sendFEC_RESET());

    // Disable Interrupt handling before ALTRO and BC config
    fDataBlock->fillRcu((uint32_t)frcuce->sendRCU_WRITE_EN_INT_BA(), false);
    fDataBlock->fillRcu((uint32_t)0);

    // Enable Interrupt handling after ALTRO andBC config
    fDataBlock->appendRcu((uint32_t)frcuce->sendRCU_WRITE_EN_INT_BA(), false);
    fDataBlock->appendRcu((uint32_t)0x3);

    for (int32_t ffec = 0; ffec<fNfecs; ffec++) {
      if (rcu==1) {
	if (ffec<13) { branch = 0; fec = ffec;    }
	else         { branch = 1; fec = ffec-13; }
      } else if (rcu >2) {
	if (ffec<10) { branch = 0; fec = ffec;    }
	else         { branch = 1; fec = ffec-10; }
      } else { // rcu 0 || rcu 2
	if (ffec<9 ) { branch = 0; fec = ffec;    }
	else         { branch = 1; fec = ffec-9;  }
      }
      for (int32_t altro=0; altro<fNaltros; altro++) {
	
	fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendZSTHR(branch, fec, altro, (branch<<15)|(fec<<11)|(altro<<8)|8)));
	fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendBCTHR(branch, fec, altro, (branch<<15)|(fec<<11)|(altro<<8)|9)));
	fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendTRCFG(branch, fec, altro, (branch<<15)|(fec<<11)|(altro<<8)|10)));
	fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendDPCFG(branch, fec, altro, (branch<<15)|(fec<<11)|(altro<<8)|11)));
	fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendDPCF2(branch, fec, altro, (branch<<15)|(fec<<11)|(altro<<8)|12)));

	for ( int32_t channel = 0; channel < fNchannels; channel++ ) {
	  fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendK1(branch, fec, altro, channel, (branch<<15)|(fec<<11)|(altro<<8)|(channel<<4)|1)));
	  fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendK2(branch, fec, altro, channel, (branch<<15)|(fec<<11)|(altro<<8)|(channel<<4)|2)));
	  fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendK3(branch, fec, altro, channel, (branch<<15)|(fec<<11)|(altro<<8)|(channel<<4)|3)));
	  fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendL1(branch, fec, altro, channel, (branch<<15)|(fec<<11)|(altro<<8)|(channel<<4)|4)));
	  fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendL2(branch, fec, altro, channel, (branch<<15)|(fec<<11)|(altro<<8)|(channel<<4)|5)));
	  fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendL3(branch, fec, altro, channel, (branch<<15)|(fec<<11)|(altro<<8)|(channel<<4)|6)));
	  fDataBlock->fillAltro(branch, fec, altro, frcu->sendAltroInstruction(faltro->sendVFPED(branch, fec, altro, channel, (branch<<15)|(fec<<11)|(altro<<8)|(channel<<4)|7)));
	} // channel

      } // altro

      if ( fConfigBCs ) {

	// We start with writing the AC threshold only
	float AC_TH = 0.9;
	fDataBlock->fillBc(branch, fec, frcu->sendBoardControllerInstruction(fbc->sendAC_TH(branch, fec, fbc->codeAC_TH(AC_TH))), false);
	// Set the interrupt mask
	uint32_t EMASK = 0x0;
	uint32_t IMASK = 0x0e5;
	fDataBlock->fillBc(branch, fec, frcu->sendBoardControllerInstruction(fbc->sendCSR0(branch, fec, fbc->codeCSR0(1, EMASK, IMASK))), false);
	// Clear CSR1
	fDataBlock->fillBc(branch, fec, (uint32_t)frcu->sendBoardControllerCommand(fbc->sendCSR1CLR(branch, fec)), false);

      }

    } // fec

    // Store the data on the DCS board and execute the instructions from the locally saved data
    if (fStoreData) fDataBlock->setConfigurePhysics1();

  } else { // READ

    for (int32_t ffec=0; ffec<fNfecs; ffec++) {
      if (rcu==1) {
	if (ffec<13) { branch = 0; fec = ffec;    }
	else         { branch = 1; fec = ffec-13; }
      } else if (rcu >2) {
	if (ffec<10) { branch = 0; fec = ffec;    }
	else         { branch = 1; fec = ffec-10; }
      } else { // rcu 0 || rcu 2
	if (ffec<9 ) { branch = 0; fec = ffec;    }
	else         { branch = 1; fec = ffec-9;  }
      }
      for (int32_t altro=0; altro<fNaltros; altro++) {

	fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveZSTHR(branch, fec, altro)));
	fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveBCTHR(branch, fec, altro)));
	fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveTRCFG(branch, fec, altro)));
	fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveDPCFG(branch, fec, altro)));
	fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveDPCF2(branch, fec, altro)));

	for (int32_t channel = 0; channel < fNchannels; channel++ ) {
	  fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveK1(branch, fec, altro, channel)));
	  fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveK2(branch, fec, altro, channel)));
	  fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveK3(branch, fec, altro, channel)));
	  fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveL1(branch, fec, altro, channel)));
	  fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveL2(branch, fec, altro, channel)));
	  fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveL3(branch, fec, altro, channel)));
	  fDataBlock->fillAltro(branch, fec, altro, (uint32_t)frcu->sendReadInstruction(faltro->receiveVFPED(branch, fec, altro, channel)));
	} // channel

      } // altro
      
      if ( fConfigBCs ) {
	fDataBlock->fillBc(branch, fec, (uint32_t)frcu->sendReadInstruction(fbc->receiveAC_TH(branch, fec)));
	fDataBlock->fillBc(branch, fec, (uint32_t)frcu->sendReadInstruction(fbc->receiveCSR0(branch, fec)));
      }

    } // fec

  } // end if

  // Finalize data block
  fDataBlock->finalize();
  return (int32_t)(fDataBlock->size()*4);
}

//===========================================================================
vector<string> &TPCCommandCoderHardcoded::getError(){
  return ferror;
}

//===========================================================================
std::string& TPCCommandCoderHardcoded::getCoCoInfo(){
  return version;
}

//===========================================================================
int32_t TPCCommandCoderHardcoded::strtonum(string number) {
  //
  // convert string to number
  //
  char * strolConversionCheck;
  if ( number[0] == '0' ) number.erase(0, 1);
  int32_t retval = strtol(number.c_str(), &strolConversionCheck, 0);
  return retval;
}

//===========================================================================
CommandCoderBase* getCoCoInst(void){
  static TPCCommandCoderHardcoded instance;
  return &instance;
}