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

  Class interfacing to FAD board

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

#include "FADBoard.h"
using namespace std;

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

  int Ret;

  // Initialization
  m = Parent;
  Active = false;
  Continue = true;
  CommOK = false;
  ACalib.Time = -1;
  Status.Update.tv_sec = -1;
  Port = ServerPort;
  Status.Frequency = 0;
  Status.Rate = 0;
  Status.BoardID = 0;
  
  Name = new char [Server.size()+1]; // Name in permanent memory for DIM service
  strcpy(Name, Server.c_str());

  // Initialise mutex for synchronization
  pthread_mutexattr_t Attr;

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

  // Initialise condition variable for synchronization
  if ((Ret = pthread_cond_init(&CondVar, NULL)) != 0) {
    m->Message(m->FATAL, "pthread_cond_init() failed in FADBoard constructor (%s)", strerror(Ret));
  }

  // 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_Status = new DimService((ID.str()+"Status").c_str(), (char *) "");
  DIM_ID = new DimService((ID.str()+"BoardID").c_str(), (char *) "S", NULL, 0);
  DIM_Rate = new DimService((ID.str()+"RateHz").c_str(), Status.Rate);
  DIM_Frequency = new DimService((ID.str()+"Frequency").c_str(), Status.Frequency);
  DIM_BoardTime = new DimService((ID.str()+"BoardTime").c_str(), (char *) "I", &Status.BoardTime, sizeof(Status.BoardTime));
  DIM_Lock = new DimService((ID.str()+"Lock").c_str(), (char *) "S", NULL, 0);
  DIM_TriggerNum = new DimService((ID.str()+"TriggerNum").c_str(), (char *) "I", &Status.TriggerNum, sizeof(Status.TriggerNum));
  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);
  DIM_ACalData = new DimService((ID.str()+"ACalData").c_str(), (char *) "F", NULL, 0);

  // Create thread that connects and receives data
  SetStatus("Trying to connect...");

  if ((Ret = pthread_create(&Thread, NULL, (void * (*)(void *)) LaunchThread, (void *) this)) != 0) {
    m->Message(m->FATAL, "pthread_create() failed in FADBoard() (%s)", strerror(Ret));
  }

  // Start thread to connect to other sockets
  DimThread::start();
}

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

  int Ret;

  // Cancel thread (if it did not quit already) and wait for it to quit
  if ((Ret = pthread_cancel(Thread)) != 0 && Ret != ESRCH) {
	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 condition variable 
  if ((Ret = pthread_cond_destroy(&CondVar)) != 0) {
	m->Message(m->ERROR, "pthread_cond_destroy() failed for %s in ~FADBoard (%s)", Name, strerror(Ret));
  }

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

  delete DIM_Name;			delete DIM_Status;
  delete DIM_ID;			delete DIM_Rate;
  delete DIM_Frequency;		delete DIM_Lock;
  delete DIM_TriggerNum;	delete DIM_Temp;
  delete DIM_DAC;			delete DIM_ROI;
  delete DIM_ACalData;		delete DIM_BoardTime;
  delete[] Name;
}


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

  // Do not send if not active or communication problem
  if (!Active || !CommOK) 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;  
}


