#include <iostream>
using namespace std;

#include <ctype.h>

#include <sys/time.h>

#include "RodModule.h"

#include "primParams.h"
#include "RCCVmeInterface.h"
#include "parameters.h"

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

using namespace SctPixelRod;

  RodPrimList primList(1);                      // Primitive List
  long myTextLength;                            // Actual length of text message
  TEXT_BUFFER_TYPE myTextType;                  // Buffer type for latest message
  PrimState returnPState;
  TextBuffState returnTState;
  std::string ipramFile("../Dsp/Pixel/Binary/sdsp_ipram.bin");
  std::string idramFile("../Dsp/Pixel/Binary/sdsp_idram.bin");
  std::string extFile("../Dsp/Pixel/Binary/sdsp_xcode.bin");

  int slaveNumber=-1;
  std::string fileName(""), option;
  bool initRod = true;
  int slot = -1;

  unsigned long baseAddress, txtBuffSize;
  MdspMemoryMap* mdspMap;
  
  if (argc > 1) {
    for (int i=1; i<argc; i++) {
      option = argv[i];
      if (option[0] != '-') break;
      switch (option[1]) {
        case 'n': {
          initRod = false;
          break;
        }
        case 's': {
          slot = atoi(option.substr(2).c_str());
          break;
        }
        case 'v': {
          slaveNumber = 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);
  try{
    rod0->initialize();
  }
  catch (HpiException &h) {
    hex(cout);
    cout << h.getDescriptor() << '\n';
    cout << "calcAddr: " << h.getCalcAddr() << ", readAddr: " << 
            h.getReadAddr() << '\n';
    dec(cout);
  }
  catch (VmeException &v) {
    cout << "VmeException creating RodModule." << endl;
    cout << "ErrorClass = " << v.getErrorClass() << endl;
    cout << "ErrorCode = " << v.getErrorCode() << endl; 
  }
  catch (RodException &r) {
  cout << r.getDescriptor() <<", " << r.getData1() << ", " << r.getData2()
          << '\n';
  };

// Initialize slave
  if (slaveNumber < 0 ) {
    cout << "Enter slave number (0-3):"; 
    cin >> slaveNumber;
    while ((slaveNumber < 0) || (slaveNumber > 3)) {
      cout << "Slave number out or range [0:3], re-enter: ";
      cin >> slaveNumber;
    }
  }

  try {
    rod0->initSlaveDsp(ipramFile, idramFile, extFile, slaveNumber, 'v');
  }
  catch (RodException & r) {
    cout << r;
    return 0;
  }
  catch (NoImageFile& i) {
    cout << i;
    return 0;
  }
  catch (VmeException &v) {
    cout << "VmeException in SendPrimList." << endl;
    cout << "ErrorClass = " << v.getErrorClass() << endl;
    cout << "ErrorCode = " << v.getErrorCode() << endl; 
    return 0;
  };

// Create a buffer for text messages
  mdspMap = rod0->getMdspMap();
  txtBuffSize = mdspMap->txtBufferSize(0);
  char * myTextBuffer;
  try {
    myTextBuffer= new char[txtBuffSize];
  }
  catch (std::bad_alloc) {
    cout << "Unable to allocate text buffer in main.";
  }
  
// Create and Send a FLASH_LED primitive to a slave DSP
  FLASH_LED_IN ledData;
  ledData.ledNum = YELLOW_LED;
  ledData.period = 1000;
  ledData.numTimes = 10;
//   long ledData[3] = {0,1000,10};           //State and toggle flag
  RodPrimitive* led0;
  try {
    led0 = new RodPrimitive(4+3, 0, FLASH_LED, R_FLASH_LED, (long*)&ledData);
  }
  catch (std::bad_alloc) {
    cout << "Unable to allocate led0 primitive in main." << endl;
  }
  primList.insert(primList.begin(), *led0);
  try {
    primList.bufferBuild();
  }
  catch (PrimListException &p) {
    cout << p.getDescriptor() << " ";
    cout << p.getData1() << ", " << p.getData2() << "\n";
  };
  
// Get a copy of buffer and use SEND_SLAVE_LIST/START_SLAVE_LIST primitives
// to have MDSP send it to the slave
  long buffLength = primList.getBufferLength();
  unsigned long* slavePrimList;
  try {
    slavePrimList = new unsigned long[buffLength];
  }
  catch (bad_alloc) {
    cout << "Unable to allocate slavePrimList in main." << endl;
  };
  
  unsigned long* bufferStart = primList.getBuffer();
  for (int i=0; i< buffLength; i++) {
    slavePrimList[i] = bufferStart[i];
  }
  primList.clear();
  delete led0;
  
// Create Slave List primitives
  long slaveData[4+buffLength]; 
  slaveData[0] = slaveNumber;
  slaveData[1] = buffLength;
  slaveData[2] = DEFAULT;
  slaveData[3] = DEFAULT;
  for (int i=0; i< buffLength; i++) {
    slaveData[i+4] = slavePrimList[i];
  }
  RodPrimitive* send0;
  try {
    send0 = new RodPrimitive(8+buffLength, 0, SEND_SLAVE_LIST, 
                R_SEND_SLAVE_LIST, slaveData);
  }
  catch (bad_alloc) {
    cout << "Unable to allocate send0 primitive in main." << endl;
  };
  
  primList.insert(primList.begin(), *send0);
  long startData[3] = {slaveNumber, 1, 0};
  RodPrimitive* start0;
  try {
    start0 = new RodPrimitive(7, 0, START_SLAVE_LIST, 
                R_START_SLAVE_LIST, startData);
  }
  catch (bad_alloc) {
    cout << "Unable to allocate start0 primitive in main." << endl;
  };
  primList.insert(primList.end(), *start0);
  try {
    primList.bufferBuild();
  }
  catch (PrimListException *p) {
    cout << p->getDescriptor() << " ";
    cout << p->getData1() << ", " << p->getData2() << "\n";
  };

  struct timeval startTime;
  gettimeofday(&startTime, NULL);

  try {
    rod0->sendPrimList(&primList);
  }
  catch (HpiException *h) {
    hex(cout);
    cout << h->getDescriptor() << '\n';
    cout << "calcAddr: " << h->getCalcAddr() << ", readAddr: " << 
            h->getReadAddr() << '\n';
    dec(cout);
  }
  catch (VmeException &v) {
    cout << "VmeException in SendPrimList." << endl;
    cout << "ErrorClass = " << v.getErrorClass() << endl;
    cout << "ErrorCode = " << v.getErrorCode() << endl; 
  };

// Wait for ROD to begin executing and then wait for it to finish executing
// Check for error messages in text buffer and read them out if they exist.
// Note: this is NOT how primHandler and textHandler will be used once we
// go to threads.  This is for debugging the code only.
  do {
    try {
      returnPState = rod0->primHandler();
    }
    catch (VmeException &v) {
      cout << "VmeException in first primHandler call." << endl;
      cout << "ErrorClass = " << v.getErrorClass() << endl;
      cout << "ErrorCode = " << v.getErrorCode() << endl; 
    }
    try {
      returnTState = rod0->textHandler();
    }
    catch (VmeException &v) {
      cout << "VmeException in first textHandler call." << endl;
      cout << "ErrorClass = " << v.getErrorClass() << endl;
      cout << "ErrorCode = " << v.getErrorCode() << endl; 
    }
    if (returnTState == TEXT_RQ_SET) {
      do {
        try {
          returnTState = rod0->textHandler();
          }
        catch (VmeException &v) {
          cout << "VmeException in second textHandler call." << endl;
          cout << "ErrorClass = " << v.getErrorClass() << endl;
          cout << "ErrorCode = " << v.getErrorCode() << endl; 
        }
      } while (returnTState != TEXT_READOUT);
      rod0->getTextBuffer(myTextBuffer, myTextLength, myTextType);
      rod0->clearTextBuffer();
      for (int i=0; i<myTextLength; i++) {
        cout << myTextBuffer[i];
        if (0==(i+1)%64) cout << endl;
        }
      cout << endl; 
    }
  } while (returnPState != PRIM_EXECUTING); 
  do {
    try {
      returnPState = rod0->primHandler();
    }
    catch (RodException *r) {
    cout << r->getDescriptor() <<", " << r->getData1() << ", " << r->getData2()
            << '\n';
    }
    catch (VmeException &v) {
      cout << "VmeException in second primHandler call." << endl;
      cout << "ErrorClass = " << v.getErrorClass() << endl;
      cout << "ErrorCode = " << v.getErrorCode() << endl; 
    }
  } while ((returnPState != PRIM_WAITING)&&(returnPState != PRIM_IDLE)); 
    
  struct timeval endTime;
  gettimeofday(&endTime, NULL);

  long diff = (endTime.tv_sec - startTime.tv_sec) * 1000000;
  diff += (endTime.tv_usec - startTime.tv_usec);

  cout << "LED flashing took " << ((float(diff))/1e6) << " seconds\n";

// Clean up: clear primList, delete primitive, delete the outList
  primList.clear();
  delete send0;
  delete start0;
  delete [] slavePrimList;
  delete [] myTextBuffer;
  rod0->deleteOutList();
  
// Delete the ROD and VME objects before exiting
  delete rod0;
  delete vme1;

  return 0;  
}