/********************************************************************\

  Class interfacing to FAD board

\********************************************************************/

#include "FADBoard.h"
using namespace std;

//
// Constructor
// 
FADBoard::FADBoard(string Server, unsigned short Port, class FAD *Parent, unsigned int Num) {

  int Ret;

  // Initialization
  m = Parent;
  InitOK = false;
  Active = true;
  CommError = false;
  ACalibTime = -1;
  Status.Update.tv_sec = -1;
  Thread = pthread_self(); // For checking in destructor

  Name = new char [Server.size()+1]; // Name in permanent memory for DIM service
  strcpy(Name, Server.c_str());

  // Resolve hostname
  struct hostent *Host = gethostbyname(Server.c_str());
  if (Host == 0) {
    m->PrintMessage("Could not resolve host name for %s\n", Server.c_str());
    return;
  }

  // Open socket descriptor
  if ((Socket = socket(PF_INET, SOCK_STREAM, 0)) == -1) {
    m->PrintMessage("Could not open socket for %s (%s)\n", Server.c_str(), strerror(errno));
    return;
  }
    
  // Connect to server
  struct sockaddr_in SocketAddress;
  SocketAddress.sin_family = PF_INET;
  SocketAddress.sin_port = htons(Port);
  SocketAddress.sin_addr = *(struct in_addr*) Host->h_addr;
  
  if (connect(Socket, (struct sockaddr *) &SocketAddress, sizeof(SocketAddress)) == -1) {
    m->PrintMessage("Could not connect to %s at port %d (%s)\n", Server.c_str(), Port, strerror(errno));
    return;
  }
  
  // Construct DIM service name prefix
  stringstream ID;
  ID << SERVER_NAME"/Board" << setfill('0') << setw(2) << Num << "/";

  DIM_Name = new DimService((ID.str()+"Server").c_str(), Name);
  DIM_ID = new DimService((ID.str()+"BoardID").c_str(), (char *) "S", NULL, 0);
  DIM_Temp = new DimService((ID.str()+"Temperature").c_str(), (char *) "F", NULL, 0);
  DIM_DAC = new DimService((ID.str()+"DAC").c_str(), (char *) "S", NULL, 0);
  DIM_ROI = new DimService((ID.str()+"ROI").c_str(), (char *) "S", NULL, 0);

  // Initialise mutex for synchronization
  pthread_mutexattr_t Attr;

  if ((Ret = pthread_mutexattr_settype(&Attr, PTHREAD_MUTEX_ERRORCHECK)) != 0) {
    m->Message(m->ERROR, "pthread_mutex_settype() failed (%s)", strerror(Ret));
  }
  if ((Ret = pthread_mutex_init(&Mutex, &Attr)) != 0) {
    m->Message(m->ERROR, "pthread_mutex_init() failed (%s)", strerror(Ret));
	return;
  }

  // Create thread that receives data
  if ((Ret = pthread_create(&Thread, NULL, (void * (*)(void *)) LaunchThread,(void *) this)) != 0) {
    m->Message(m->ERROR, "pthread_create() failed in FADBoard() (%s)\n", strerror(Ret));
	Thread = pthread_self();
	return;
  }

  InitOK = true;
}

//
// Destructor
//
FADBoard::~FADBoard() {

  int Ret;

  // Cancel thread and wait for it to quit
  if (pthread_equal(Thread, pthread_self()) == 0) {
	if ((Ret = pthread_cancel(Thread)) != 0) m->Message(m->ERROR, "pthread_cancel() failed in ~FADBoard() (%s)", strerror(Ret));
	if ((Ret = pthread_join(Thread, NULL)) != 0) m->Message(m->ERROR, "pthread_join() failed in ~FADBoard (%s)", strerror(Ret));
  }

  // Delete mutex  
  if (InitOK && ((Ret = pthread_mutex_destroy(&Mutex)) != 0)) {
	m->Message(m->ERROR, "pthread_mutex_destroy() failed in ~FADBoard (%s)", strerror(Ret));
  }

  delete DIM_Name;
  delete DIM_ID;
  delete DIM_Temp;
  delete DIM_DAC;
  delete DIM_ROI;
  delete[] Name;

  // Close socket descriptor
  if ((Socket != -1) && (close(Socket) == -1)) {
	m->PrintMessage("Could not close socket descriptor (%s)", strerror(errno));  
  }
}