//
// Perform amplitude calibration in steps
//
// The steps are intended to assure that up to date data is available
void FADBoard::AmplitudeCalibration() {

  vector<unsigned short> ROICmd;
  unsigned short DACCmd[] = {htons(CMD_Write | (BADDR_DAC + 1)), 0, htons(CMD_Write | (BADDR_DAC + 2)), 0, htons(CMD_Write | (BADDR_DAC + 3)), 0, htons(CMD_Execute) };
  string Message = string("ACALIBDONE")+Name+"\n";

  switch (State) {
  // ====== Part A: Check if amplitude calibration should start and initialise =====
  case standbye:
	if (m->Mode != m->acalib) break;

	// Invalidate current calibration
	ACalib.Time = -1;
	Count = 0;

	// Save initial board status, set all ROIs to 1024 and set DAC values (no triggers while setting ROI)
	InitialStatus = GetStatus();

	for (unsigned int i=0; i<NChips*NChannels; i++) {
	  ROICmd.push_back(htons(CMD_Write | (BADDR_ROI + i)));
	  ROICmd.push_back(htons(NBins));
	}
	ROICmd.push_back(htons(CMD_Execute));
	Send(&ROICmd[0], ROICmd.size()*sizeof(unsigned short));

	DACCmd[1] = htons(0);
	DACCmd[3] = htons(0);
	DACCmd[5] = htons(0);
	Send(DACCmd, sizeof(DACCmd));

	// Clear sum vector and set state to accumulate
	memset(Sum, 0, sizeof(Sum));
	State = baseline;
	SetStatus("Starting calilbration");
	break;

  // ====== Part B: Baseline calibration =====
  case baseline:
	// Check for stopping
	if (m->Mode != m->acalib) {
	  State = cleanup;
	  break;
	}

	// Average
	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];
		}
	  }
	}
    Count++;
	
	// Determine baseline if integration finished
	if (Count < m->NumEventsRequested) break;

	for (unsigned int i=0; i<NChips; i++) {
	  for (unsigned int j=0; j<NChannels; j++) {
		for (unsigned int k=0; k<NBins; k++) {
		  ACalib.Baseline[i][j][k] = Sum[i][j][k] / m->NumEventsRequested;
		}
	  }
	}

	// Set new DAC values and start accumulation
	DACCmd[1] = htons(50000);
	DACCmd[3] = htons(50000);
	DACCmd[5] = htons(50000);
	Send(DACCmd, sizeof(DACCmd));

	// Clear sum vector and set state to accumulate
	memset(Sum, 0, sizeof(Sum));
	Count = 0;
	State = gain;
	break;

  // ====== Part C: Gain calibration =====
  case gain:
	// Check for stopping
	if (m->Mode != m->acalib) {
	  State = cleanup;
	  break;
	}

	// Average
	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];
		}
	  }
	}
    Count++;
	
	// Determine gain if integration finished
	if (Count < m->NumEventsRequested) break;
	
	for (unsigned int i=0; i<NChips; i++) {
	  for (unsigned int j=0; j<NChannels; j++) {
		for (unsigned int k=0; k<NBins; k++) {
		  ACalib.Gain[i][j][k] = (Sum[i][j][k] / m->NumEventsRequested) - ACalib.Baseline[i][j][k];
		}
	  }
	}

	// Set new DAC values and start accumulation
	DACCmd[1] = htons(0);
	DACCmd[3] = htons(0);
	DACCmd[5] = htons(0);
	Send(DACCmd, sizeof(DACCmd));

	// Clear sum vector and set state to accumulate
	memset(Sum, 0, sizeof(Sum));
	Count = 0;
	State = secondary;
	break;

  // ====== Part D: Secondary calibration =====
  case secondary:
	// Check for stopping
	if (m->Mode != m->acalib) {
	  State = cleanup;
	  break;
	}

	// Average
	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] = Data[Chip][Chan][i] - ACalib.Baseline[Chip][Chan][(i-Status.TriggerCell[Chip]) % NBins];
		}
	  }
	}
    Count++;
	
	// Determine secondary baseline if integration finished
	if (Count < m->NumEventsRequested) break;

	for (unsigned int i=0; i<NChips; i++) {
	  for (unsigned int j=0; j<NChannels; j++) {
		for (unsigned int k=0; k<NBins; k++) {
		  ACalib.Secondary[i][j][k] = Sum[i][j][k] / (double) m->NumEventsRequested;
		}
	  }
	}

	// Store calibration time and temperature
	ACalib.DNA = Status.DNA;
	ACalib.Frequency = Status.Frequency;
	ACalib.Time = time(NULL);
	ACalib.Temp = 0;
	for (unsigned int i=0; i<NTemp; i++) ACalib.Temp += Status.Temp[i] / NTemp;

	// Inform event thread that calibration is finished for this board
	if (write(m->Pipe[1], Message.data(), Message.size()) == -1) {
	  m->Message(m->ERROR, "write() to Pipe[1] failed in class FADBoard::AmplitudeCalibration (%s)", strerror(errno));
	}

	SetStatus("Finished calibration");
	State = cleanup;
	break;

  // ====== Part E: Write back original ROI and DAC settings =====
  case cleanup:
    // ROI values

	ROICmd.clear();
	for (unsigned int i=0; i<NChips*NChannels; i++) {
	  ROICmd.push_back(htons(CMD_Write | (BADDR_ROI + i)));
	  ROICmd.push_back(htons(InitialStatus.ROI[i/NChannels][i%NChannels]));
	}
	ROICmd.push_back(htons(CMD_Execute));
	Send(&ROICmd[0], ROICmd.size()*sizeof(unsigned short));

	// DAC values
	DACCmd[1] = htons(InitialStatus.DAC[1]);
	DACCmd[3] = htons(InitialStatus.DAC[2]);
	DACCmd[5] = htons(InitialStatus.DAC[3]);
  	Send(DACCmd, sizeof(DACCmd));
	
	// Update DIM service with calibration information
	for (unsigned int i=0; i<NChips; i++) {
	  for (unsigned int j=0; j<NChannels; j++) {
		for (unsigned int k=0; k<NBins; k++) {
		  ACalData[0][i][j][k] = ACalib.Baseline[i][j][k];
		  ACalData[1][i][j][k] = ACalib.Gain[i][j][k];
		  ACalData[2][i][j][k] = ACalib.Secondary[i][j][k];
		}
	  }
	}
	DIM_ACalData->updateService(ACalData, 3*NChips*NChannels*NBins*sizeof(float));

	State = wait;
	break;

  // ====== Wait for Mode not being idle =====
  case wait:
	if (m->Mode == m->idle) State = standbye;
	break;
  }
}  
  
