// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/tiptilt_ctrl/test_tiptilt_ctrl.cpp 2012 2019-07-18 13:45:02Z depa $

#include <config.h>
#include <Nice/App/Application.h>
#include <Nice/App/ConfigClient.h>
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/ServiceWorker.h"
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/BcuInterface.h"
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/Const.h"
#include "tiptilt_ctrl/bcu/Interface/Sharknir/Bcu/OverallDiagnosticRecordRingBuffer.h"
#include "tiptilt_ctrl/bcu/util/ClockPtr.h"
#include "tiptilt_ctrl/bcu/util/NiceClock.h"
#include "tiptilt_ctrl/tiptilt_ctrl.h"
#include "util/shins_util.h"

//#include "tiptilt_ctrl/util/Eigen/Core"

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <stdint.h>
#include <sstream>
#include <cmath>
#include <CCfits>
#include <vector>
#include <Eigen/Core>
#include <fstream>
#include <algorithm>
#include <functional>
#include <chrono>
#include <thread>

#include <Ice/Initialize.h>

class test_tiptilt_ctrl: public Nice::Application
{
public:
  virtual int usage(int);
  virtual int run(int argc, char* argv[]);
  virtual void interrupCallback(int _sig);
private:
  Sharknir::BcuServiceWorkerInterfacePtr m_bcuService;
  std::valarray<double> load_fits(std::string _filename, int& _loadedActNum, int& _loadedNModes);
  Nice::SeqFloat set_PID_filter_coeff(const int& _frameRate, const int& _CCDMode);
  template <typename Derived>
  Nice::SeqInt convert_to_seqint(const Eigen::MatrixBase<Derived>& _emat);
  template <typename Derived>
  Nice::SeqFloat convert_to_seqfloat(const Eigen::MatrixBase<Derived>& _emat);
  int send_command_to_camera(const std::string& _command);
};

int
test_tiptilt_ctrl::usage(int _ret)
{
  if (_ret == EXIT_FAILURE)
    {
      std::cerr << "Usage: " << appName() << " --CONNECT:Endpoint=[..]\n";
      std::cerr << "Options:\n"
        "-h, --help                                       Show this message.\n"
        "-v, --version                                    Display version.\n"
        "\n"
        "\n"
        "--CONNECT:Endpoint=[NAME=NODENAME,...]          NAME=NODENAME  - name of the service\n"
        "                                                PORT=intval    - tcp port of the service\n"
        "                                                HOST=string    - tcp host address of the service\n"
        "                                                MULTICAST=true - resolve service by multicast\n"
        "\n"
        "Examples:\n"
        "\n";
      std::cerr  << appName() << " --CONNECT:Endpoint=[NAME=svc,HOST=host,PORT=4711]\n";
      std::cerr  << appName() << " --CONNECT:Endpoint=[NAME=svc,PORT=4711]\n";
      std::cerr  << appName() << " --CONNECT:Endpoint=[NAME=svc,MULTICAST=1]\n"
        "\n"
        "Hints:\n"
        " Don't put spaces in commandline arguments or use quotes\n"
        " NO: --CONNECT:Endpoint=[NAME=svc, PORT=4711]\n"
        " OK: --CONNECT:Endpoint=\"[NAME=svc, PORT=4711]\"\n"
        "\n"
        "\n"
        ;
    }
  return _ret;
}