//
// Send data to board
//
void FADBoard::Send(const void *Data, size_t Bytes) {

  // Do not send if not active
  if (!Active) return;

  // Write data
  ssize_t Result = write(Socket, Data, Bytes);

  // Check result
  if (Result == -1) m->PrintMessage("Error: Could not write to socket (%s)\n", strerror(errno));
  else if ((size_t) Result < Bytes) m->PrintMessage("Error: Could only write %d bytes out of %d to socket\n", Result, Bytes);
}

void FADBoard::Send(unsigned short Data) {

  unsigned short Buffer = htons(Data);
  
  Send(&Buffer, sizeof(unsigned short));
}


//
// Get board status (mutex protected to avoid concurrent access in ReadLoop)
//
struct FADBoard::BoardStatus FADBoard::GetStatus() {

  int Ret;
  struct BoardStatus S;
  
  // Lock
  if ((Ret = pthread_mutex_lock(&Mutex)) != 0) {
	m->Message(m->FATAL, "pthread_mutex_lock() failed in ReadLoop() (%s)", strerror(Ret));
  }

  S = Status; 

  // Unlock
  if ((Ret = pthread_mutex_unlock(&Mutex)) != 0) {
	m->Message(m->FATAL, "pthread_mutex_unlock() failed in Unlock() (%s)", strerror(Ret));
  }
  
  return S;  
}

//
// Initiate average
//
void FADBoard::AccumulateSum(unsigned int n) {

  if (!Active) return;
  
  Lock();
  memset(Sum, 0, sizeof(Sum));
  NumForSum = n;
  DoSum = true; 
  Unlock();
}


