#include "MonitorCommander.hh" 

#include "TFBPacketHandler.hh"


#include<iostream>
#include<fstream>



namespace T2KDAQ
{

  MonitorCommander::MonitorCommander():
    mFillLevel(0), 
    mCmdCount(0), 
    mReturnPktSize(0)
  {


  }

  MonitorCommander::~MonitorCommander()
  {
    
  }
  
  void MonitorCommander::ResetBuffer()
  {
    mFillLevel=0;    
    mCmdCount=0;
    mReturnPktSize=0;
  }
  

    
  void MonitorCommander::AddRead(unsigned short aSlaveAddr,
				 bool aWordXfer)
  {
    
    aSlaveAddr&=0x7f; // mask ls 7 bits.
    aSlaveAddr|=0x80;  // set the read bit: 
    if(aWordXfer){
      // set the word transfer: 
      aSlaveAddr|=0x100;
    }
    // we don't want to do anything else: just load this word:
    mBuf[mFillLevel++]=aSlaveAddr;
    mCmdCount++;
    mReturnPktSize+=2;
  }
    
  void MonitorCommander::AddRead(unsigned short aSlaveAddr,
				 unsigned short aIntAddr,
				 bool aWordXfer)
  {
    aSlaveAddr&=0x7f; // mask ls 7 bits.
    aSlaveAddr|=0x80;  // set the read bit: 
    if(aWordXfer){
      // set the word transfer: 
      aSlaveAddr|=0x100;
    }
    aSlaveAddr|=0x200;// set the address transfer bit
    aIntAddr&=0xff;// mask off the internal address
    
    // set the buffer accordingly:
    mBuf[mFillLevel++]=aSlaveAddr;
    mBuf[mFillLevel++]=aIntAddr;
    mCmdCount++;
    mReturnPktSize+=2;
  }
    
  void MonitorCommander::AddWrite(unsigned short aSlaveAddr,
				  unsigned short aDatum,
				  bool aWordXfer)
  {
    aSlaveAddr&=0x7f; // mask ls 7 bits.
    if(aWordXfer){
      // set the word transfer: 
      aSlaveAddr|=0x100;
    }
    mBuf[mFillLevel++]=aSlaveAddr;
    
    if(aWordXfer){
      mBuf[mFillLevel++]=aDatum;
    }else{
      mBuf[mFillLevel++]=((aDatum&0xff)<<8);
    }
    
    mCmdCount++;
    mReturnPktSize++;
  }
    
  void MonitorCommander::AddWrite(unsigned short aSlaveAddr,
				  unsigned short aIntAddr,
				  unsigned short aDatum,
				  bool aWordXfer)
  {
    aSlaveAddr&=0x7f; // mask ls 7 bits.
    if(aWordXfer){
      // set the word transfer: 
      aSlaveAddr|=0x100;
    }
    aSlaveAddr|=0x200;// set the address transfer bit
    aIntAddr&=0xff;// mask off the internal address
    
    mBuf[mFillLevel++]=aSlaveAddr;
    mBuf[mFillLevel++]=aIntAddr;
    if(aWordXfer){
      mBuf[mFillLevel++]=aDatum;
    }else{
      mBuf[mFillLevel++]=((aDatum&0xff)<<8);
    }
    mCmdCount++;
    mReturnPktSize++;
  }

  void MonitorCommander::AddPause(unsigned short aLen){
    aLen|=0x8000;
    mBuf[mFillLevel++]=aLen;
  }
    
  unsigned short MonitorCommander::GetFillLevel()const
  {
    return mFillLevel;
  }

  unsigned short MonitorCommander::GetCommandCount()const{
    return mCmdCount;
  }

  unsigned short MonitorCommander::GetDataPacketSize()const{
    return mReturnPktSize;
  }
  
  
  void MonitorCommander::GetPacket(TFBPacketHandler & aPkt)const
  {
    aPkt.SetTFBChannel(0);
    aPkt.SetPayload(mBuf, mFillLevel);
    aPkt.AddCRC();
  }
  
  void MonitorCommander::MakeSimScript(const std::string & aFileName){
    std::ofstream lOutput(aFileName.c_str());
    this->MakeSimScript(lOutput);
    lOutput.close();
  }

  void MonitorCommander::MakeSimScript(std::ostream & aOs)
  {
    
    aOs<<"restart -f \n";
    aOs<<"force -freeze clk 1 0, 0 {25000 ps} -r 50ns \n";
    aOs<<"force rst 1 \n";
    aOs<<"force activate 0 \n";
    aOs<<"force rst_cmd_addr 0 \n";
    aOs<<"force cmd 0101010101001010 \n";
    aOs<<"force load_cmd 0 \n";
    aOs<<"force rd_en 0 \n";
    //aOs<<"force i2c_status 00000000 \n";
    aOs<<"force i2c_din 0000000000000000 \n";
    //aOs<<"force i2c_error 0 \n";
    aOs<<"force readback 0 \n";
    aOs<<"force interval 00000000000000000000000000100000 \n";
    aOs<<"run 200ns \n";
    aOs<<"force rst 0 \n";
    aOs<<"run 50ns \n";
    aOs<<"run 500ns \n";
    
    aOs<<"force load_cmd 1 \n";

    for(unsigned short i(0); i<mFillLevel; i++){
      aOs<<"force cmd ";
      this->PrintBinary(aOs, mBuf[i]);
      aOs<<"\nrun 50ns\n";
    }    
    aOs<<"force load_cmd 0 \n";

    aOs<<"run 500ns \n";
    
    aOs<<"force readback 1 \n";
    aOs<<"run 50ns \n";
    aOs<<"force readback 0 \n";
    aOs<<"run 3us \n";
    aOs<<"force activate 1 \n";
    
    unsigned short lRunTime=0;
    for(unsigned short i(0); i<mCmdCount; i++){
      lRunTime+=(50 * 125);
      
       //       //aOs<<"force i2c_status 00000010 \n";
//       //aOs<<"run 2us \n";
//       //aOs<<"force i2c_status 00000000 \n";
      
     }
    aOs<<"run "<<lRunTime<<"ns \n";
//     aOs<<"run 2000ns \n";
    
//     for(unsigned short i(0); i<mCmdCount; i++){
//       aOs<<"run 500ns \n";
//       aOs<<"force i2c_status 00000010 \n";
//       aOs<<"run 2us \n";
//       aOs<<"force i2c_status 00000000 \n";
//     } 
    
//     aOs<<"run 1us \n";
  }

  void MonitorCommander::PrintBinary(std::ostream & aOs, unsigned short aDatum)
  {
    unsigned short lMask=0x8000;
    for(int i=0; i<16; i++){
      aOs<<((aDatum&(lMask>>i))?"1":"0");
    }
  }


//   void MonitorCommander::ConvertBinary(unsigned short aVal, std::string & aBin)
//   {
//     aBin="";
    
    
//   }

  std::ostream & operator<<(std::ostream & aOs, 
			    const MonitorCommander & aMC)
  {
    for(unsigned short i(0); i<aMC.mFillLevel; i++){
      aOs<<std::dec<<i<<'\t'<<std::hex<<aMC.mBuf[i]<<'\n';
    }
    return aOs;
  }
}//~namespace T2KDAQ