#include "converterv1Tov2.h"
#include <iostream>
#include <fstream>
#include <string>
#include <iomanip>
#include <climits>
#include <vector>
#include <map>
#include <deque>
#include <cmath>

#include "JDAQ/JDAQSuperFrame.hh"
#include "JDAQ/JDAQTimesliceHeader.hh"
#include "JDAQ/JDAQTimeslice.hh"
#include "JDAQ/JDAQHit.hh"
#include "JDAQ/JDAQPreamble.hh"

#include "Jeep/JParser.hh"
#include "Jeep/JMessage.hh"
#include "JIO/JIOLibrary.hh"


using namespace std;
using namespace KM3NETDAQ;
using namespace JIO;

float Slice_Time;
const int nChannel = 31;

long long int totalSize1 = 0;
long long int totalSize2 = 0;
long long int timeShift[3] = {0, 0, 0};

std::vector<JDAQHit> vOldHits[3];

inline int
GetPmtId(int oldId) {
  static const int oldIds[nChannel] = {28, 21, 22, 23, 29, 27, 24, 26, 30, 25, 19, 20, 18, 12, 6, 17, 13, 7, 1, 5, 8, 11, 0, 16, 3, 4, 2, 14, 9, 10, 15};
  return oldIds[oldId];
}

inline void
FillHitVector(const Hitv2& hit, vector<JDAQHit>& allHits, const int omTimeShift, const int omId) {
  const uint32_t curTimeStamp = 8*hit.timeStamp + uint32_t(hit.firstBit) + omTimeShift;
  int totShift = 0;
  while ((hit.widthPat - totShift) > 255) {
    if (curTimeStamp > unsigned(134217727 - totShift)) {
      vOldHits[omId].push_back(JDAQHit(uint8_t(GetPmtId(int(hit.id))), uint32_t(curTimeStamp + totShift - 134217728), uint8_t(255)));
    } else {
      allHits.push_back(JDAQHit(uint8_t(GetPmtId(int(hit.id))), uint32_t(curTimeStamp + totShift), uint8_t(255)));
    }
    totShift += 256;
  }
  if (curTimeStamp > unsigned(134217727 - totShift)) {
    vOldHits[omId].push_back(JDAQHit(uint8_t(GetPmtId(int(hit.id))), uint32_t(curTimeStamp + totShift - 134217728), uint8_t(hit.widthPat - totShift)));
  } else {
    allHits.push_back(JDAQHit(uint8_t(GetPmtId(int(hit.id))), uint32_t(curTimeStamp + totShift), uint8_t(hit.widthPat - totShift)));
  }
}

inline void
CoutFrameInfo(Frame& frame) {
  const double shiftTime = (timeShift[int(frame.omId()) - 101] - timeShift[0])/0.134217728;
  const int shiftSliceNb = int(shiftTime);
  const int omTimeShift = int((shiftTime - double(shiftSliceNb))*134217728);
  const uint64_t timeTot = uint64_t(frame.frameIndex() - 1)*uint64_t(134217728) + uint64_t(omTimeShift); 
  cout << frame.frameSize() << ' ' << frame.frameType() << ' ' << frame.frameTarget() 
       << ' ' << uint64_t(timeTot) << ' ' << frame.frameTime() 
       << ' ' << frame.frameIndex() << ' ' << frame.frameError() << ' ' << frame.nbItems() 
       << " 0 " << frame.omId() << ' ' << frame.streamId() << ' ' << frame.runNumber() << endl;
}

inline void
CoutHitv2(const Hitv2& hit, const uint64_t time) {
  cout << time << ' ' << int(hit.id) << ' ' << 8*hit.timeStamp + hit.firstBit << ' ' << int(hit.widthPat) << endl;
}

inline void
CoutHitv2(const JDAQHit& hit, const uint64_t time) {
  cout << time << ' ' << int(hit.getPMT()) << ' ' << hit.getT() << ' ' << int(hit.getToT()) << endl;
}

uint16_t Hit::channel()
{
  if (data == 0)
    throw "Hit un-initialized";
  uint8_t header = data[0];
  uint16_t chan = ((header >> 0) & 0x3); // 2 lower bits  of channel 
  chan |= ((header >> 3) & (0x7 << 2));  // 3 higher bits of channel
  return chan;
}

