#include <iostream>
using namespace std;

#include <ctype.h>

#include "RodModule.h"

#include "RCCVmeInterface.h"

int main(int argc, char *argv[]) {

using namespace SctPixelRod;

  const unsigned long mapSize=0xc00040;         // Map size
  const long numSlaves=4;                       // Number of slaves
  long numWords;                                // Number of words to dump
  long dspStart, dspEnd;                        // Address limits to dump
  
  string fileName(""), option;
  bool safeMode = false;
  int slot = -1;
  unsigned long baseAddress;

  if (argc > 1) {
    for (int i=1; i<argc; i++) {
      option = argv[i];
      if (option[0] != '-') break;
      switch (option[1]) {
        case 'f': {
          safeMode = true;
          break;
        }
        case 's': {
          slot = atoi(option.substr(2).c_str());
          break;
        }
        default: {
          break;
        }
      }
    }
  }

// Prompt for slot number
  if (slot < 0 ) {
    cout << "Enter slot number (decimal):"; 
    cin >> slot;
    while ((slot < 1) || (slot > 21)) {
      cout << "Slot number out or range [1:21], re-enter: ";
      cin >> slot;
    }
  }
  baseAddress = slot << 24;

// Create VME interface
  RCCVmeInterface *vme1 = new RCCVmeInterface();
  
// Create RodModule and initialize it
  RodModule* rod0 = new RodModule(baseAddress, mapSize, *vme1, numSlaves);

  if(!safeMode) {
    try{
      rod0->initialize();
    }
    catch (BaseException &b) {
      cout << "Exception initing ROD:\n" << b << endl;
      cout << "Try the safe mode (-f)\n";
      exit(0);
    }
  } else {
    try {
      unsigned long int fpgaHold = 0x40;
      unsigned long int mdspReset = 0x2;

      cerr << "Put FPGAs on reset hold\n";
      // Put FPGAs on hold
      rod0->hpiLoad(FPGA_CONTROL_REG_REL_ADDR[1], fpgaHold);
      rod0->sleep(1000);
      // Reset MDSP 
      rod0->hpiLoad(FPGA_CONTROL_REG_REL_ADDR[2], mdspReset);

      rod0->sleep(1000);

      unsigned long hpicValue = 0x00010001;
      rod0->hpiLoad(HPIC, hpicValue);

      rod0->sleep(1000);
      // mdspSingleRead/Write use HPID not HPID++ (not allowed)

      cerr << "Read from EMIF (1)\n";
      for(unsigned long addr = 0x01800000; addr < 0x0180001c; addr+=4) {
        unsigned long val = rod0->mdspSingleRead(addr);
        cerr << "0x" << hex << addr << ": 0x" << val << dec << endl;
      } 

      rod0->sleep(100);

      cerr << "Write CE space setup\n";
      rod0->mdspSingleWrite(0x01800004, 0xffff3f03);
      rod0->mdspSingleWrite(0x01800008, 0x00000040);
      rod0->mdspSingleWrite(0x01800010, 0xffff3f33);
      rod0->mdspSingleWrite(0x01800014, 0xffff3f33);

      rod0->sleep(500);

      cerr << "Read from EMIF (2)\n";

      // mdspSingleRead/Write use HPID not HPID++ (not allowed)

      for(unsigned long addr = 0x01800000; addr < 0x0180001c; addr+=4) {
        unsigned long val = rod0->mdspSingleRead(addr);
        cerr << "0x" << hex << addr << ": 0x" << val << dec << endl;
      } 
    }
    catch (BaseException &b) {
      cout << "Exception \"initing\" ROD:\n" << b << endl;
      exit(0);
    }

    // Two seconds has failed
    rod0->sleep(3000);
  }

// get starting address and number of bytes to read

  dspStart = 0x01400000;
  dspEnd = dspStart + 0x80000;

  numWords = (dspEnd +1 - dspStart)/4;

  cout << "Reading from 0x" << hex << dspStart << dec << endl;

// create buffer and read data
  unsigned long * buffer = new unsigned long[numWords];

  if(!safeMode) {
    try{
      rod0->mdspBlockRead(dspStart, buffer, numWords);
    } catch (BaseException &b) {
      cout << "Exception Reading from ROD:\n" << b << endl;
    }
  } else {   // No block reads, no AUTO, if this doesn't work...
    try{
      for(int i=0; i<numWords; i++) {
        buffer[i] = rod0->mdspSingleRead(dspStart + (i*4));
      }
    } catch (BaseException &b) {
      cout << "Exception Reading from ROD:\n" << b << endl;
    }
  }

  ofstream outFile("MdspFlashSave.bin", ios::binary);
  outFile.write((char *)buffer, numWords*4);

// Clean up before exiting
  delete rod0;
  delete vme1;

  return 0;  
}