int
test_tiptilt_ctrl::run(int argc, char* argv[])
{
  I_LOG("here, beginning run");
  Ice::StringSeq strArgs = Ice::argsToStringSeq(argc, argv);
  int status = EXIT_SUCCESS;
  int loadedActNum, loadedNModes, i=0;
  int tmpInt = 0;
  float tmpFloat = 0.0;
  std::string SHINS_CONFIG = shins::util::get_env("SHINS_CONFIG");
  std::string SHINS_DATA   = shins::util::get_env("SHINS_DATA");
  std::string srvName = "DATA.TIPTILT_CTRL.SVC";
  std::string dspProgramPath;
  std::string etcPath = SHINS_CONFIG + "/etc/tiptilt_ctrl/";
  std::string timeHistoryPath = SHINS_DATA + "/time_history/";
  std::string timeHistoryFile = "shifts_spool.txt";
  std::string strMode2commands = etcPath + "ALPAO97-M2C_matrix.fits";
  std::string strDMMapFile    = etcPath + "BAX91-DM97-map.dat";
  std::string strDMFlatFile   = etcPath + "BAX91-DM97-bias.dat";
  std::string strDMSignFile   = etcPath + "BAX91-DM97-sign.dat";
  std::string strDMCoordsFile = etcPath + "BAX91-DM97-coords.dat";
  std::string line = "";

  std::ifstream ifstrm;
  std::istringstream isstream;

  // ----------------------------
  // general principal parameters
  // ----------------------------
  D_LOG("general principal parameters");
  float baseFreq       = 106.25e6;
  float boardFreq      = baseFreq/7*4;
  float dspFreq        = boardFreq*4;

  // ----------------------------
  // principal parameters for BCU
  // ----------------------------
  D_LOG("principal parameters for BCU");
  int CCDType = 0; // 0 = external OCAM;  NOT USED
  // 1 = simulated OCAM no binning;
  // 2 = simulated C-RED2 with windowing
  // 3 = simulated C-RED2 full

  int CCDMode = 7; // 6 = C-RED2 full
  // 7 = C-RED2 con windowing (96x84)
  // 8 = C-RED2 con windowing (320x256)
  // 9 = C-RED2 con windowing (64x64)
  int zonal = 0;   // set to 1 to enable zonal interaction matrix

  int CCDInputType = 1; // 0 = continuous NOT USED

  // 1 = single shot
  int paramSelector = 0;
  int diagDecFactor = 0+ 0x80000000; // bit 31 to 1 to enable savings of diagnostic frames (from DSP to SDRAM)
  int enaMasterDiag = 0;  // NOT USED

  // ----------------------------
  // mirror parameters
  // ----------------------------
  D_LOG("mirror parameters");
  uint32_t DMMaxPower  = 40;
  int masterDecFactor  = 0; // NOT USED
  // loading mode to command matrix from FITS file
  std::valarray<double> vecM2C = load_fits(strMode2commands, loadedActNum, loadedNModes);
  D_LOG("loadedActNum = " << loadedActNum); // actuator number
  D_LOG("loadedNModes = " << loadedNModes); // number of modes
  Eigen::MatrixXf DMInfluenceMatrixALPAO(loadedActNum,loadedNModes); // modes x num_actuators
  Eigen::MatrixXi matrixDMcoords(97,2); // not used in initialization
  Nice::SeqInt DMActuatorMap;  //matrixDMmap;
  Nice::SeqFloat DMFlatVectorFloat; //matrixDMFlat;
  Nice::SeqInt DMFlatVectorInt;
  Nice::SeqInt DMActuatorSign; //matrixDMSign;
  //uint32_t ModesVectorLen  = 0;
  uint32_t DMInuseActNum   = 0;
  unsigned int DMActNumber = 0;
  // unsigned int HSDLVectorLen  = 0;
  int DMOutputEnableSelect = 1;
  int TTLoopEnable   = 0;

  Nice::SeqFloat newModeVectorFloat; //(ModesVectorLen);
  Nice::SeqInt newModeVectorInt;
  Nice::SeqFloat modeVector0Float; //(ModesVectorLen);
  Nice::SeqInt modeVector0Int;
  Nice::SeqFloat modeVector1Float; //(ModesVectorLen);
  Nice::SeqInt modeVector1Int;
  // Eigen::Matrix<uint32_t, Eigen::Dynamic,1> HSDLInputVector(HSDLVectorLen,1);
  Nice::SeqInt HSDLInputVector; //(HSDLVectorLen);

  uint32_t num_rows = 0;
  uint32_t num_cols = 0;


  // ----------------------------
  // Detector parameters
  // ----------------------------
  // their init values depend on the CCDMode variable
  // -----------------------------------
  //

  // Sharknir::util::rtcDetector detector();
  D_LOG("Detector parameters");
  int CCDNumRows     = 0;
  int CCDNumCols     = 0;
  int CCDSize        = 0;
  int CCDFrameRate   = 0;
  int nSlopePixels   = 0; // deve essere divisibile per 16 e compatibile con la generazione automatica delle sottoaperture
  int IrqNiosDelay   = 0;
  int timeHistoryLen = 0;
  int pixelDecFactor = 0;
  //float HSDLCommandScale = 0; // NOT USED IS SHARK-NIr
  // float centroidGainFloat[2] = {0};
  // float centroidOrigin[2] = {0};
  std::string strPixelBias96x84 = etcPath + "20181108_113809_PixelsBias_096x084.dat"; // generated by Microgate
  std::string strPixelBias64x64 = etcPath + "20181108_152754_PixelsBias_064x064.dat"; // generated by Microgate
  Eigen::MatrixXf pixelBias(nSlopePixels,1);

  // ------------------------
  // Slope computer paramters
  // ------------------------
  Nice::SeqInt FramesCounter(1);
  Nice::SeqInt ResetLoop(1);
  Nice::SeqInt FramesCounterIrq(1);
  Nice::SeqInt ParamSelectorIrq(1);
  Nice::SeqInt DMNumActuatorsIrq(1);
  Nice::SeqInt DMNumActuators(1);
  // Nice::SeqInt DMOutputVector(DMActNumber);
  Nice::SeqInt DMOutputVector(256);
  Nice::SeqInt ParamSelector(1);
  Nice::SeqInt CCDInterfaceEnable(1);
  Nice::SeqInt CCDWaitForReset(1);
  Nice::SeqInt CCDSubFrameCnt(1);
  Nice::SeqInt CCDTotalFlux(1);
  Nice::SeqInt NewModeVectorUpd(1);
  Nice::SeqInt ModesVectorLen(1);
  Nice::SeqInt DMOutputEnable(1);
  Nice::SeqInt TTControlLoopEna(1);
  Nice::SeqInt DMInuseNumAct(1);
  Nice::SeqInt enableRTCommands(1);
  Nice::SeqInt DMUpdateStart(1);
  Nice::SeqInt NumCentroidPixels(1);
  Nice::SeqInt TimeHistoryAct(1);
  Nice::SeqFloat centroidGainFloat(2);
  Nice::SeqInt centroidGainInt;
  Nice::SeqFloat centroidOriginFloat(2);
  Nice::SeqInt centroidOriginInt;

  Nice::SeqInt HSDLVectorLen(1);
  Nice::SeqInt HSDLCmdStart(1);
  Nice::SeqInt HSDLCmdNotReadyCnt(1);
  Nice::SeqInt HSDLToSkipFrameCnt(1);
  Nice::SeqFloat pixelMinValueFloat(1);
  Nice::SeqInt pixelMinValueInt;
  Nice::SeqInt MinCentroidValue(1);
  Nice::SeqInt MaxCentroidValue(1);
  Nice::SeqInt MinTTCommandValue(1);
  Nice::SeqInt MaxTTCommandValue(1);
  Nice::SeqInt PIDDelayLinePtr(1); // this is never used!!
  Nice::SeqInt SubapIntensityThres(1);
  Nice::SeqInt HSDLCommandScale(1);
  Nice::SeqInt DiagnosticFrameDec(1);
  Nice::SeqInt PixelFrameDec(1);
  Nice::SeqInt NiosIrqDelay(1);
  Nice::SeqFloat centroidOutFloat(2);
  Nice::SeqInt centroidOutInt;
  Nice::SeqFloat PIDDelayLineFloat(12);
  Nice::SeqInt PIDDelayLineInt;
  Nice::SeqFloat PIDFilterCoeffFloat;// = set_PID_filter_coeff(CCDFrameRate, CCDMode);
  Nice::SeqInt PIDFilterCoeffInt;// = shins::util::reinterpret_float_to_int(PIDFilterCoeffFloat);

  Nice::SeqInt MaxAbsDMCommand(1);
  Nice::SeqInt MaxAbsDMCommandSkip(1);
  Nice::SeqInt DMBitOffset(1);
  float DMBitOffsetFloat = 0.;
  Nice::SeqInt DMm2bitConversion(1);
  Nice::SeqInt MaxDMPower(1);
  Nice::SeqInt DMTotalPower(1);
  Nice::SeqInt DMCmdSkippedCnt(1);
  Nice::SeqInt NewModeVectorPtr(1);
  Nice::SeqInt HSDLInputVectorPtr(1);
  Nice::SeqInt HSDLReplyVectorPtr(1);
  Nice::SeqInt NumPixelsToDoPtr(1);
  Nice::SeqInt CentroidPixelAreaPtr(1);
  Nice::SeqInt DiagBuffHeaderPtr(1);
  Nice::SeqInt DMCommandVectPtr(1);
  Nice::SeqInt DiagBuffDataPtr(1);
  Nice::SeqInt DiagBuffFooterPtr(1);
  Nice::SeqInt ModeVector0Ptr(1);
  Nice::SeqInt DMInfluenceMatrix0Ptr(1);
  Nice::SeqInt TimeHistoryPtr(1);
  Nice::SeqInt TimeHistoryEndPtr(1);
  Nice::SeqInt PixelOffsetPtr(1);
  Nice::SeqInt PixelGainPtr(1);
  Nice::SeqInt CentroidFactPtr(1);
  Nice::SeqInt PreviousPixelAreaPtr(1);
  Nice::SeqInt ModeVector1Ptr(1);
  Nice::SeqInt DMInfluenceMatrix1Ptr(1);
  Nice::SeqInt DMBiasVectorPtr(1);
  Nice::SeqInt DMActuatorSignPtr(1);
  Nice::SeqInt DMTmpOutputVectorPtr(1);
  Nice::SeqInt DMActuatorMapPtr(1);
  Nice::SeqInt DMTmpOutputVector;
  Nice::SeqInt NumPixelsToDo_A(4);
  Nice::SeqInt NumPixelsToDo_B(4);


  DMInfluenceMatrixALPAO.setZero();
  matrixDMcoords.setZero();

  D_LOG("ModesVectorLen = " << ModesVectorLen[0]);
  D_LOG("DMInuseActNum = " << DMInuseActNum);
  D_LOG("DMActNumber = " << DMActNumber);

  if (zonal == 1)
    {
      DMInfluenceMatrixALPAO = Eigen::MatrixXf::Identity(loadedActNum,loadedNModes);
    }
  else
    {
      for (uint i=0; i<vecM2C.size(); i++)
        {
          // turn vecM2C vector to matrix
          DMInfluenceMatrixALPAO(i%loadedActNum,i/loadedActNum)=vecM2C[i];
          // this matrix must have dimension as number_of_actuators x number_of_modes.
          // Then, it must be transposed. The reason is in the original M2C matrix shape
          // from ALPAO
        }
      // DMInfluenceMatrixALPAO.transposeInPlace();
    }

  D_LOG("DMInfluenceMatrixALPAO shape" << DMInfluenceMatrixALPAO.rows() << "x" << DMInfluenceMatrixALPAO.cols());
  ModesVectorLen[0]  = static_cast<unsigned int>(std::ceil((DMInfluenceMatrixALPAO.cols()+2.)/8.))*8;
  DMInuseActNum   = static_cast<unsigned int>(std::ceil(DMInfluenceMatrixALPAO.rows()/8.)*8); // number of actuators used.
  DMActNumber     = static_cast<unsigned int>(std::ceil(DMInfluenceMatrixALPAO.rows()/64.)*64);
  U1_LOG("ModesVectorLen  = " << ModesVectorLen[0]);
  U1_LOG("DMInuseActNum   = " << DMInuseActNum );
  U1_LOG("DMActNumber     = " << DMActNumber   );

  // loading actuators map
  ifstrm.open(strDMMapFile.c_str());
  while (std::getline(ifstrm, line))
    {
      isstream.str(line);
      isstream >> tmpInt; //matrixDMmap(i,0);
      DMActuatorMap.push_back(tmpInt);
      i++;
      line="";
    }
  U1_LOG("DMActuatorMap.size() = " << DMActuatorMap.size());
  i=0;
  ifstrm.close();

  // loading bias
  ifstrm.open(strDMFlatFile.c_str());
  while (std::getline(ifstrm, line))
    {
      isstream.str(line);
      isstream >> tmpFloat;// matrixDMBias(i,0);
      DMFlatVectorFloat.push_back(tmpFloat);
      i++;
      line="";
    }
  i=0;
  ifstrm.close();

  // loading actuators sign
  ifstrm.open(strDMSignFile.c_str());
  while (std::getline(ifstrm, line))
    {
      isstream.str(line);
      isstream >> tmpInt; //matrixDMSign(i,0);
      DMActuatorSign.push_back(tmpInt);
      i++;
      line="";
    }
  i=0;
  ifstrm.close();

  // loading actuators coordinates
  ifstrm.open(strDMCoordsFile.c_str());
  while (std::getline(ifstrm, line))
    {
      isstream.str(line);
      isstream >> matrixDMcoords(i,0) >> matrixDMcoords(i,1);
      i++;
      line="";
    }
  i=0;
  ifstrm.close();

  // ----------------------------
  // Detector parameters
  // ----------------------------

  pixelBias.setZero();
  I_LOG("pixelBias shape " << pixelBias.rows() << "x" << pixelBias.cols());
  switch (CCDMode)
    {
    case 1:
      CCDNumCols   =  264;
      CCDNumRows   =  242;
      nSlopePixels = 5712;
      pixelBias.resize(nSlopePixels,1);
      break;
    case 2:
      CCDNumCols   =  264;
      CCDNumRows   =  122;
      nSlopePixels = 5712;
      pixelBias.resize(nSlopePixels,1);
      break;
    case 3:
      CCDNumCols       =  264;
      CCDNumRows       =  124;
      nSlopePixels     = 5712;
      pixelBias.resize(nSlopePixels,1);
      break;
    case 4:
      CCDNumCols       =  268;
      CCDNumRows       =   82;
      nSlopePixels     = 4208;
      pixelBias.resize(nSlopePixels,1);
      break;
    case 5:
      CCDNumCols       =  268;
      CCDNumRows       =   62;
      nSlopePixels     = 2560;
      pixelBias.resize(nSlopePixels,1);
      break;
    case 6:
      CCDNumCols       =   640;
      CCDNumRows       =   512;
      nSlopePixels     =    16;
      CCDFrameRate     =   200;
      IrqNiosDelay     =  2760;
      timeHistoryLen   = 50000;
      pixelDecFactor   = 1 + 0x80000000; //1 + hex2dec('80000000') put to 1 bit 31 to enable pixel frames saving CCD ->SDRAM .
      HSDLCommandScale[0] = 0;
      centroidGainFloat[0]   = 0;
      centroidGainFloat[1]   = 0;
      centroidOriginFloat[0] = 0;
      centroidOriginFloat[1] = 0;
      pixelBias.resize(nSlopePixels,1);
      break;
    case 7:
      CCDNumCols       =    96;
      CCDNumRows       =    84;
      CCDFrameRate     =  1000;
      nSlopePixels     =  8064;
      IrqNiosDelay     =     0;
      timeHistoryLen   = 50000;
      pixelDecFactor   = 1 + 0x80000000;
      pixelBias.resize(nSlopePixels,1);
      D_LOG("pixelBias shape " << pixelBias.rows() << "x" << pixelBias.cols());

      // loading Pixels bias from file, it depends on the SPECIFIC window
      ifstrm.open(strPixelBias96x84.c_str());
      while (std::getline(ifstrm, line))
        {
          isstream.str(line);
          for (int j=0; j<CCDNumCols; j++)
            isstream >> pixelBias(i+j,0);
          i++;
          line="";
        }
      i=0;
      ifstrm.close();

      centroidGainFloat[0]   = 8.957422e+00;
      centroidGainFloat[1]   = 8.970333e+00;
      centroidOriginFloat[0] = -1.968234e-01;
      centroidOriginFloat[1] = -7.346098e-02;
      break;
    case 8:
      CCDNumCols       =   320;
      CCDNumRows       =   256;
      CCDFrameRate     =   800;
      nSlopePixels     =    16;
      IrqNiosDelay     =   700;
      timeHistoryLen   = 50000;
      pixelDecFactor   = 1 + 0x80000000;
      pixelBias.resize(nSlopePixels,1);
      D_LOG("pixelBias shape " << pixelBias.rows() << "x" << pixelBias.cols());
      break;
    case 9:
      CCDNumCols       =    64;
      CCDNumRows       =    64;
      CCDFrameRate     =  1000;
      nSlopePixels     =  4096;
      IrqNiosDelay     =     0;
      timeHistoryLen   = 50000;
      pixelDecFactor   = 1 + 0x80000000;
      pixelBias.resize(nSlopePixels,1);
      D_LOG("pixelBias shape " << pixelBias.rows() << "x" << pixelBias.cols());

      // loading Pixels bias from file, it depends on the SPECIFIC window
      ifstrm.open(strPixelBias64x64.c_str());
      while (std::getline(ifstrm, line))
        {
          isstream.str(line);
          for (int j=0; j<CCDNumCols; j++)
            isstream >> pixelBias(i+j,0);
          i++;
          line="";
        }
      i=0;
      ifstrm.close();

      centroidGainFloat[0]   = 8.957422e+00;
      centroidGainFloat[1]   = 8.970333e+00;
      centroidOriginFloat[0] = 2.077437e-01;
      centroidOriginFloat[1] = -1.581149e-01;
      break;
    default:
      // raise error for invalid CCDMode
      break;
    }

  CCDSize = CCDNumCols*CCDNumRows;
  if ((nSlopePixels%16) != 0)
    {
      // raise error for number of slope pixels not multiple of 16
      // throw(Sharknir::Bcu::BcuServiceException("The number of slope pixels is not multiple of 16"));
    }


  // ------------------------
  // Slope computer paramters
  // ------------------------
  FramesCounter[0]      = 0;
  ResetLoop[0]          = 0;
  FramesCounterIrq[0]   = 0;
  ParamSelectorIrq[0]   = 0;
  DMNumActuatorsIrq[0]  = 0;
  DMNumActuators[0]     = DMActNumber; // never changed
  U1_LOG("DMNumActuators[0] = " << DMNumActuators[0]);
  std::fill(DMOutputVector.begin(), DMOutputVector.end(), 0);
  ParamSelector[0]      = paramSelector;
  CCDInterfaceEnable[0] = 0;
  CCDWaitForReset[0]    = 0;
  CCDSubFrameCnt[0]     = 0;
  CCDTotalFlux[0]       = 0;
  NewModeVectorUpd[0]   = 0;
  // ModesVectorLen[0]     = ModesVectorLen;
  DMOutputEnable[0]     = DMOutputEnableSelect;
  TTControlLoopEna[0]   = TTLoopEnable;
  DMInuseNumAct[0]      = DMInuseActNum;
  enableRTCommands[0]   = 0;
  DMUpdateStart[0]      = 0;
  NumCentroidPixels[0]  = nSlopePixels;
  TimeHistoryAct[0]     = 0;

  // centroidGainFloat[0]       = centroidGain[0];
  // centroidGainFloat[1]       = centroidGain[1];
  centroidGainInt = shins::util::reinterpret_float_to_int(centroidGainFloat);

  // centroidOriginFloat[0]     = centroidOrigin[0];
  // centroidOriginFloat[1]     = centroidOrigin[1];
  centroidOriginInt = shins::util::reinterpret_float_to_int(centroidOriginFloat);

  HSDLVectorLen[0]        = 0;
  HSDLCmdStart[0]         = 0;
  HSDLCmdNotReadyCnt[0]   = 0;
  HSDLToSkipFrameCnt[0]   = 0;
  pixelMinValueFloat[0]        = 300.0;
  pixelMinValueInt = shins::util::reinterpret_float_to_int(pixelMinValueFloat);
  MinCentroidValue[0]     = -2;
  MaxCentroidValue[0]     =  2;
  MinTTCommandValue[0]    = -5;
  MaxTTCommandValue[0]    =  5;
  // PIDDelayLinePtr[0]      = 11.;
  SubapIntensityThres[0]  = CCDNumRows*CCDNumCols;
  std::fill(centroidOutFloat.begin(), centroidOutFloat.end(), 0.);
  centroidOutInt = shins::util::reinterpret_float_to_int(centroidOutFloat);
  std::fill(PIDDelayLineFloat.begin(), PIDDelayLineFloat.end(), 0.);
  PIDDelayLineInt = shins::util::reinterpret_float_to_int(PIDDelayLineFloat);


  // float SubapIntensityThres  = CCDNumRows*CCDNumCols;
  // float CentroidOut[2]          = {0.0};
  // float PIDDelayLine[1][12]     = {0.}; // line vector

  // set PID contreller derived coefficients
  // Eigen::MatrixXf PIDFilterCoeff(10,1);
  PIDFilterCoeffFloat = set_PID_filter_coeff(CCDFrameRate, CCDMode);
  PIDFilterCoeffInt = shins::util::reinterpret_float_to_int(PIDFilterCoeffFloat);
  D_LOG(PIDFilterCoeffInt);
  // HSDLCommandScale[0]   = HSDLCommandScale;
  DiagnosticFrameDec[0] = diagDecFactor;
  PixelFrameDec[0]      = pixelDecFactor;

  // uint32_t HSDLCommandScale   = HSDLCommandScale;
  // uint32_t DiagnosticFrameDec = diagDecFactor;
  // uint32_t PixelFrameDec      = pixelDecFactor;
  // uint32_t NiosIrqDelay;
  if (IrqNiosDelay == 0)
    {
      NiosIrqDelay[0] = 0;
    }
  else
    {
      NiosIrqDelay[0] = static_cast<unsigned int>(22.1000*IrqNiosDelay+3.3298);
    }


  MaxAbsDMCommand[0]     = 1;
  MaxAbsDMCommandSkip[0] = 0;
  DMBitOffsetFloat             = 1.;
  std::memcpy(&DMBitOffset[0], &DMBitOffsetFloat, sizeof(DMBitOffsetFloat));
  U1_LOG("DMBitOffset[0] = " << DMBitOffset[0]);
  DMm2bitConversion[0]   = 8191;
  MaxDMPower[0]          = DMMaxPower;
  DMTotalPower[0]        = 0;
  DMCmdSkippedCnt[0]     = 0;
  NewModeVectorPtr[0]     = static_cast<int>(Sharknir::Bcu::BANK_0 + 0x0000F000);
  HSDLInputVectorPtr[0]   = static_cast<int>(Sharknir::Bcu::BANK_0 + 0x0000F000);
  HSDLReplyVectorPtr[0]   = static_cast<int>(HSDLInputVectorPtr[0]+0);
  NumPixelsToDoPtr[0]     = static_cast<int>(Sharknir::Bcu::BANK_1);
  CentroidPixelAreaPtr[0] = static_cast<int>(NumPixelsToDoPtr[0]+4);
  std::cout << "CentroidPixelAreaPtr[0]" << std::hex << CentroidPixelAreaPtr[0] << std::dec << std::endl;
  DiagBuffHeaderPtr[0]    = static_cast<int>(CentroidPixelAreaPtr[0]+ nSlopePixels);
  std::cout << "DiagBuffHeaderPtr[0] = " << std::hex << DiagBuffHeaderPtr[0] << std::dec << std::endl;
  DMCommandVectPtr[0]      = static_cast<int>(DiagBuffHeaderPtr[0]+4);
  std::cout << "DMCommandVectPtr[0] = " << std::hex << DMCommandVectPtr[0] << std::dec << std::endl;
  DiagBuffDataPtr[0]       = static_cast<int>(DMCommandVectPtr[0]+DMInuseActNum);
  std::cout << "DiagBuffDataPtr[0] = " << std::hex << DiagBuffDataPtr[0] << std::dec << std::endl;
  DiagBuffFooterPtr[0]     = static_cast<int>(DiagBuffDataPtr[0]+8);
  std::cout << "DiagBuffFooterPtr[0] = " << std::hex << DiagBuffFooterPtr[0] << std::dec << std::endl;
  ModeVector0Ptr[0]        = static_cast<int>(DiagBuffFooterPtr[0]+4);
  std::cout << ModeVector0Ptr[0] << " -- " << std::hex << ModeVector0Ptr[0] << std::dec << std::endl;
  std::cout << ModesVectorLen[0] << std::endl;
  DMInfluenceMatrix0Ptr[0] = static_cast<int>(ModeVector0Ptr[0]+ModesVectorLen[0]);
  std::cout << DMInfluenceMatrix0Ptr[0] << " -- " << std::hex << DMInfluenceMatrix0Ptr[0] << std::dec << std::endl;
  TimeHistoryPtr[0]        = static_cast<int>(Sharknir::Bcu::SaveRegs + 24); // FROM mat structure or excel file (aoGetVar('_bttub_SaveRegs',0,'memPointer')+aoGetVar('_bttub_SaveRegs',0,'nrItem'));
  TimeHistoryEndPtr[0]     = static_cast<int>(TimeHistoryPtr[0]+timeHistoryLen);
  U3_LOG("TimeHistoryEndPtr[0] = " << TimeHistoryEndPtr[0]);
  PixelOffsetPtr[0]        = static_cast<int>(Sharknir::Bcu::BANK_2);
  PixelGainPtr[0]          = static_cast<int>(PixelOffsetPtr[0] + nSlopePixels);
  CentroidFactPtr[0]       = static_cast<int>(PixelGainPtr[0] + nSlopePixels);
  PreviousPixelAreaPtr[0]  = static_cast<int>(CentroidFactPtr[0] + 2*nSlopePixels);
  ModeVector1Ptr[0]        = static_cast<int>(PreviousPixelAreaPtr[0] + nSlopePixels);
  DMInfluenceMatrix1Ptr[0] = static_cast<int>(ModeVector1Ptr[0] + ModesVectorLen[0]);
  DMBiasVectorPtr[0]       = static_cast<int>(DMInfluenceMatrix1Ptr[0] + ModesVectorLen[0]*DMInuseActNum/2);
  DMActuatorSignPtr[0]     = static_cast<int>(DMBiasVectorPtr[0] + DMInuseActNum);
  DMTmpOutputVectorPtr[0]  = static_cast<int>(DMActuatorSignPtr[0] + DMInuseActNum);
  DMActuatorMapPtr[0]      = static_cast<int>(DMTmpOutputVectorPtr[0] + DMInuseActNum);
  U1_LOG("DMActuatorMapPtr = 0x" << std::hex << DMActuatorMapPtr[0] << std::dec);
  DMTmpOutputVector.resize(DMTmpOutputVectorPtr[0] - DMActuatorSignPtr[0], 0);
  std::fill(NumPixelsToDo_A.begin(), NumPixelsToDo_A.end(), 0);
  std::fill(NumPixelsToDo_B.begin(), NumPixelsToDo_B.end(), 0);

  newModeVectorFloat.resize(ModesVectorLen[0], 0.0);
  newModeVectorInt;
  modeVector0Float.resize(ModesVectorLen[0], 0.0);
  modeVector0Int;
  modeVector1Float.resize(ModesVectorLen[0], 0.0);
  modeVector1Int;
  HSDLInputVector.resize(HSDLVectorLen[0], 0);
  // NewModeVector.setZero();
  // std::fill(newModeVectorFloat.begin(), newModeVectorFloat.end(), 0.0);
  newModeVectorInt = shins::util::reinterpret_float_to_int(newModeVectorFloat);
  // ModeVector0.setZero();
  // std::fill(modeVector0Float.begin(), modeVector0Float.end(), 0.0);
  // ModeVector1.setZero();
  // std::fill(modeVector1Float.begin(), modeVector1Float.end(), 0.0);
  // HSDLInputVector.setZero();
  // std::fill(HSDLInputVector.begin(), HSDLInputVector.end(), 0);

  modeVector0Int = shins::util::reinterpret_float_to_int(modeVector0Float);
  modeVector1Int = shins::util::reinterpret_float_to_int(modeVector1Float);

  num_rows = DMInuseActNum;
  num_cols = ModesVectorLen[0];
  D_LOG("num_rows = " << num_rows);
  D_LOG("num_cols = " << num_cols);
  int mapEl = 0;
  // remapping M2C matrix before sending to BCU
  Eigen::MatrixXi subMap0(4,2);
  Eigen::MatrixXi subMap1(1,num_cols);
  Eigen::MatrixXi subMap2(num_rows,1);
  Eigen::MatrixXi DMInfluenceMatrixMap0(num_rows,num_cols);
  Eigen::MatrixXi DMInfluenceMatrixMap1(num_rows,num_cols);
  Eigen::MatrixXi DMInfluenceMatrixMap2(num_rows,num_cols);
  Eigen::MatrixXi DMInfluenceMatrixMap(num_rows,num_cols);
  Eigen::MatrixXf DMInfluenceMatrix(num_rows,num_cols);
  Eigen::MatrixXf DMInfluenceMatrixR(num_rows*num_cols,1);
  Eigen::MatrixXf DMInfluenceMatrix0(num_rows*num_cols/2,1); // to be written to DSP DONE
  Nice::SeqFloat DMInfluenceMatrix0Float(num_rows*num_cols/2);
  Nice::SeqInt DMInfluenceMatrix0Int;
  Nice::SeqFloat DMInfluenceMatrix1Float(num_rows*num_cols/2);
  Nice::SeqInt DMInfluenceMatrix1Int;
  Eigen::MatrixXf DMInfluenceMatrix1(num_rows*num_cols/2,1); // to be written to DSP DONE
  D_LOG("Declared Eigen library matrices for influence (M2C) matrix");
  subMap0.setZero();
  subMap1.setZero();
  subMap2.setZero();
  DMInfluenceMatrixMap0.setZero();
  DMInfluenceMatrixMap1.setZero();
  DMInfluenceMatrixMap2.setZero();
  DMInfluenceMatrixMap.setZero();
  DMInfluenceMatrix.setZero();
  DMInfluenceMatrixR.setZero();
  DMInfluenceMatrix0.setZero();
  DMInfluenceMatrix1.setZero();
  D_LOG("Declared Eigen library matrices");
  DMInfluenceMatrix.block(0,0,loadedActNum,1) = DMInfluenceMatrixALPAO.col(1);
  DMInfluenceMatrix.block(0,1,loadedActNum,1) = DMInfluenceMatrixALPAO.col(0);
  DMInfluenceMatrix.block(0,2,loadedActNum,1) = DMInfluenceMatrixALPAO.col(3);
  DMInfluenceMatrix.block(0,3,loadedActNum,1) = DMInfluenceMatrixALPAO.col(2);
  DMInfluenceMatrix.block(0,4,loadedActNum,loadedNModes-4) = DMInfluenceMatrixALPAO.block(0,4,loadedActNum,loadedNModes-4);
  D_LOG("Swapped cols from input M2C matrix");
  subMap0 << (Eigen::MatrixXi(4,2) << 0, 4, 2, 6, 1, 5, 3, 7).finished();
  D_LOG("Filled subMap0");
  D_LOG(subMap0);
  // creating mapping matrix
  for (unsigned int i=0; i<num_cols; i+=2)
    {
      // std::cout << i << std::endl;
      subMap1(0,i) = i*4;
      subMap1(0,i+1) = i*4;
    }
  D_LOG("Filled subMap1");
  for (unsigned int i=0; i<num_rows; i+=4)
    {
      // std::cout << i << std::endl;
      subMap2(i,0)   = i*num_cols;
      subMap2(i+1,0) = i*num_cols;
      subMap2(i+2,0) = i*num_cols;
      subMap2(i+3,0) = i*num_cols;
    }
  D_LOG("Filled subMap2");
  DMInfluenceMatrixMap0 = subMap0.replicate(num_rows/4,num_cols/2);
  DMInfluenceMatrixMap1 = subMap1.replicate(num_rows,1);
  DMInfluenceMatrixMap2 = subMap2.replicate(1,num_cols);
  D_LOG("Replicated sub matrices");
  DMInfluenceMatrixMap = DMInfluenceMatrixMap0+DMInfluenceMatrixMap1+DMInfluenceMatrixMap2;
  D_LOG("Summed submatrices together");
  // D_LOG(DMInfluenceMatrixMap)
  // remapping influence matrix
  D_LOG("remapping influence matrix ...");
  for (unsigned int i=0; i<num_rows*num_cols; i++)
    {
      // D_LOG("row index = " << i%num_rows)
      // D_LOG("col index = " << i/num_rows)
      mapEl = DMInfluenceMatrixMap(i%num_rows,i/num_rows);
      // D_LOG("mapEl = " << mapEl);
      // D_LOG("mapEl\%num_rows = " << mapEl%num_rows);
      // D_LOG("mapEl/num_rows = " << mapEl/num_rows);
      DMInfluenceMatrixR(i,0) = DMInfluenceMatrix(mapEl%num_rows,mapEl/num_rows);
    }
  D_LOG("remapping influence matrix ... DONE")
    // splitting remapped influence matrix
    D_LOG("splitting remapped influence matrix ... ");
  DMInfluenceMatrix0 = DMInfluenceMatrixR.block(0,0,num_rows*num_cols/2,1);
  DMInfluenceMatrix1 = DMInfluenceMatrixR.block(num_rows*num_cols/2,0,num_rows*num_cols/2,1);
  D_LOG("splitting remapped influence matrix ... DONE" << std::dec);
  DMInfluenceMatrix0Float = convert_to_seqfloat(DMInfluenceMatrix0);
  DMInfluenceMatrix1Float = convert_to_seqfloat(DMInfluenceMatrix1);
  DMInfluenceMatrix0Int = shins::util::reinterpret_float_to_int(DMInfluenceMatrix0Float);
  DMInfluenceMatrix1Int = shins::util::reinterpret_float_to_int(DMInfluenceMatrix1Float);
  D_LOG(DMInfluenceMatrix1Int);
  // setting bias, sign and map vectors
  // Eigen::MatrixXi DMActuatorMap(matrixDMmap.rows(),1); // to be written to DSP DONE
  // Eigen::MatrixXf DMBiasVector(DMInuseActNum,1);   // to be written to DSP DONE
  // Eigen::MatrixXi DMActuatorSign(DMInuseActNum,1); // to be written to DSP DONE
  // DMActuatorMap.setZero();
  // DMBiasVector.setZero();
  // DMActuatorSign.setZero();

  // DMBiasVector.block(0,0,matrixDMBias.rows(),1) = matrixDMBias;
  // U1_LOG("DMFlatVectorFloat.size()" << DMFlatVectorFloat.size());
  DMFlatVectorFloat.resize(DMInuseActNum, 0); // increasing size and adding 0 to additional elements.
  // U1_LOG("DMFlatVectorFloat.size()" << DMFlatVectorFloat.size());
  D_LOG("Filled DMFlatVectorFloat. Resulting in:");
  U1_LOG(DMFlatVectorFloat);
  DMFlatVectorInt = shins::util::reinterpret_float_to_int(DMFlatVectorFloat);
  // DMActuatorSign.block(0,0,matrixDMSign.rows(),1) = matrixDMSign;
  DMActuatorSign.resize(DMInuseActNum, 0);
  D_LOG("Filled DMActuatorSign");
  //DMActuatorMap = matrixDMmap;
  D_LOG("Filled DMActuatorMap");


  // ------------------------
  // camera parameters
  // ------------------------
  //
  const int pixelMapAreaSize = 2048;
  const int nSlopesTodo = 512;
  const int slopePixelMapSize = 8192;
  Eigen::Matrix<uint32_t, pixelMapAreaSize, 1> pixelInUseMask; // _ocaub_OcamInUsePixelMap
  Eigen::Matrix<uint32_t, nSlopesTodo, 1> nSlopesToDoMap; // _ocaub_OcamNumSlopeToDo
  Eigen::Matrix<uint32_t, slopePixelMapSize, 1> tmpSlopePixelMap; // pixel pointer map _ocaub_OcamSlopePixelsMap
  Eigen::Matrix<uint32_t, pixelMapAreaSize, 1> pixelToSaveMap; // _ocaub_OcamToSavePixelMap
  pixelInUseMask.setZero();
  nSlopesToDoMap.setZero();
  tmpSlopePixelMap.setZero();
  pixelToSaveMap.setZero();

  if (nSlopePixels != 0)
    {
      // fill bit mask with pixels to be copied in DSP memory
      for (int i=0; i<nSlopePixels; i++)
        {
          pixelInUseMask((i/32),0) += std::pow(2,(i%32));
        }
      D_LOG("Filled pixelInUseMask");
      // D_LOG(pixelInUseMask);
      // number of slopes to do every 16 pixels
      for (int i=0; i<(nSlopePixels/16); i++)
        {
          nSlopesToDoMap(i,0) = i*2;
        }
      D_LOG("Filled nSlopesToDoMap");
      // D_LOG(nSlopesToDoMap);
      for (int i=4; i<nSlopePixels; i++)
        {
          tmpSlopePixelMap(i,0) = i;
        }
      D_LOG("Filled tmpSlopePixelMap");
      // D_LOG(tmpSlopePixelMap);
      Eigen::Map<Eigen::Matrix<uint32_t, 4, slopePixelMapSize/4> > slopePixelMap(tmpSlopePixelMap.data(), 4, slopePixelMapSize/4);

      D_LOG("Declared and init slopePixelMap by Eigen::Map");
      // D_LOG(slopePixelMap);
      // Eigen::Matrix<uint32_t, 1, slopePixelMapSize/4> tmp;
      // tmp = slopePixelMap.row(1);
      // slopePixelMap.row(1) = slopePixelMap.row(2);
      // slopePixelMap.row(2) = tmp;
      // mat1.col(j1).swap(mat1.col(j2));
      slopePixelMap.row(1).swap(slopePixelMap.row(2));
      D_LOG("swapped 2nd with 3rd row in slopePixelMap");
      // D_LOG(slopePixelMap);
      for (int i=0; i<4; i++)
        {
          slopePixelMap(i,0) = i+4;
        }
      // D_LOG(slopePixelMap);
      // vectorizing slopePixelMap column wise
      Eigen::Map<Eigen::Matrix<uint32_t, slopePixelMapSize, 1> > slopePixelMapVec(slopePixelMap.data(), slopePixelMapSize, 1);
      slopePixelMapVec(slopePixelMapVec.rows()-1,0) += 0x8000;
      D_LOG("Declared and init slopePixelMapVec by Eigen::Map");
      // D_LOG(slopePixelMapVec);

      for (int i=0; i<(( (CCDSize - 1) < (pixelMapAreaSize*32-1)) ? (CCDSize-1) : (pixelMapAreaSize*32-1)); i++)
        {
          pixelToSaveMap((i/32)+1, 0) += std::pow(2,(i%32));
        }
      D_LOG("Filled pixelToSaveMap with upper index " << (( (CCDSize - 1) < (pixelMapAreaSize*32-1)) ? (CCDSize-1) : (pixelMapAreaSize*32-1)));
      // D_LOG(pixelToSaveMap.transpose())
    }
  else
    {
      // matrix declared above should be empitied out
    }

  // -----------------------------------
  // parameters for centroid calculation
  // -----------------------------------
  D_LOG("Parameters for centroid calculation")
    D_LOG("CCDNumCols = " << CCDNumCols)
    D_LOG("CCDNumRows = " << CCDNumRows)
    Eigen::MatrixXf subCentroidX(1,CCDNumCols);
  Eigen::MatrixXf subCentroidY(1,CCDNumRows);
  Eigen::MatrixXf centroidX(CCDNumRows, CCDNumCols);
  Eigen::MatrixXf centroidY(CCDNumRows, CCDNumCols);
  subCentroidX.setZero();
  subCentroidY.setZero();
  centroidX.setZero();
  centroidY.setZero();
  float factorX = 0;
  float factorY = 0;


  for (int i=0; i<CCDNumCols; i++)
    {
      subCentroidX(0,i) = i+0.5;
    }
  D_LOG("Filled subCentroidX")

    for (int i=CCDNumRows-1; i>0.5; i--)
      {
        subCentroidY(0,i) = i-0.5;
      }
  D_LOG("Filled subCentroidY")
    D_LOG(subCentroidY)

    subCentroidX.transposeInPlace();
  centroidX = subCentroidX.replicate(CCDNumRows,1);
  centroidX -= (CCDNumCols/2)*Eigen::MatrixXf::Ones(centroidX.rows(), centroidX.cols());
  D_LOG("Init centroidX")

    subCentroidY.transposeInPlace();
  centroidY = subCentroidY.replicate(1,CCDNumCols);
  centroidY.transposeInPlace();
  centroidY -= (CCDNumRows/2)*Eigen::MatrixXf::Ones(centroidY.rows(),centroidY.cols());
  D_LOG("Init centroidY")

    factorX = centroidX.cwiseAbs().maxCoeff();
  factorY = centroidY.cwiseAbs().maxCoeff();
  D_LOG("factorX = " << factorX)
    D_LOG("factorY = " << factorY)
    centroidX /= factorX;
  centroidY /= factorY;

  centroidX.resize(centroidX.size(),1);
  D_LOG("resized centroidX")
    centroidY.resize(centroidY.size(),1);
  D_LOG("resized centroidY")
    centroidX.transposeInPlace();
  centroidY.transposeInPlace();

  for (int i=0; i<4; i++ )
    {
      centroidX(0,i)=0;
      centroidY(0,i)=0;
    }
  num_cols = centroidX.cols();
  // Eigen::Matrix<uint32_t, 2, Eigen::Dynamic> centroid(2,centroidX.size());
  Eigen::MatrixXf centroid(2,centroidX.size());
  Eigen::MatrixXf CentroidFact(1,2*nSlopePixels);
  Nice::SeqFloat centroidFactFloat(2*nSlopePixels); // to be written to DSP DONE
  Nice::SeqInt centroidFactInt;
  centroid.setZero();
  CentroidFact.setZero();
  std::fill(centroidFactFloat.begin(), centroidFactFloat.end(), 0.0);

  centroid.row(0) = centroidX;
  centroid.row(1) = centroidY;
  D_LOG("Init centroid")
    centroid.resize(1,centroid.size());
  D_LOG("Resized centroid")
    CentroidFact = centroid.block(0,0,1,2*nSlopePixels);
  centroidFactFloat = convert_to_seqfloat(CentroidFact);
  centroidFactInt = shins::util::reinterpret_float_to_int(centroidFactFloat);
  D_LOG("Init CentroidFact")
    // -------------------
    // Defining pixel bias (called offset)
    // -------------------
    D_LOG("Defining pixel bias")
    Eigen::Matrix<uint32_t, Eigen::Dynamic, 1> PixelOffset(nSlopePixels,1);
  Nice::SeqInt mttib_PixelOffset(nSlopePixels); // to be written to DSP DONE
  PixelOffset.setZero();
  std::fill(mttib_PixelOffset.begin(), mttib_PixelOffset.end(), 0);
  D_LOG("Set to zero PixelOffset")
    // D_LOG(mttib_PixelOffset)

    PixelOffset.resize(pixelBias.rows(), pixelBias.cols());
  D_LOG("Resized PixelOffset")
    // mttib_PixelOffset = static_cast<uint32_t>(pixelBias.array().round());
    for (int i=0; i<PixelOffset.rows(); i++)
      for (int j=0; j<PixelOffset.cols(); j++)
        {
          if (((i == 0) || (i == 1)) && ((j == 0) || (j == 1)))
            PixelOffset(i,j) = 0;
          PixelOffset(i,j) = static_cast<unsigned int>(pixelBias(i,j));
        }

  mttib_PixelOffset = convert_to_seqint(PixelOffset);
  std::fill(mttib_PixelOffset.begin(), mttib_PixelOffset.begin()+4, 0);

  D_LOG("Casted to unsigned int PixelOffset");
  // -------------------
  // Defining pixel gain
  // -------------------
  // Eigen::MatrixXf PixelGain(nSlopePixels, 1);
  Nice::SeqFloat pixelGainFloat(nSlopePixels);
  Nice::SeqInt pixelGainInt; // to be written to DSP DONE

  // PixelGain.setOnes();
  // PixelGain.topLeftCorner(4,1).setZero();

  std::fill(pixelGainFloat.begin()+4, pixelGainFloat.end(), 1.0);
  std::fill(pixelGainFloat.begin(), pixelGainFloat.begin()+4, 0.0);
  pixelGainInt = shins::util::reinterpret_float_to_int(pixelGainFloat);

  D_LOG("Set to zero topLeftCorner for PixelGain");
    // D_LOG(PixelGain)

    // --------------------------
    // Time history
    // --------------------------
  float timeHistoryX = 0;
  float timeHistoryY = 0;
  float pi = std::acos(-1);
  float linspace = pi/50.;

  std::vector<float> vecTimeHistoryX;
  std::vector<float> vecTimeHistoryY;
  std::vector<float> vecTimeHistoryXDecimated;
  std::vector<float> vecTimeHistoryYDecimated;
  std::vector<float> vecTimeHistoryYInterp;
  std::vector<float> vecTimeHistoryXInterp;
  std::vector<float> vecMeanX(0);
  std::vector<float> vecMeanY(0);
  std::vector<float> vecCos1(51);
  std::vector<float> vecCos2(51);
  Nice::SeqFloat timeHistoryFloat;
  Nice::SeqInt timeHistoryInt;
  Nice::SeqFloat timeHistoryGainFloat(2);
  Nice::SeqInt timeHistoryGainInt;
  int decimationFactor = 0;
  int interpolationFactor = 0;


  ifstrm.open(timeHistoryPath+timeHistoryFile);
  while (std::getline(ifstrm, line))
    {
      isstream.str(line);
      isstream >> timeHistoryX >> timeHistoryY;
      vecTimeHistoryX.push_back(timeHistoryX);
      vecTimeHistoryY.push_back(timeHistoryY);
      line="";
    }
  D_LOG("Loaded time history from file: " << vecTimeHistoryY.size() << " lines.");
  for (std::size_t i=0; i<vecCos1.size(); i++)
    {
      vecCos1[i] = (1-(i*std::cos(linspace)))/2.;
    }
  for (std::size_t i=0; i<vecCos2.size(); i++)
    {
      vecCos2[i] = (1+(i*std::cos(linspace)))/2.;
    }

  // check frame rate and adjust time history accordingly

  if (CCDFrameRate < 1000)
    {
      decimationFactor = std::round(1000/CCDFrameRate);
      for (int i=0; i<std::round(vecTimeHistoryY.size()/decimationFactor); i+=decimationFactor)
        {
          vecTimeHistoryXDecimated.push_back(vecTimeHistoryX[i]);
          vecTimeHistoryYDecimated.push_back(vecTimeHistoryY[i]);
        }
      vecTimeHistoryY.clear();
      vecTimeHistoryX.clear();
      vecTimeHistoryY = vecTimeHistoryYDecimated;
      vecTimeHistoryX = vecTimeHistoryXDecimated;
    }
  if (CCDFrameRate > 1000)
    {
      interpolationFactor = CCDFrameRate / 1000;
      // interpolate time history
      // vecTimeHistoryYInterp = ...
      // vecTimeHistoryXInterp = ...
      vecTimeHistoryX.clear();
      vecTimeHistoryY.clear();
      vecTimeHistoryY = vecTimeHistoryYInterp;
      vecTimeHistoryX = vecTimeHistoryXInterp;
    }
  if (CCDFrameRate >= 1000)
    {
      vecTimeHistoryX.resize(timeHistoryLen/2);
      vecTimeHistoryY.resize(timeHistoryLen/2);
    }

  vecMeanX.resize(vecTimeHistoryX.size(),
                  std::accumulate(vecTimeHistoryX.begin(), vecTimeHistoryX.end(), 0) / vecTimeHistoryX.size());

  vecMeanY.resize(vecTimeHistoryX.size(),
                  std::accumulate(vecTimeHistoryY.begin(), vecTimeHistoryY.end(), 0) / vecTimeHistoryY.size());

  // removes the mean value from each decimated vector
  std::transform(vecTimeHistoryX.begin(), vecTimeHistoryX.end(),
                 vecMeanX.begin(), vecTimeHistoryX.begin(), std::minus<float>());

  std::transform(vecTimeHistoryY.begin(), vecTimeHistoryY.end(),
                 vecMeanY.begin(), vecTimeHistoryY.begin(), std::minus<float>());

  // multiplies the first vecCos.size() elements by vecCos, element-wise
  std::transform(vecTimeHistoryX.begin(), vecTimeHistoryX.begin()+vecCos1.size(),
                 vecTimeHistoryX.begin(), vecCos1.begin(), std::multiplies<float>());

  std::transform(vecTimeHistoryY.begin(), vecTimeHistoryY.begin()+vecCos1.size(),
                 vecTimeHistoryY.begin(), vecCos1.begin(), std::multiplies<float>());

  // multiplies the last vecCos.size() elements by vecCos, element-wise
  std::transform(vecTimeHistoryX.end()-vecCos2.size(), vecTimeHistoryX.end(),
                 vecTimeHistoryX.begin(), vecCos2.begin(), std::multiplies<float>());

  std::transform(vecTimeHistoryY.end()-vecCos2.size(), vecTimeHistoryY.end(),
                 vecTimeHistoryY.begin(), vecCos2.begin(), std::multiplies<float>());

  if (CCDFrameRate < 1000)
    {
      // replication depending on timeHistoryLen last step on vectors if CCDFrameRate < 1000
      if (static_cast<std::size_t>(timeHistoryLen) < vecTimeHistoryX.size())
        {
          vecTimeHistoryX.resize(timeHistoryLen);
          vecTimeHistoryY.resize(timeHistoryLen);
        }
      else if (static_cast<std::size_t>(timeHistoryLen) > vecTimeHistoryX.size())
        {
          int nRep = std::ceil((timeHistoryLen/2.)/vecTimeHistoryX.size());
          if (nRep > 1)
            {
              vecTimeHistoryX.resize(vecTimeHistoryX.size()*std::ceil((timeHistoryLen/2.)/vecTimeHistoryX.size()), 0);
              vecTimeHistoryY.resize(vecTimeHistoryY.size()*std::ceil((timeHistoryLen/2.)/vecTimeHistoryY.size()), 0);

              for (int i=1; i<nRep; i++)
                {
                  std::transform(vecTimeHistoryX.begin()+(vecTimeHistoryX.size()/nRep)*i, vecTimeHistoryX.begin()+(vecTimeHistoryX.size()/nRep)*(i+1), vecTimeHistoryX.begin(), vecTimeHistoryX.begin()+(vecTimeHistoryX.size()/nRep)*i, std::plus<double>());

                  std::transform(vecTimeHistoryY.begin()+(vecTimeHistoryY.size()/nRep)*i, vecTimeHistoryY.begin()+(vecTimeHistoryY.size()/nRep)*(i+1), vecTimeHistoryY.begin(), vecTimeHistoryY.begin()+(vecTimeHistoryY.size()/nRep)*i, std::plus<double>());
                }
            }
        }
      vecTimeHistoryX.resize(timeHistoryLen/2);
      vecTimeHistoryY.resize(timeHistoryLen/2);
    }
  timeHistoryFloat.resize(vecTimeHistoryX.size()*2);
  std::copy(vecTimeHistoryX.begin(), vecTimeHistoryX.end(), timeHistoryFloat.begin());
  std::copy(vecTimeHistoryY.begin(), vecTimeHistoryY.end(), timeHistoryFloat.begin()+vecTimeHistoryX.size()/2);
  timeHistoryInt = shins::util::reinterpret_float_to_int(timeHistoryFloat);
  for (int i=0; i<20; i++)
    {
      U3_LOG(timeHistoryFloat[i] << " -> " << timeHistoryInt[i])
        }
  U3_LOG(timeHistoryInt.size());
  // copy vecTimeHistoryX in first half of timeHistory  (using STL algoruthms?)
  // copy vecTimeHistoryY in second half of timeHistory (using STL algoruthms?)

  std::fill(timeHistoryGainFloat.begin(), timeHistoryGainFloat.end(), 3.73/21.*0.72*1.2);
  timeHistoryGainInt = shins::util::reinterpret_float_to_int(timeHistoryGainFloat);
  // TimeHistoryGain = single([3.73/21 3.73/21]*0.72);
  // TimeHistoryGain = 1.2*TimeHistoryGain;
  // vecMeanX.resize(vecTimeHistoryX.size(),
  // std::accumulate(vecTimeHistoryX.begin(), vecTimeHistoryX.end(), 0) / vecTimeHistoryX.size());
  //
  // vecMeanY.resize(vecTimeHistoryX.size(),
  // std::accumulate(vecTimeHistoryY.begin(), vecTimeHistoryY.end(), 0) / vecTimeHistoryY.size());
  //
  // std::transform(vecTimeHistoryX.begin(), vecTimeHistoryX.end(),
  // vecMeanX.begin(), vecTimeHistoryX.begin(), std::minus<double>());
  // std::transform(vecTimeHistoryY.begin(), vecTimeHistoryY.end(),
  // vecMeanY.begin(), vecTimeHistoryY.begin(), std::minus<double>());
  //

  I_LOG("here, before try");
  Nice::SeqInt dspStart(1);
  Nice::SeqInt buffer(5);
  Nice::SeqInt buffer2(DMOutputVector.size());
  try
    {//try
      callbackOnInterrupt();

      D_LOG( config()->leaf("CONNECT"));
      Nice::AppConfigClientPtr con = Nice::AppConfigClient::create(config()->getAtNode<Nice::Endpoint>("CONNECT"), config()->node("DATA"));
      I_LOG("here");
      D_LOG("create");
      if (config()->exist(srvName))
        {// if srvName exists
        m_bcuService = Sharknir::BcuServiceWorkerInterface::create(config()->node(srvName)->path());
        dspProgramPath = etcPath + m_bcuService->BcuDspProgramPath();
        I_LOG(dspProgramPath << " uploading to DSP...");
        m_bcuService->uploadDspCode(Sharknir::Bcu::Const::BCU_DSP,Sharknir::Bcu::Const::BCU_DSP,dspProgramPath);
        I_LOG(dspProgramPath << " uploading to DSP... DONE");

        // sending init parameters to BCU
        D_LOG("Sharknir::Bcu::Const::BCU_DSP = " << static_cast<int>(Sharknir::Bcu::Const::BCU_DSP));
        I_LOG("Wrinting parameters to BCU DSP ... ")
          // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::FramesCounter,         FramesCounter); D_LOG("FramesCounter")
        //                                                                                                                                                        buffer.resize(FramesCounter.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::FramesCounter, FramesCounter.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ResetLoop,             ResetLoop); D_LOG("ResetLoop")
        //                                                                                                                                                  buffer.resize(ResetLoop.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ResetLoop, ResetLoop.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::FramesCounterIrq,      FramesCounterIrq); D_LOG("FramesCounterIrq")
        //                                                                                                                                                         buffer.resize(FramesCounterIrq.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::FramesCounterIrq, FramesCounterIrq.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ParamSelectorIrq,      ParamSelectorIrq); D_LOG("ParamSelectorIrq")
        //                                                                                                                                                         buffer.resize(ParamSelectorIrq.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ParamSelectorIrq, ParamSelectorIrq.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ParamSelector,         ParamSelector); D_LOG("ParamSelector")
        //                                                                                                                                                      buffer.resize(ParamSelector.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ParamSelector, ParamSelector.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::enableRTCommands,      enableRTCommands); D_LOG("enableRTCommands")
        //                                                                                                                                                         buffer.resize(enableRTCommands.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::enableRTCommands, enableRTCommands.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TTControlLoopEna,      TTControlLoopEna); D_LOG("TTControlLoopEna")
        //                                                                                                                                                         buffer.resize(TTControlLoopEna.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TTControlLoopEna, TTControlLoopEna.size());
        // U2_LOG(buffer);


        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CCDInterfaceEnable,    CCDInterfaceEnable); D_LOG("CCDInterfaceEnable")
        //                                                                                                                                                           buffer.resize(CCDInterfaceEnable.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CCDInterfaceEnable, CCDInterfaceEnable.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CCDWaitForReset,       CCDWaitForReset); D_LOG("CCDWaitForReset")
        //                                                                                                                                                        buffer.resize(CCDWaitForReset.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CCDWaitForReset, CCDWaitForReset.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CCDSubFrameCnt,        CCDSubFrameCnt); D_LOG("CCDSubFrameCnt")
        //                                                                                                                                                       buffer.resize(CCDSubFrameCnt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CCDSubFrameCnt, CCDSubFrameCnt.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CCDTotalFlux,          CCDTotalFlux); D_LOG("CCDTotalFlux")
        //               buffer.resize(CCDTotalFlux.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CCDTotalFlux, CCDTotalFlux.size());
        // U2_LOG(buffer);


        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NewModeVectorUpd,      NewModeVectorUpd); D_LOG("NewModeVectorUpd")
        //                                                                                                                                                         buffer.resize(NewModeVectorUpd.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NewModeVectorUpd, NewModeVectorUpd.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ModesVectorLen,        ModesVectorLen); D_LOG("ModesVectorLen")
        //                                                                                                                                                       buffer.resize(ModesVectorLen.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ModesVectorLen, ModesVectorLen.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMOutputEnable,        DMOutputEnable); D_LOG("DMOutputEnable")
        //                                                                                                                                                       buffer.resize(DMOutputEnable.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMOutputEnable, DMOutputEnable.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMNumActuatorsIrq,     DMNumActuatorsIrq); D_LOG("DMNumActuatorsIrq")
        //                                                                                                                                                          buffer.resize(DMNumActuatorsIrq.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMNumActuatorsIrq, DMNumActuatorsIrq.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMOutputVector,        DMOutputVector); D_LOG("DMOutputVector")
        //                                                                                                                                                       buffer.resize(DMOutputVector.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMOutputVector, DMOutputVector.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMNumActuators,        DMNumActuators); D_LOG("DMNumActuators")
        //                                                                                                                                                       buffer.resize(DMNumActuators.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMNumActuators, DMNumActuators.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMInuseNumAct,         DMInuseNumAct); D_LOG("DMInuseNumAct")
        //                                                                                                                                                      buffer.resize(DMInuseNumAct.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMInuseNumAct, DMInuseNumAct.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMUpdateStart,         DMUpdateStart); D_LOG("DMUpdateStart")
        //                                                                                                                                                      buffer.resize(DMUpdateStart.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMUpdateStart, DMUpdateStart.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MaxAbsDMCommand,       MaxAbsDMCommand); D_LOG("MaxAbsDMCommand")
        //                                                                                                                                                        buffer.resize(MaxAbsDMCommand.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MaxAbsDMCommand, MaxAbsDMCommand.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MaxAbsDMCommandSkip,   MaxAbsDMCommandSkip); D_LOG("MaxAbsDMCommandSkip")
        //                                                                                                                                                            buffer.resize(MaxAbsDMCommandSkip.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MaxAbsDMCommandSkip, MaxAbsDMCommandSkip.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMBitOffset,           DMBitOffset); D_LOG("DMBitOffset " << DMBitOffset[0 ])
        //                                                                                                                                                    buffer.resize(DMBitOffset.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMBitOffset, DMBitOffset.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMm2bitConversion,     DMm2bitConversion); D_LOG("DMm2bitConversion")
        //                                                                                                                                                          buffer.resize(DMm2bitConversion.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMm2bitConversion, DMm2bitConversion.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MaxDMPower,            MaxDMPower); D_LOG("MaxDMPower")
        //                                                                                                                                                   buffer.resize(MaxDMPower.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MaxDMPower, MaxDMPower.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMTotalPower,          DMTotalPower); D_LOG("DMTotalPower")
        //                                                                                                                                                     buffer.resize(DMTotalPower.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMTotalPower, DMTotalPower.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMCmdSkippedCnt,       DMCmdSkippedCnt); D_LOG("DMCmdSkippedCnt")
        //                                                                                                                                                        buffer.resize(DMCmdSkippedCnt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMCmdSkippedCnt, DMCmdSkippedCnt.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NewModeVectorPtr,      NewModeVectorPtr); D_LOG("NewModeVectorPtr")
        //                                                                                                                                                         buffer.resize(NewModeVectorPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NewModeVectorPtr, NewModeVectorPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMCommandVectPtr,      DMCommandVectPtr); D_LOG("DMCommandVectPtr")
        //                                                                                                                                                         buffer.resize(DMCommandVectPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMCommandVectPtr, DMCommandVectPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMInfluenceMatrix0Ptr, DMInfluenceMatrix0Ptr); D_LOG("DMInfluenceMatrix0Ptr")
        //                                                                                                                                                              buffer.resize(DMInfluenceMatrix0Ptr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMInfluenceMatrix0Ptr, DMInfluenceMatrix0Ptr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMInfluenceMatrix1Ptr, DMInfluenceMatrix1Ptr); D_LOG("DMInfluenceMatrix1Ptr")
        //                                                                                                                                                              buffer.resize(DMInfluenceMatrix1Ptr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMInfluenceMatrix1Ptr, DMInfluenceMatrix1Ptr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMBiasVectorPtr,       DMBiasVectorPtr); D_LOG("DMBiasVectorPtr")
        //                                                                                                                                                        buffer.resize(DMBiasVectorPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMBiasVectorPtr, DMBiasVectorPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMActuatorSignPtr,     DMActuatorSignPtr); D_LOG("DMActuatorSignPtr")
        //                                                                                                                                                          buffer.resize(DMActuatorSignPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMActuatorSignPtr, DMActuatorSignPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMTmpOutputVectorPtr,  DMTmpOutputVectorPtr); D_LOG("DMTmpOutputVectorPtr")
        //                                                                                                                                                             buffer.resize(DMTmpOutputVectorPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMTmpOutputVectorPtr, DMTmpOutputVectorPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMActuatorMapPtr,      DMActuatorMapPtr); D_LOG("DMActuatorMapPtr")
        //                                                                                                                                                         buffer.resize(DMActuatorMapPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMActuatorMapPtr, DMActuatorMapPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, NewModeVectorPtr[0],            newModeVectorInt); D_LOG("newModeVectorInt")
        //                                                                                                                                                   buffer.resize(newModeVectorInt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, NewModeVectorPtr[0], newModeVectorInt.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ModeVector0Ptr,        ModeVector0Ptr); D_LOG("ModeVector0Ptr")
        //                                                                                                                                                       buffer.resize(ModeVector0Ptr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ModeVector0Ptr, ModeVector0Ptr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ModeVector1Ptr,        ModeVector1Ptr); D_LOG("ModeVector1Ptr")
        //                                                                                                                                                       buffer.resize(ModeVector1Ptr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::ModeVector1Ptr, ModeVector1Ptr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, ModeVector0Ptr[0],              modeVector0Int); D_LOG("modeVector0Int")
        //                                                                                                                                                 buffer.resize(modeVector0Int.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, ModeVector0Ptr[0], modeVector0Int.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, ModeVector1Ptr[0],              modeVector1Int); D_LOG("modeVector1Int")
        //                                                                                                                                                 buffer.resize(modeVector1Int.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, ModeVector1Ptr[0], modeVector1Int.size());
        // U2_LOG(buffer);

        // DMInfluenceMatrix0[0] = 14;

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, DMInfluenceMatrix0Ptr[0],       DMInfluenceMatrix0Int); D_LOG("DMInfluenceMatrix0Int")
        //                                                                                                                                                        buffer.resize(DMInfluenceMatrix0Int.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, DMInfluenceMatrix0Ptr[0], DMInfluenceMatrix0Int.size());
        // // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, DMInfluenceMatrix1Ptr[0],       DMInfluenceMatrix1Int); D_LOG("DMInfluenceMatrix1Int")
        //                                                                                                                                                        buffer.resize(DMInfluenceMatrix1Int.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, DMInfluenceMatrix1Ptr[0], DMInfluenceMatrix1Int.size());
        // // U2_LOG(buffer);

        // D_LOG("Reading back " << DMInfluenceMatrix0Int.size() << " int32_t from DSP memory at location 0x" << std::hex << DMInfluenceMatrix0Ptr[0] << std::dec)
        //   buffer.resize(DMInfluenceMatrix0Int.size(), 99);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, DMInfluenceMatrix0Ptr[0],       DMInfluenceMatrix0Int.size());
        // D_LOG(DMInfluenceMatrix0Int)
        // D_LOG(buffer) // will have to be reinterpreted as float

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, DMActuatorMapPtr[0],            DMActuatorMap); D_LOG("DMActuatorMap")
        //                                                                                                                                                      buffer.resize(DMActuatorMap.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, DMActuatorMapPtr[0], DMActuatorMap.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, DMBiasVectorPtr[0],             DMFlatVectorInt); D_LOG("DMFlatVectorInt")
        //                                                                                                                                                  buffer.resize(DMFlatVectorInt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, DMBiasVectorPtr[0], DMFlatVectorInt.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, DMActuatorSignPtr[0],           DMActuatorSign); D_LOG("DMActuatorSign"); D_LOG("DMActuatorSign.size() = " << DMActuatorSign.size()); //std::cin >> ch;

        // buffer.resize(DMActuatorSign.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, DMActuatorSignPtr[0], DMActuatorSign.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, DMTmpOutputVectorPtr[0],        DMTmpOutputVector); D_LOG("DMTmpOutputVector"); //std::cin >> ch;
        // buffer.resize(DMTmpOutputVector.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, DMTmpOutputVectorPtr[0], DMTmpOutputVector.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PIDDelayLine,          PIDDelayLineInt); D_LOG("PIDDelayLineInt"); //std::cin >> ch;
        // buffer.resize(PIDDelayLineInt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PIDDelayLine, PIDDelayLineInt.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PIDFilterCoeff,        PIDFilterCoeffInt); D_LOG("PIDFilterCoeffInt"); //std::cin >> ch;
        // buffer.resize(PIDFilterCoeffInt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PIDFilterCoeff, PIDFilterCoeffInt.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NumCentroidPixels,     NumCentroidPixels); D_LOG("NumCentroidPixels"); //std::cin >> ch;
        // buffer.resize(NumCentroidPixels.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NumCentroidPixels, NumCentroidPixels.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TimeHistoryAct,        TimeHistoryAct); D_LOG("TimeHistoryAct"); //std::cin >> ch;
        // buffer.resize(TimeHistoryAct.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TimeHistoryAct, TimeHistoryAct.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CentroidGain,          centroidGainInt); D_LOG("centroidGainInt"); //std::cin >> ch;
        // buffer.resize(centroidGainInt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CentroidGain, centroidGainInt.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CentroidOrigin,        centroidOriginInt); D_LOG("centroidOriginInt"); //std::cin >> ch;
        // buffer.resize(centroidOriginInt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CentroidOrigin, centroidOriginInt.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PixelMinValue,         pixelMinValueInt); D_LOG("pixelMinValueInt"); //std::cin >> ch;
        // buffer.resize(pixelMinValueInt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PixelMinValue, pixelMinValueInt.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MinCentroidValue,      MinCentroidValue); D_LOG("MinCentroidValue"); //std::cin >> ch;
        // buffer.resize(MinCentroidValue.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MinCentroidValue, MinCentroidValue.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MaxCentroidValue,      MaxCentroidValue); D_LOG("MaxCentroidValue"); //std::cin >> ch;
        // buffer.resize(MaxCentroidValue.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MaxCentroidValue, MaxCentroidValue.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MinTTCommandValue,     MinTTCommandValue); D_LOG("MinTTCommandValue"); //std::cin >> ch;
        // buffer.resize(MinTTCommandValue.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MinTTCommandValue, MinTTCommandValue.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MaxTTCommandValue,     MaxTTCommandValue); D_LOG("MaxTTCommandValue"); //std::cin >> ch;
        // buffer.resize(MaxTTCommandValue.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::MaxTTCommandValue, MaxTTCommandValue.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::SubapIntensityThres,   SubapIntensityThres); D_LOG("SubapIntensityThres");
        // buffer.resize(SubapIntensityThres.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::SubapIntensityThres, SubapIntensityThres.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CentroidOut,           centroidOutInt); D_LOG("centroidOutInt"); //std::cin >> ch;
        // buffer.resize(centroidOutInt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CentroidOut, centroidOutInt.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NumPixelsToDoPtr,      NumPixelsToDoPtr); D_LOG("NumPixelsToDoPtr"); //std::cin >> ch;
        // buffer.resize(NumPixelsToDoPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NumPixelsToDoPtr, NumPixelsToDoPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CentroidPixelAreaPtr,  CentroidPixelAreaPtr); D_LOG("CentroidPixelAreaPtr");
        // buffer.resize(CentroidPixelAreaPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CentroidPixelAreaPtr, CentroidPixelAreaPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DiagBuffHeaderPtr,     DiagBuffHeaderPtr); D_LOG("DiagBuffHeaderPtr"); //std::cin >> ch;
        // buffer.resize(DiagBuffHeaderPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DiagBuffHeaderPtr, DiagBuffHeaderPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DiagBuffDataPtr,       DiagBuffDataPtr); D_LOG("DiagBuffDataPtr"); //std::cin >> ch;
        // buffer.resize(DiagBuffDataPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DiagBuffDataPtr, DiagBuffDataPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DiagBuffFooterPtr,     DiagBuffFooterPtr); D_LOG("DiagBuffFooterPtr"); //std::cin >> ch;
        // buffer.resize(DiagBuffFooterPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DiagBuffFooterPtr, DiagBuffFooterPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DiagnosticFrameDec,    DiagnosticFrameDec); D_LOG("DiagnosticFrameDec");
        // buffer.resize(DiagnosticFrameDec.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DiagnosticFrameDec, DiagnosticFrameDec.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PixelOffsetPtr,        PixelOffsetPtr); D_LOG("PixelOffsetPtr"); //std::cin >> ch;
        // buffer.resize(PixelOffsetPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PixelOffsetPtr, PixelOffsetPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PixelGainPtr,          PixelGainPtr); D_LOG("PixelGainPtr"); //std::cin >> ch;
        // buffer.resize(PixelGainPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PixelGainPtr, PixelGainPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CentroidFactPtr,       CentroidFactPtr); D_LOG("CentroidFactPtr"); //std::cin >> ch;
        // buffer.resize(CentroidFactPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::CentroidFactPtr, CentroidFactPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PreviousPixelAreaPtr,  PreviousPixelAreaPtr); D_LOG("PreviousPixelAreaPtr");
        // buffer.resize(PreviousPixelAreaPtr.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PreviousPixelAreaPtr, PreviousPixelAreaPtr.size());
        // U2_LOG(buffer);

        // m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, CentroidFactPtr[0],             centroidFactInt); D_LOG("centroidFactInt"); //std::cin >> ch;
        // buffer.resize(centroidFactInt.size(), 0);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, CentroidFactPtr[0], centroidFactInt.size());
        // // U2_LOG(buffer);

        m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, PixelOffsetPtr[0],              mttib_PixelOffset); D_LOG("mttib_PixelOffset"); //std::cin >> ch;
        buffer.resize(mttib_PixelOffset.size(), 0);
        buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, PixelOffsetPtr[0], mttib_PixelOffset.size());
        // U2_LOG(buffer);

        m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, PixelGainPtr[0],                pixelGainInt); D_LOG("pixelGainInt"); //std::cin >> ch;
        buffer.resize(pixelGainInt.size(), 0);
        buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, PixelGainPtr[0], pixelGainInt.size());
        // U2_LOG(buffer);

        m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, NumPixelsToDoPtr[0],            NumPixelsToDo_A); D_LOG("NumPixelsToDo_A"); //std::cin >> ch;
        buffer.resize(NumPixelsToDo_A.size(), 0);
        buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, NumPixelsToDoPtr[0], NumPixelsToDo_A.size());
        U2_LOG(buffer);

        m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, NumPixelsToDoPtr[0]+0x8000,     NumPixelsToDo_B); D_LOG("NumPixelsToDo_B"); //std::cin >> ch;
        buffer.resize(NumPixelsToDo_B.size(), 0);
        buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, NumPixelsToDoPtr[0]+0x8000, NumPixelsToDo_B.size());
        U2_LOG(buffer);


        m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PixelFrameDec,         PixelFrameDec); D_LOG("PixelFrameDec"); //std::cin >> ch;
        buffer.resize(PixelFrameDec.size(), 0);
        buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::PixelFrameDec, PixelFrameDec.size());
        U2_LOG(buffer);

        m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NiosIrqDelay,          NiosIrqDelay); D_LOG("NiosIrqDelay"); //std::cin >> ch;
        buffer.resize(NiosIrqDelay.size(), 0);
        buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NiosIrqDelay, NiosIrqDelay.size());
        U2_LOG(buffer);

        m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TimeHistoryPtr,        TimeHistoryPtr); D_LOG("TimeHistoryPtr"); //std::cin >> ch;
        buffer.resize(TimeHistoryPtr.size(), 0);
        buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TimeHistoryPtr, TimeHistoryPtr.size());
        U2_LOG(buffer);

        m_bcuService->writeDsp  (Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TimeHistoryEndPtr,     TimeHistoryEndPtr); D_LOG("TimeHistoryEndPtr"); //std::cin >> ch;
        buffer.resize(TimeHistoryEndPtr.size(), 0);
        buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TimeHistoryEndPtr, TimeHistoryEndPtr.size());
        U2_LOG(buffer);

        m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TimeHistoryGain,        timeHistoryGainInt); D_LOG("timeHistoryGainInt"); //std::cin >> ch;
        buffer.resize(timeHistoryGainInt.size(), 0);
        buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TimeHistoryGain, timeHistoryGainInt.size());
        U2_LOG(buffer);

        m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, TimeHistoryPtr[0],               timeHistoryInt); D_LOG("timeHistoryInt"); //std::cin >> ch;
        buffer.resize(timeHistoryInt.size(), 0);


        I_LOG("Wrinting parameters to BCU DSP ... DONE");
        buffer.resize(1, 0);
        buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::TimeHistoryAct, 1);
        U2_LOG("Read TimeHistoryAct value");
        U2_LOG(buffer);

        // Starting BCU
        dspStart[0]=0;
        //m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP,Sharknir::Bcu::Const::BCU_DSP,0x180730,buffer); // start DSP
        m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP,Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DSPstart, dspStart); D_LOG("dspStart"); // start DSP

        //         int nCycles = 0;
        // buffer.resize(timeHistoryInt.size(),0);
        // while ((m_bcuService->isReachable()) && (nCycles < 5000))
        //   {
        //     try
        //       {
        //         buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, 0x1188+24, timeHistoryInt.size()-nCycles);// std::round(timeHistoryInt.size()/(1+0.01*(nCycles))));
        //         // U2_LOG(std::hex << TimeHistoryPtr[0] << std::dec << " " << std::hex <<  TimeHistoryEndPtr[0])
        //         U2_LOG(std::dec << buffer.size());
        //         // U2_LOG("Waited " << nCycles  << " cycles");
        //         // U2_LOG("Size of int " << sizeof(buffer[0]));
        //         // for (int i=0; i<20; i++)
        //         // {
        //         //     U3_LOG(timeHistoryInt[i])
        //         // }
        //         // break;
        //       }
        //     catch (const Sharknir::BcuServiceException &bcuEx)
        //       {
        //         U2_LOG("caught BCU service error")
        //           // U2_LOG(bcuEx)
        //           // U2_LOG(TimeHistoryPtr[0])
        //           // U2_LOG(TimeHistoryEndPtr[0])
        //           std::this_thread::sleep_for(std::chrono::milliseconds(100));
        //         // nCycles++;
        //       }
        //     nCycles++;
        //   }
        // U2_LOG("timeHistoryInt.size() = " << timeHistoryInt.size());
        // U2_LOG(std::hex << 0x1188+24+timeHistoryInt.size());

        //   // Nice::SeqInt readDsp(int DSP_INDEX, int ADDRESS, int LENGTH_IN_DWORD)
        //   // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, 0x180730,2);
        //   I_LOG("Reading back " << 1 << " int32_t from DSP memory at location 0x" << std::hex << Sharknir::Bcu::DSPstart << std::dec)
        dspStart = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DSPstart, 1);

        I_LOG("Start DSP flag read from DSP");
        I_LOG(dspStart);

        Nice::SeqInt NewModeVectorUpd(1), DMUpdateStart(1);
        NewModeVectorUpd[0] = 1;
        DMUpdateStart[0] = 1;
        m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::NewModeVectorUpd, NewModeVectorUpd);
        m_bcuService->writeDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::DMUpdateStart, DMUpdateStart);

        Nice::SeqInt uartRxControloReg(1);
        uartRxControloReg = m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP,
                                                   Sharknir::Bcu::UartRxControlReg, 1);
        I_LOG(uartRxControloReg[0]);
        send_command_to_camera("status");

        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, 0x1022, 1);
        // buffer = m_bcuService->readDsp(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::_bttub_FramesCounter, 1);
        // I_LOG("Frames Counter " << buffer[0]);
      }
    }
  catch ( const ::Nice::Exception & ex )
    {
      E_LOG("exception 1")
        E_LOG ( ex );
      status = EXIT_FAILURE;
    }

  catch ( const IceUtil::Exception& ex )
    {
      E_LOG("exception 2")
        E_LOG ( ex );
      status = EXIT_FAILURE;
    }

  catch ( const std::exception& ex )
    {
      E_LOG("exception 3")
        E_LOG ( ex.what() );
      status = EXIT_FAILURE;
    }

  catch ( const char * ex )
    {
      E_LOG("exception 4")
        E_LOG ( ex );
      status = EXIT_FAILURE;
    }

  catch ( ... )
    {
      E_LOG("exception 5")
        E_LOG ( "Unknow Error" );
      status = EXIT_FAILURE;
    }

  return usage ( status );
}