uint16_t Hit::type()
{
  if (data == 0)
    throw "Hit un-initialized";
  uint8_t header = reinterpret_cast<uint16_t*>(data)[0];
  uint16_t type = ((header >> 2) & 0x7); // bits 2-4
  return type;
}

uint32_t Hit::timeStamp()
{
  if (data == 0)
    throw "Hit un-initialized";
  uint32_t ts = reinterpret_cast<uint32_t*>(data)[0];
  swap(ts);
  return ts & 0x00FFFFFF;
}

uint16_t Hit::pattern()
{
  if (data == 0)
    throw "Hit un-initialized";
  uint16_t patt = reinterpret_cast<uint16_t*>(data + 4)[0];
  swap(patt);
  return patt;
}

bool Frame::read(std::istream& input)
{
  if (rawPreamble == 0)
    rawPreamble = new uint8_t[PREAMBLE_SIZE_BYTES];
  input.read(reinterpret_cast<char*>(rawPreamble), PREAMBLE_SIZE_BYTES);
  if (input.fail())
    return false;
  if (rawData != 0)
    delete [] rawData;
  rawData = new uint8_t[dataSize_bytes()];
  input.read(reinterpret_cast<char*>(rawData), dataSize_bytes());
  if (input.fail())
    return false;  
  return true;
}

uint32_t Frame::frameIndex()
{
  const uint32_t curFrameIndex = getNumber<uint32_t>(16u);
  const uint32_t correctedIndex = curFrameIndex + uint32_t((timeShift[int(omId() - 101)] - timeShift[0])/0.134217728);
  return correctedIndex;
}

uint64_t Frame::frameTime()
{
  uint64_t time = getNumber<uint64_t>( 8u);
  return 8*time + uint64_t(timeShift[int(omId() - 101)]*1000000000);
}

uint32_t Frame::nbItems()
{
  uint32_t Nb = reinterpret_cast<uint32_t*>(rawPreamble + 20u)[0];
  swap(Nb);
  return Nb & 0x00FFFFFF;
}


Hit Frame::hit(size_t iHit)
{
  if (rawData == 0)
    throw "Frame not initialized";
  if (iHit >= nbItems())
    throw "Hit is out of bounds";  
  return Hit(rawData + iHit * Hit::SIZE_BYTES);
}

int getFirstBit(size_t pattern, int firstBit)
{
  for (size_t i=firstBit; i<(int(sizeof(pattern))*CHAR_BIT); ++i) {
    if ((pattern & (1 << i)) != 0) {
      return i;
    }
  }
  return -10;
}

int getLastBit(size_t pattern, int firstBit)
{
  for (int i=firstBit; i<(int(sizeof(pattern))*CHAR_BIT); ++i) {
    if ((pattern & (1 << i)) == 0) {
      return i;
    }
  }
  return -10;
}

int Analyse(Frame& thisFrame, vector<JDAQHit>& allHits)
{
  Hitv2 previousHit[nChannel];
  Hitv2 currentHit;
  currentHit.firstBit = 0;
  currentHit.timeStamp = 0;
  currentHit.widthPat = 0;
  currentHit.id = 32;
  int curFirstBit = 0;
  int curLastBit = 0;
  int lastTimeStamp[nChannel];
  int nbItems = thisFrame.nbItems();
  for (int i = 0; i < nChannel; ++i) {
    lastTimeStamp[i] = 0;
    previousHit[i].firstBit = 0;
    previousHit[i].timeStamp = 0;
    previousHit[i].widthPat = 0;
    previousHit[i].id = 32;
  }
  const double shiftTime = (timeShift[int(thisFrame.omId()) - 101] - timeShift[0])/0.134217728;
  const int shiftSliceNb = int(shiftTime);
  const int omTimeShift = int((shiftTime - double(shiftSliceNb))*134217728);
  //const uint64_t timeTot = uint64_t(thisFrame.frameIndex() - 1)*uint64_t(134217728) + uint64_t(omTimeShift); 
  for (int it = 0; it < nbItems; ++it) {
    if (thisFrame.hit(it).data == 0) {
      return -1;
    }
    currentHit.id = thisFrame.hit(it).channel();  
    currentHit.timeStamp = thisFrame.hit(it).timeStamp();
    uint16_t curPattern = thisFrame.hit(it).pattern(); 
    if((curFirstBit = getFirstBit(curPattern, 0)) == -10)
      return -2; 
    if((curLastBit = getLastBit(curPattern, curFirstBit)) == -10)
      return -3;
    currentHit.firstBit = curFirstBit;
    currentHit.widthPat = curLastBit - curFirstBit;
    if (abs((int)(lastTimeStamp[currentHit.id] - currentHit.timeStamp)) == 2) { 
      previousHit[currentHit.id].widthPat += currentHit.widthPat;
    } else {
      if (previousHit[currentHit.id].id != 32) {
        FillHitVector(previousHit[currentHit.id], allHits, omTimeShift, int(thisFrame.omId() - 101));
        //if ((thisFrame.omId() == 103) && (thisFrame.frameIndex() > 1783)) {
        //  CoutHitv2(previousHit[currentHit.id], timeTot);
        //} 
      }
      previousHit[currentHit.id].id = currentHit.id;
      previousHit[currentHit.id].timeStamp = currentHit.timeStamp;
      previousHit[currentHit.id].firstBit = currentHit.firstBit;
      previousHit[currentHit.id].widthPat = currentHit.widthPat;
    }
    totalSize1 += 6;
    lastTimeStamp[currentHit.id] = currentHit.timeStamp;	
  }
  for (int i = 0; i < nChannel; ++i) {
    if (previousHit[i].id != 32) {
      FillHitVector(previousHit[i], allHits, omTimeShift, int(thisFrame.omId() - 101));
      //if ((thisFrame.omId() == 103) && (thisFrame.frameIndex() > 1783)) {
      //  CoutHitv2(previousHit[i], timeTot);
      //}
    }
  }
  return 1;
}