//
// Connect to board and read data
//
void FADBoard::ReadLoop() {

  char Buffer[READ_BUFFER_SIZE];
  unsigned int Pos = 0, Count = 0;
  const PEVNT_HEADER *Header = (PEVNT_HEADER *) Buffer;
  ssize_t Result;
  struct sockaddr_in SocketAddress;
  struct BoardStatus PrevStatus;
  int Ret;

  // Resolve hostname
  struct hostent *Host = gethostbyname(Name);
  if (Host == 0) {
    SetStatus("Could not resolve host name '%s'", Name);
    return;
  }

  SocketAddress.sin_family = PF_INET;
  SocketAddress.sin_port = htons(Port);
  SocketAddress.sin_addr = *(struct in_addr*) Host->h_addr;

  // Open socket descriptor
  if ((Socket = socket(PF_INET, SOCK_STREAM, 0)) == -1) {
    m->Message(m->ERROR, "Could not open socket for %s (%s)\n", Name, strerror(errno));
    return;
  }
    
  // Connect to server
  if (connect(Socket, (struct sockaddr *) &SocketAddress, sizeof(SocketAddress)) == -1) {
    SetStatus("Could not connect to port %hu (%s)", Port, strerror(errno));
  }
  else {
	CommOK = true;
	Active = true;
    SetStatus("Connected");
  }

  // Use not zero so that comparing Status and PrevStatus at first test will likely show differences
  memset(&PrevStatus, 0xee, sizeof(PrevStatus));

  // Leave loop if program termination requested or board communication not OK
  while (!m->ExitRequest && CommOK) {
    // Read data from socket
    Result = read(Socket, Buffer + Pos, sizeof(Buffer)-Pos);

	// Check result of read
	if (Result == -1) {
	  m->Message(m->ERROR, "Could not read from socket for %s, exiting read loop (%s)\n", Name, strerror(errno));
	  CommOK = false;
	  break;
	}
	else if (Result == 0) {
	  SetStatus("Server not existing anymore, exiting read loop");
	  CommOK = false;
	  break;
	}
	
	// If not active, discard incoming data
	if (!Active) continue;

	// Advance write pointer
	Pos += Result;
	
	// Check if internal buffer full
	if (Pos == sizeof(Buffer)) {
	  SetStatus("Internal buffer full, deleting all data in buffer");
	  Pos = 0;
	  continue;
	}
	
	// Check if buffer starts with start_package_flag, remove data if not
	unsigned int Temp = 0;
	while (ntohs(*((unsigned short *) (Buffer+Temp))) != 0xfb01 && Temp<Pos) Temp++;
	if (Temp != 0) {
	  memmove(Buffer, Buffer+Temp, Pos-Temp);
	  Pos -= Temp;
	  SetStatus("Removed %d bytes because of start_package_flag not found", 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));
	  } 

	  // Wait until event thread processed the previous data and lock to avoid concurrent access in GetStatus()
	  Lock();
	  while (!Continue) {
		struct timespec Wakeup;
		Wakeup.tv_sec = time(NULL)+MAX_WAIT_FOR_CONDITION;
		Wakeup.tv_nsec = 0;
		if ((Ret = pthread_cond_timedwait(&CondVar, &Mutex, &Wakeup)) != 0) {
		  if (Ret == ETIMEDOUT) SetStatus("Board %s timed out (%d s) waiting for condition\n", Name, MAX_WAIT_FOR_CONDITION);
		  else m->Message(m->ERROR, "pthread_cond_wait() failed (%s)", strerror(Ret));
		}
	  }
	  gettimeofday(&Status.Update, NULL);

	  // Extract board and trigger information
	  Status.BoardID = ntohs(Header->board_id);	  
	  Status.FirmwareRevision = ntohs(Header->version_no);
	  Status.BoardTime = ntohl(Header->time);
	  Status.EventCounter = ntohl(Header->fad_evt_counter);
	  Status.TriggerNum = ntohl(Header->trigger_id);
	  Status.Runnumber = ntohl(Header->runnumber);
	  Status.TriggerType = ntohs(Header->trigger_type);
	  Status.TriggerCRC = ntohs(Header->trigger_crc);
	  Status.DNA = Header->DNA;

	  // Extract frequency related information
	  Status.Frequency = ntohl(Header->REFCLK_frequency)/1.0e3*2.048;
	  Status.PhaseShift = Header->adc_clock_phase_shift;
	  for (unsigned int i=0; i<NChips; i++) {
		if ((ntohs(Header->PLLLCK)>>12 & (1<<i)) != 0) Status.Lock[i] = 1;
		else Status.Lock[i] = 0;
	  }

	  // Extract Firmware status info
	  Status.denable = (bool) ( ntohs(Header->PLLLCK) & (1<<11) );
	  Status.dwrite = (bool) ( ntohs(Header->PLLLCK) & (1<<10) );
	  Status.DCM_lock = (bool) ( ntohs(Header->PLLLCK) & (1<<7) );
	  Status.DCM_ready = (bool) ( ntohs(Header->PLLLCK) & (1<<6) );
	  Status.spi_clk = (bool) ( ntohs(Header->PLLLCK) & (1<<5) );
	  Status.RefClk_low = (bool) ( ntohs(Header->PLLLCK) & (1<<8) );

	  // 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]);

	  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 as signed short)
		  // FADs ADC is 12 bit (values -2048 .. 2047)
		  // negative/positive overflow is -2049 / +2048
		  for (int i=0; i<Status.ROI[Chip][Chan]; i++) {			  
			Data[Chip][Chan][i] = Channel[Chip+NChips*Chan]->adc_data[i];
		  }
		}
	  }
	  
	  // Prepare predicate for condition variable
	  Continue = false;
	  Count++;
	  Unlock();
	  	  
  	  // Amplitude calibration (will check if Mode is acalib)
	  AmplitudeCalibration();

	  // Update DIM services if necessary
	  if (Status.Update.tv_sec - PrevStatus.Update.tv_sec > m->EventUpdateDelay) {

		// Check if trigger cells resonable (to trace FAD 'double signal' bug)
		int Diff = abs((*max_element(Status.TriggerCell,Status.TriggerCell+4) - *min_element(Status.TriggerCell,Status.TriggerCell+4)));
		if (Diff > 20 && Diff < 1000) {
		  SetStatus("Warning: Trigger cell mismatch board %s, cells are %d %d %d %d", Name, Status.TriggerCell[0], Status.TriggerCell[1], Status.TriggerCell[2], Status.TriggerCell[3]);
		  m->Message(m->WARN, "Trigger cell mismatch board %s, cells are %d %d %d %d", Name, Status.TriggerCell[0], Status.TriggerCell[1], Status.TriggerCell[2], Status.TriggerCell[3]);
		}

		// Determine event rate
	    Status.Rate =
	   Count / (double(Status.Update.tv_sec-PrevStatus.Update.tv_sec) + (Status.Update.tv_usec-PrevStatus.Update.tv_usec)/1000000.0);
	    Count = 0;

		if (PrevStatus.Frequency != Status.Frequency) DIM_Frequency->updateService();
		if (PrevStatus.TriggerNum != Status.TriggerNum) DIM_TriggerNum->updateService();
		if (PrevStatus.BoardTime != Status.BoardTime) DIM_BoardTime->updateService();
		if (PrevStatus.Rate != Status.Rate) DIM_Rate->updateService();

		if (memcmp(PrevStatus.Lock, Status.Lock, sizeof(Status.Lock)) != 0) {
		  DIM_Lock->updateService(Status.Lock, sizeof(Status.Lock));
		}
		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));
		}
		
		PrevStatus = Status;
	  }
	  
	  // Inform event thread of new data
	  string Message = string("EVENT")+Name+"\n";
	  if (write(m->Pipe[1], Message.data(), Message.size()) == -1) {
		m->Message(m->ERROR, "write() to Pipe[1] failed in class FADBoard (%s)", strerror(errno));
		break;
	  }
	}
	else SetStatus("End package flag incorrect, removing corrupt event");

	// Remove event data from internal buffer
	memmove(Buffer, Buffer+Length, Pos-Length);
	Pos = Pos-Length;	
  } // while()
  
  // Set inactive and close socket descriptor
  Active = false;

  if (close(Socket) == -1) {
	m->Message(m->ERROR, "Could not close socket descriptor for board %s (%s)", Name, strerror(errno));  
  }

}