void test_tiptilt_ctrl::interrupCallback(int sig)
{
  N_LOG ( "Interrupt #" << sig );
}

int main(int argc, char* argv[])
{
  test_tiptilt_ctrl niceApp;
  I_LOG("MRDA");
  return niceApp.start(argc, argv);
}


/**
 * @brief      Loads fits. Load data from primary header of FITS file and copies them in valarray type variable, colmajor
 *
 * @param[in]  _filename  The filename
 * @param      _loadedActNum       The ax 1
 * @param      _loadedNModes       The ax 2
 *
 * @return     { standard C++ valarray, contains FITS data colmajor fashion. }
 */
std::valarray<double>
test_tiptilt_ctrl::load_fits(std::string _filename, int& _loadedActNum, int& _loadedNModes)
{
  T_LOG("loading FITS file ...");
  std::valarray<double> vecM2C;
  std::auto_ptr<CCfits::FITS> pInFile(new CCfits::FITS(_filename, CCfits::Read, true));
  CCfits::PHDU& phduM2C = pInFile->pHDU();

  phduM2C.read(vecM2C); // this is a vector of size 2910 (30x97)
  _loadedActNum = phduM2C.axis(0); // actuators number in DM. Number of columns
  _loadedNModes = phduM2C.axis(1); // number of modes. Number of rows
  T_LOG("loading FITS file ... DONE");
  return vecM2C;
}