struct JBuffer :
  public JDAQChronometer,
  public std::map<int, std::vector<Frame> >
{
  JBuffer() :
    JDAQChronometer(),
    std::map<int, std::vector<Frame> >()
  {
    setFrameIndex(-1);
  }
};


inline bool operator<(const JBuffer& first, const JBuffer& second)
{
  return first.getFrameIndex() < second.getFrameIndex();
}


inline bool operator<(const JBuffer& first, const int frame_index)
{
  return first.getFrameIndex() < frame_index;
}


struct JQueue :
  public deque<JBuffer>
{
  JQueue() :
    deque<JBuffer>()
  {}

  iterator find(const int frame_index)
  {
    return lower_bound(this->begin(), this->end(), frame_index);
  }
};


int main(int argc, char* argv[])
{
  int debug;
  string inputFile;
  string outputFile;
  long long int starttime1;
  long long int starttime2;
  long long int starttime3;

  try {

    JParser<> zap;

    zap['d'] = make_field(debug) = 0;
    zap['f'] = make_field(inputFile);
    zap['o'] = make_field(outputFile);
    zap['t'] = make_field(starttime1) = 0;
    zap['u'] = make_field(starttime2) = 0;
    zap['v'] = make_field(starttime3) = 0;

    if (zap.read(argc, argv) != 0)
      return 1;
  }
  catch(const exception &error) {
    FATAL(error.what() << endl);
  }

  ifstream rdatFile;
  rdatFile.open(inputFile.c_str());

  ofstream out;
  out.open(outputFile.c_str());
  JStreamWriter os(out);
  JBufferedWriter bos(os, 65536);

  JQueue queue;

  size_t MAX = 100;

  Frame frame;
  vOldHits[0].clear();
  vOldHits[1].clear();
  vOldHits[2].clear();

  long long int lastFrameId = 0;
  int lastFrameRunNumber = 0;
  int lastFrameOmId = -1;

  for (long long int number_of_frames = 0; true; ++number_of_frames) {
    DEBUG("frame: " << number_of_frames << '\r');

    timeShift[0] = starttime1;
    timeShift[1] = starttime2;
    timeShift[2] = starttime3;

    const bool ok = frame.read(rdatFile);

    //CoutFrameInfo(frame);
      
    while ((!ok && !queue.empty()) || queue.size() > MAX) {

      JBuffer& buffer = queue.front();

      JDAQTimeslice timeslice;

      timeslice.setDAQChronometer(buffer);
      bool testOM[3] = {false, false, false};
      for (JBuffer::iterator module = buffer.begin(); module != buffer.end(); ++module) {
        if (int(module->first) == 101) testOM[0]= true;
        if (int(module->first) == 102) testOM[1]= true;
        if (int(module->first) == 103) testOM[2]= true;
      }
      for (int i = 0; i < 3; ++i) {
        if ((testOM[i] == false) && (vOldHits[i].size() != 0)) {
          JDAQSuperFrame super_frame(JDAQSuperFrameHeader(buffer, i + 101));
          sort(vOldHits[i].begin(), vOldHits[i].end());
          /*
          for (unsigned int j = 0; j < vOldHits[i].size(); ++j) {
            if (i + 101 == 102) {
              const uint64_t timeTot = uint64_t(buffer.begin()->second[0].frameIndex() - 1)* uint64_t(134217728);
              CoutHitv2(vOldHits[i][j], timeTot);
            }
          }
          */
          super_frame.add(vOldHits[i].size(), vOldHits[i].data());
          timeslice.push_back(super_frame);
          vOldHits[i].clear();
        }
      }

      for (JBuffer::iterator module = buffer.begin(); module != buffer.end(); ++module) {
        const int omId = int(module->first - 101); 

        vector<JDAQHit> zbuf = vOldHits[omId];

        vOldHits[omId].clear();

        lastFrameId = module->second[0].frameIndex();
        lastFrameRunNumber =  module->second[0].runNumber();
        lastFrameOmId = omId;

	for (vector<Frame>::iterator i = module->second.begin(); i != module->second.end(); ++i) {

	  switch(Analyse(*i, zbuf)) {
	    
	  case -1:
	    DEBUG("error: empty frame" << endl);
	    break;
	    
	  case -2:
	    DEBUG("error: pb with firstBit" << endl);
	    break;
	    
	  case -3:
	    DEBUG("error: pb with lastBit" << endl);
	      break;
	      
	  default:
	    break;
	  }
	}
		
	JDAQSuperFrame super_frame(JDAQSuperFrameHeader(buffer, module->first));

	sort(zbuf.begin(), zbuf.end());
        /* 	
        for (unsigned int j = 0; j < zbuf.size(); ++j) {
          if (module->first == 102) {
            const uint64_t timeTot = uint64_t(lastFrameId - 1)* uint64_t(134217728);
            CoutHitv2(zbuf[j], timeTot);
          }
        }
        */
	super_frame.add(zbuf.size(), zbuf.data());
	
	timeslice.push_back(super_frame);
        zbuf.clear();
      }
      
      totalSize2 += timeslice.getSize();

      bos << timeslice;

      queue.pop_front();
    }

    if (!ok)
      break;

    JQueue::iterator p = queue.find((int) frame.frameIndex());

    if (p == queue.end() || p->getFrameIndex() != (int) frame.frameIndex()) {

      JBuffer buffer;
      JDAQUTCExtended utc;
      
      utc.setTimeNanoSecond(uint64_t(frame.frameIndex() - 1)* uint64_t(134217728));
      buffer.setDAQChronometer(JDAQChronometer(frame.runNumber(), 
					       frame.frameIndex(), 
					       utc));

      p = queue.insert(p, buffer);
    }
      
    (*p)[frame.omId()].push_back(frame);

    totalSize1 += 32;
    
    if (frame.streamId() == 6)
      totalSize1 += frame.frameSize();
  }
  JBuffer buffer;
  JDAQUTCExtended utc;
  utc.setTimeNanoSecond(lastFrameId * 134217728);
  buffer.setDAQChronometer(JDAQChronometer(lastFrameRunNumber, lastFrameId + 1, utc));
  JDAQTimeslice timeslice;
  timeslice.setDAQChronometer(buffer);
  JDAQSuperFrame super_frame(JDAQSuperFrameHeader(buffer, lastFrameOmId + 101));
  sort(vOldHits[lastFrameOmId].begin(), vOldHits[lastFrameOmId].end());
  //cout << lastFrameOmId << ' ' << lastFrameId << ' ' << vOldHits[lastFrameOmId].size() << endl;
  /*
  for (unsigned int j = 0; j < vOldHits[lastFrameOmId].size(); ++j) {
    if (lastFrameOmId == 2) {
      const uint64_t timeTot = uint64_t(lastFrameId - 1)* uint64_t(134217728);
      CoutHitv2(vOldHits[lastFrameOmId][j], timeTot);
    }
  }
  */
  super_frame.add(vOldHits[lastFrameOmId].size(), vOldHits[lastFrameOmId].data());
  timeslice.push_back(super_frame);
  bos << timeslice;
  NOTICE("\ntotal size - CLBv1: " << totalSize1 << " - CLBv2: " << totalSize2 << endl);

  bos.flush();
  out.close();
}