//
// Install cleanup handler and launch read thread inside class
//
void FADBoard::LaunchThread(class FADBoard *m) {

  pthread_cleanup_push((void (*)(void *)) FADBoard::ThreadCleanup, (void *) m);
  m->ReadLoop();
  pthread_cleanup_pop(0);
}


//
// Set status message
//
void FADBoard::SetStatus(const char *Format, ...) {

  int Ret;

  // Assemble message
  va_list ArgumentPointer;
  va_start(ArgumentPointer, Format);
  Lock();
  Ret = vsnprintf(Status.Message, sizeof(Status.Message), Format, ArgumentPointer);
  Unlock();
  va_end(ArgumentPointer);

  if (Ret == -1) m->Message(m->FATAL, "snprintf() in FADBoard::SetStatus() failed (%s)", strerror(errno));

  // Update status service
  DIM_Status->updateService(Status.Message);
}


//
// 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));
  }
}

// Ensure that mutex is unlocked when before cancelling thread
void FADBoard::ThreadCleanup(class FADBoard *This) {

  int Ret;

  if ((Ret = pthread_mutex_trylock(&This->Mutex)) != 0) {
	if (Ret != EBUSY) This->m->Message(This->m->FATAL, "pthread_mutex_trylock() failed in FADBoard::ThreadCleanup (%s)", strerror(Ret));
  }
  This->Unlock();
}