Nice::SeqFloat
test_tiptilt_ctrl::set_PID_filter_coeff(const int& _frameRate, const int& _CCDMode){
  Nice::SeqFloat PIDFilterCoeff(10);
  if (_frameRate == 200)
    {
      PIDFilterCoeff[0] = 0.5;
      PIDFilterCoeff[1] = 0.5;
      PIDFilterCoeff[2] = 0.;
      PIDFilterCoeff[3] = 0.;
      PIDFilterCoeff[4] = 0.;
      PIDFilterCoeff[5] = 0.;
      PIDFilterCoeff[6] = 1.;
      PIDFilterCoeff[7] = 1.;
      PIDFilterCoeff[8] = 0.;
      PIDFilterCoeff[9] = 0.;
    }
  if (_frameRate == 250)
    {
      PIDFilterCoeff[0] = 0.25;
      PIDFilterCoeff[1] = 0.25;
      PIDFilterCoeff[2] = 0.;
      PIDFilterCoeff[3] = 0.;
      PIDFilterCoeff[4] = 0.;
      PIDFilterCoeff[5] = 0.;
      PIDFilterCoeff[6] = 1.;
      PIDFilterCoeff[7] = 1.;
      PIDFilterCoeff[8] = -0.15;
      PIDFilterCoeff[9] = -0.15;
    }
  if (_frameRate == 500)
    {
      PIDFilterCoeff[0] = 1;
      PIDFilterCoeff[1] = 1;
      PIDFilterCoeff[2] = 0;
      PIDFilterCoeff[3] = 0;
      PIDFilterCoeff[4] = 0;
      PIDFilterCoeff[5] = 0;
      PIDFilterCoeff[6] = 1;
      PIDFilterCoeff[7] = 1;
      PIDFilterCoeff[8] = 0;
      PIDFilterCoeff[9] = 0;
    }
  if (_frameRate == 800)
    {
      PIDFilterCoeff[0] = 1.5;
      PIDFilterCoeff[1] = 1.5;
      PIDFilterCoeff[2] = 0.;
      PIDFilterCoeff[3] = 0.;
      PIDFilterCoeff[4] = 0.;
      PIDFilterCoeff[5] = 0.;
      PIDFilterCoeff[6] = 1.;
      PIDFilterCoeff[7] = 1.;
      PIDFilterCoeff[8] = -0.25;
      PIDFilterCoeff[9] = -0.25;
    }
  if ((_frameRate == 1000) && (_CCDMode == 7))
    {
      PIDFilterCoeff[0] = 1.4;
      PIDFilterCoeff[1] = 1.4;
      PIDFilterCoeff[2] = 0.;
      PIDFilterCoeff[3] = 0.;
      PIDFilterCoeff[4] = 0.;
      PIDFilterCoeff[5] = 0.;
      PIDFilterCoeff[6] = 1.;
      PIDFilterCoeff[7] = 1.;
      PIDFilterCoeff[8] = 0.;
      PIDFilterCoeff[9] = 0.;
    }
  if ((_frameRate == 1000) && (_CCDMode == 9))
    {
      PIDFilterCoeff[0] = 1.5;
      PIDFilterCoeff[1] = 1.5;
      PIDFilterCoeff[2] = 0.;
      PIDFilterCoeff[3] = 0.;
      PIDFilterCoeff[4] = 0.;
      PIDFilterCoeff[5] = 0.;
      PIDFilterCoeff[6] = 1.;
      PIDFilterCoeff[7] = 1.;
      PIDFilterCoeff[8] = 0.;
      PIDFilterCoeff[9] = 0.;
    }
  if ((_frameRate == 2000) && (_CCDMode == 7))
    {
      PIDFilterCoeff[0] = 1.6;
      PIDFilterCoeff[1] = 1.6;
      PIDFilterCoeff[2] = 0.;
      PIDFilterCoeff[3] = 0.;
      PIDFilterCoeff[4] = 0.;
      PIDFilterCoeff[5] = 0.;
      PIDFilterCoeff[6] = 1.;
      PIDFilterCoeff[7] = 1.;
      PIDFilterCoeff[8] = -0.8;
      PIDFilterCoeff[9] = -0.8;
    }
  if ((_frameRate == 2000) && (_CCDMode == 9))
    {
      PIDFilterCoeff[0] = 2.;
      PIDFilterCoeff[1] = 2.;
      PIDFilterCoeff[2] = 0.;
      PIDFilterCoeff[3] = 0.;
      PIDFilterCoeff[4] = 0.;
      PIDFilterCoeff[5] = 0.;
      PIDFilterCoeff[6] = 1.;
      PIDFilterCoeff[7] = 1.;
      PIDFilterCoeff[8] = -1.;
      PIDFilterCoeff[9] = -1.;
    }
  return PIDFilterCoeff;
}