//
// Read data from board
//
void FADBoard::ReadLoop() {

  static char Buffer[READ_BUFFER_SIZE];
  static unsigned int Pos = 0, Temp;
  const PEVNT_HEADER *Header = (PEVNT_HEADER *) Buffer;
  ssize_t Result;
  struct BoardStatus PrevStatus;

  memset(&PrevStatus, 0, sizeof(PrevStatus));

  while (!m->ExitRequest) {
    // Read data from socket
    Result = read(Socket, Buffer + Pos, sizeof(Buffer)-Pos);

	// Check result of read
	if (Result == -1) {
	  m->PrintMessage("Error: Could not read from socket, exiting read loop (%s)\n", strerror(errno));
	  CommError = true;
	  break;
	}
	else if (Result == 0) {
	  m->PrintMessage("Server not existing anymore, exiting read loop\n");
	  CommError = true;
	  break;
	}
	
	// If not active, discard incoming data
	if (!Active) continue;
	
	// Advance write pointer
	Pos += Result;
	
	// Check if internal buffer full
	if (Pos == sizeof(Buffer)) {
	  m->PrintMessage("Internal buffer full, deleting all data in buffer\n");
	  Pos = 0;
	  continue;
	}
	
	// Check if buffer starts with start_package_flag, remove data if not
	Temp = 0;
	while (ntohs(*((unsigned short *) (Buffer+Temp))) != 0xfb01 && Temp<Pos) Temp++;
	if (Temp != 0) {
	  memmove(Buffer, Buffer+Temp, Pos-Temp);
	  Pos -= Temp;
	  m->PrintMessage("Removed %d bytes because of start_package_flag not found\n", Temp);
	  continue;
	}

	// Wait until the buffer contains at least enough bytes to potentially hold a PEVNT_HEADER
	if (Pos < sizeof(PEVNT_HEADER)) continue;
	
	unsigned int Length = ntohs(Header->package_length)*2*sizeof(char);
	if (Pos < Length) continue;

	// Extract data if event end package flag correct
	if (ntohs(*(unsigned short *) (Buffer+Length-sizeof(unsigned short))) == 0x04FE) {

	  // Prepare pointers to channel data (channels stored in order 0,9,18,27 - 1,10,19,28 - ... - 8,17,26,35)
	  PCHANNEL *Channel[NChips*NChannels], *Pnt=(PCHANNEL *) (Header+1); 
	  for(unsigned int i=0; i<NChips*NChannels; i++) {
		Channel[i] = Pnt;
		Pnt = (PCHANNEL *) ((short *) (Channel[i] + 1) + ntohs(Channel[i]->roi));
	  } 

	  PrevStatus = Status;

	  // Lock to avoid concurrent access in GetStatus()
	  Lock();

	  gettimeofday(&Status.Update, NULL);

	  // Extract ID and type information
	  Status.BoardID = ntohl(Header->board_id);
	  Status.TriggerID = ntohl(Header->local_trigger_id);
	  Status.TriggerType = ntohs(Header->local_trigger_type);

	  // Extract temperatures (MSB indicates if temperature is positive or negative)
	  for (unsigned int i=0; i<NTemp; i++) {
		if ((ntohs(Header->drs_temperature[i]) & 0x8000) == 0) Status.Temp[i] = float(ntohs(Header->drs_temperature[i]) >> 3)/16;
		else Status.Temp[i] = float(0xE000 | (ntohs(Header->drs_temperature[i])) >> 3)/16;
	  }

	  // Extract DAC channels
	  for (unsigned int i=0; i<NDAC; i++) Status.DAC[i] = ntohs(Header->dac[i]);

	  short Buf;
	  for (unsigned int Chip=0; Chip<NChips; Chip++) {
		// Extract trigger cells	  
		Status.TriggerCell[Chip] = (int) ntohs(Channel[Chip]->start_cell);
	  
		for (unsigned int Chan=0; Chan<NChannels; Chan++) {
		  // Extract ROI
		  Status.ROI[Chip][Chan] = ntohs(Channel[Chip+NChips*Chan]->roi);

		  // Extract ADC data (stored in 12 bit signed twis complement with out-of-range-bit and leading zeroes)
		  for (int i=0; i<Status.ROI[Chip][Chan]; i++) {
			  Buf = ntohs(Channel[Chip+NChips*Chan]->adc_data[i]);
			  (Buf <<= 4) >>= 4;			//delete the sign-bit by shifting left and shift back
			  Data[Chip][Chan][i] = Buf;					
		  }
		}
	  }
	  
	  // If requested for calibration, add rotated data for later averaging
	  if (DoSum && NumForSum>0) {
		for (unsigned int Chip=0; Chip<NChips; Chip++) for (unsigned int Chan=0; Chan<NChannels; Chan++) {
		  for (int i=0; i<Status.ROI[Chip][Chan]; i++) {
			Sum[Chip][Chan][(i+Status.TriggerCell[Chip]) % NBins] += Data[Chip][Chan][i];
		  }
		}
		NumForSum--;
		if (NumForSum == 0) DoSum = false;
	  }

	  Unlock();
	  
	  // Update DIM services if necessary
	  if (memcmp(PrevStatus.Temp, Status.Temp, sizeof(Status.Temp)) != 0) {
		DIM_Temp->updateService(Status.Temp, sizeof(Status.Temp));
	  }
	  if (memcmp(PrevStatus.DAC, Status.DAC, sizeof(Status.DAC)) != 0) {
		DIM_DAC->updateService(Status.DAC, sizeof(Status.DAC));
	  }  
	  if (memcmp(PrevStatus.ROI, Status.ROI, sizeof(Status.ROI)) != 0) {
		DIM_ROI->updateService(Status.ROI, sizeof(Status.ROI));
	  }  
	  if (PrevStatus.BoardID != Status.BoardID) {
		DIM_ID->updateService(&Status.BoardID, sizeof(Status.BoardID));
	  }  
	}
	else printf("End package flag incorrect, removing corrupt event\n");

	// Remove event data from internal buffer
	memmove(Buffer, Buffer+Length, Pos-Length);
	Pos = Pos-Length;	
  } // while()
}


//
// Launch read thread inside class
//
void FADBoard::LaunchThread(class FADBoard *m) {

  m->ReadLoop();
}


//
// Lock and unlock mutex
//
void FADBoard::Lock() {

  int Ret;

  if ((Ret = pthread_mutex_lock(&Mutex)) != 0) {
	m->Message(m->FATAL, "pthread_mutex_lock() failed in class FADBoard (%s)", strerror(Ret));
  }
}

void FADBoard::Unlock() {

  int Ret;

  if ((Ret = pthread_mutex_unlock(&Mutex)) != 0) {
	m->Message(m->FATAL, "pthread_mutex_unlock() failed in class FADBoard (%s)", strerror(Ret));
  }
}