//
// Open other sockets
//
//  Error reporting is limited as this function is expected to be removed when firmware allows single socket
//
void FADBoard::threadHandler() {

  int List[] = {31920, 31921, 31922, 31923, 31924, 31925, 31926};
  int Socket[sizeof(List)/sizeof(int)], MaxSocketNum, Ret;
  fd_set DescriptorList;
  char Buffer[1000000];
  
  // Resolve hostname
  struct hostent *Host = gethostbyname(Name);
  if (Host == 0) return;

  // Connect to server
  struct sockaddr_in SocketAddress;
  SocketAddress.sin_family = PF_INET;
  SocketAddress.sin_addr = *(struct in_addr*) Host->h_addr;

  for (unsigned int i=0; i<sizeof(List)/sizeof(int); i++) {
	// Open socket descriptor
	if ((Socket[i] = socket(PF_INET, SOCK_STREAM, 0)) == -1) {
      m->Message(m->ERROR, "OtherSockets: Could not open socket for port %d (%s)\n", List[i], strerror(errno));
      return;
	}
	MaxSocketNum = *max_element(Socket, Socket+sizeof(List)/sizeof(int));
	 
	// Connect to server
    SocketAddress.sin_port = htons((unsigned short) List[i]);
	if (connect(Socket[i], (struct sockaddr *) &SocketAddress, sizeof(SocketAddress)) == -1) return;
  }
  
  while(true) {
    // Wait for data from sockets
    FD_ZERO(&DescriptorList);   
    for (unsigned int i=0; i<sizeof(List)/sizeof(int); i++) FD_SET(Socket[i], &DescriptorList);
    if (select(MaxSocketNum+1, &DescriptorList, NULL, NULL, NULL) == -1) {
      m->Message(m->ERROR, "OtherSockets: Error with select() (%s)\n", strerror(errno));
      break;
    }
	
	// Data from socket
	for (unsigned int i=0; i<sizeof(List)/sizeof(int); i++) if (FD_ISSET(Socket[i], &DescriptorList)) {
	  Ret = read(Socket[i], Buffer, sizeof(Buffer));
      if (Ret == -1) m->Message(m->ERROR, "OtherSockets: Error reading from port %d (%s)\n", List[i], strerror(errno));
    }
  }

  // Close all sockets
  for (unsigned int i=0; i<sizeof(List)/sizeof(int); i++) {
	if ((Socket[i] != -1) && (close(Socket[i]) == -1)) {
	  m->Message(m->ERROR, "OtherSockets: Could not close socket of port %d (%s)", List[i], strerror(errno));  
	}
  }
}