template <typename Derived> Nice::SeqInt
test_tiptilt_ctrl::convert_to_seqint(const Eigen::MatrixBase<Derived>& _emat)
{
  Nice::SeqInt outSeq;
  for (int i=0; i<_emat.cols(); i++)
    {
      for (int j=0; j<_emat.rows(); j++)
        outSeq.push_back(_emat(j,i));
    }
  return outSeq;
}

template <typename Derived> Nice::SeqFloat
test_tiptilt_ctrl::convert_to_seqfloat(const Eigen::MatrixBase<Derived>& _emat)
{
  Nice::SeqFloat outSeq;
  for (int i=0; i<_emat.cols(); i++)
    {
      for (int j=0; j<_emat.rows(); j++)
        outSeq.push_back(_emat(j,i));
    }
  return outSeq;
}


/**
 * @brief      Sends a command to camera.
 *
 * @param[in]  _command  The command
 *
 * @return     { EXIT_SUCCESS or EXIT_FAILURE }
 */
int
test_tiptilt_ctrl::send_command_to_camera(const std::string& _command)
{
  //
  int exit = EXIT_SUCCESS;
  int  commandLength;
  std::vector<int> txBuffer;
  std::vector<int> rxBuffer;
  std::string cameraCommand = _command;
  std::vector<char> cameraCommandAsChar;
  Nice::SeqInt uartRxControloReg;
  Nice::SeqInt tmpUartRxControloReg1;
  Nice::SeqInt tmpUartRxControloReg2;

  cameraCommand += "\r\n";
  commandLength = cameraCommand.length();
  if (commandLength % 4)
    {
      cameraCommand.append(4-(commandLength % 4), '\0');
    }
  std::copy(
            cameraCommand.begin(),
            cameraCommand.end(),
            std::back_inserter(cameraCommandAsChar)
            );
  I_LOG("casting char to int .... ")
    txBuffer = shins::util::reinterpret_T_to_R<char,int>(cameraCommandAsChar);
  I_LOG("reading SRAM for UART control register ... ")
    uartRxControloReg = this->m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP,
                                                     Sharknir::Bcu::UartRxControlReg, 1);
  //  write the command to SRAM
  I_LOG("write the command to SRAM");
  m_bcuService->writeSram(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP,
                          Sharknir::Bcu::UartTxBufferArea , txBuffer); D_LOG("txBuffer");
  txBuffer.clear();
  txBuffer.resize(1, 0x80000000+commandLength);
  // std::for_each(txBuffer.begin(), txBuffer.end(),
  //               [](const int& _i){
  //                 std::cout << _i << std::endl;
  //               });
  // send the command to camera
  I_LOG("write the command length to SRAM");
  m_bcuService->writeSram(Sharknir::Bcu::Const::BCU_DSP, Sharknir::Bcu::Const::BCU_DSP,
                          Sharknir::Bcu::UartTxControlReg, txBuffer);

  // check for the reply which corresponds to a change in UARTRXControlReg
  tmpUartRxControloReg1 = uartRxControloReg;
  tmpUartRxControloReg2 = uartRxControloReg;
  I_LOG("read answer from camera ... ");
  Nice::Timer t;
  t.start(10.);
  while (((tmpUartRxControloReg2 == uartRxControloReg) || (tmpUartRxControloReg1 != tmpUartRxControloReg2))
         && !(t.elapsed()))
    {
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      tmpUartRxControloReg1 = tmpUartRxControloReg2;
      tmpUartRxControloReg2 = m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP,
                                                     Sharknir::Bcu::UartRxControlReg, 1);
    }
  if (t.elapsed())
    {
      W_LOG("Timer has elapsed!");
    }
  rxBuffer = m_bcuService->readSram(Sharknir::Bcu::Const::BCU_DSP,
                                    Sharknir::Bcu::UartRxBufferArea,
                                    Sharknir::Bcu::UartRxBufferArea-Sharknir::Bcu::UartTxBufferArea);

  if (uartRxControloReg[0] > 8)
    uartRxControloReg[0] -= 8;

  if (tmpUartRxControloReg2[0] > 8)
    tmpUartRxControloReg2[0] -= 8;

  // this part has to be understood 
  // if end_ptr1>start_ptr
  //               fprintf('%s',aa(start_ptr+1:end_ptr1));
  // else
  //   fprintf('%s',[aa(start_ptr+1:end) aa(1:end_ptr1)]);
  // end
  cameraCommandAsChar.clear();
  cameraCommandAsChar = shins::util::reinterpret_T_to_R<int, char>(rxBuffer);
  // I_LOG("Answer is: ")
  // std::for_each(
  //               cameraCommandAsChar.begin(),
  //               cameraCommandAsChar.end(),
  //               [](const char& _c){I_LOG(_c)}
  //               );
  // I_LOG("-----------------------------------------")

}

