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

  HVFeedback.cc

  Class handling the feedback of GAPD high voltage
    
  Oliver Grimm

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

#include "HVFeedback.h"
#include "PixelMap.h"

#include <sys/socket.h>
#include <netdb.h>
#include <signal.h>

#define MAX_RETRY 5
#define PIXMAP_LOCATION "../config/PixelMap.txt"

//
// Constructor: Initialise feedback 
//
HVFeedback::HVFeedback(DAQReadout* DAQClass, char* Configfile) {
  struct sockaddr_in SocketAddress;
  char Filename[MAX_PATH];

  m = DAQClass;
  PixMap = new PixelMap(PIXMAP_LOCATION, false);

  snprintf(Filename,sizeof(Filename),"%s/SlowData/", m->fRawDataPath);
  SlowDataClass = new SlowData(m, "HVFB", Filename);

  // Read configuration file
  FILE *File;
  if ((File = fopen(Configfile,"r")) == NULL) {
    printf("Error: Could not open feedback configuration file '%s'\n", Configfile);
  }
  else {
    printf("Reading feedback configuration file %s\n", Configfile);
    ReadCard("TrigBoard",           &fLedTrigBoard,      'I', File);
    ReadCard("TrigChannel",         &fLedTrigChannel,    'I', File);
    ReadCard("TrigChip",            &fLedTrigChip,       'I', File);
    ReadCard("TrigSample",          &fLedTrigSample,     'I', File);
    ReadCard("TrigThreshold",       &fLedTrigThreshold,  'f', File);
    ReadCard("SignalSample",        &fLedSignalSample,   'I', File);
    ReadCard("BaselineSample",      &fLedBaselineSample, 'I', File);
    ReadCard("IntHalfWidth",        &fIntHalfWidth, 	 'U', File);
    ReadCard("DefaultNumAverage",   &fDefaultNumAverage, 'I', File);
    ReadCard("HVControlServer",      fHVControlServer,   's', File);
    ReadCard("HVControlPort",       &fHVControlPort,     'I', File);
    ReadCard("MaxCmdAckDelay",      &fMaxCmdAckDelay,    'I', File);
    fclose(File);
  }
  PrintConfig();

  // Initialise with zero content
  Average    = new float [m->NumBoards][kNumberOfChips][kNumberOfChannels]();
  Sigma      = new float [m->NumBoards][kNumberOfChips][kNumberOfChannels]();
  Response   = new float [m->NumBoards][kNumberOfChips][kNumberOfChannels]();
  Target     = new float [m->NumBoards][kNumberOfChips][kNumberOfChannels]();
  Buffer     = new float [m->NumBoards][kNumberOfChips][kNumberOfChannels]();  

  Gain = 1;
  SetFBMode(FB_Off);
  SetNumAverages(fDefaultNumAverage);

  // Opening socket client to HV control program
  if ((SocketDescriptor = socket(PF_INET, SOCK_STREAM, 0)) == -1)
    m->PrintMessage("Could not open client socket, no HV control available.\n");

  // Resolve hostname and try to connect to server
  struct hostent *hostent = gethostbyname(fHVControlServer);
  if (hostent==0)
    m->PrintMessage("Could not resolve HV server host name \"%s\".\n", fHVControlServer);
  else {
    SocketAddress.sin_family = PF_INET;
    SocketAddress.sin_port = htons((unsigned short) fHVControlPort);
    SocketAddress.sin_addr = *(struct in_addr*) hostent->h_addr;
  
    if (connect(SocketDescriptor, (struct sockaddr *) &SocketAddress, sizeof(SocketAddress))==-1)
      m->PrintMessage("Could not connect to HV server %s at port %d (%s)\n", fHVControlServer, fHVControlPort, strerror(errno));
    else m->PrintMessage("\nFeedback connected to HV server %s (port %d).\n", fHVControlServer, fHVControlPort);
    signal(SIGPIPE,SIG_IGN);  // Do not kill process if writing to closed socket
  }
}

//
// Destructor
//
HVFeedback::~HVFeedback() {
  if (SocketDescriptor!=-1) {
    close(SocketDescriptor);
    m->PrintMessage("Feeback socket closed.\n");
  }
  delete[] Average;   	delete[] Response;
  delete[] Target;   	delete[] Buffer;
  delete SlowDataClass; delete PixMap;
}

//
// Check if LED trigger present, if yes accumulate feedback data and
// calculate new high voltages if the required number of events is reached.
//
bool HVFeedback::ProcessEvent() {
  int i,j,k,q;
  float Correction, Integral, Difference, EffectiveGain;
  
  // Check for LED trigger channel on given channel and if feedback running
  if (FBMode==FB_Off || m->WaveForm[fLedTrigBoard][fLedTrigChip][fLedTrigChannel][fLedTrigSample] < fLedTrigThreshold)
    return false;
  
  // Calculate average signal of LED pulse as integral of signal
  for (i=m->FirstBoard; i<=m->LastBoard; i++)
    for (j=0; j<kNumberOfChips; j++)
      for (k=0; k<kNumberOfChannels; k++) {
	for (Integral=0, q=-fIntHalfWidth; q<=(int) fIntHalfWidth; q++) 
          Integral += (m->WaveForm[i][j][k][fLedSignalSample+q] - m->WaveForm[i][j][k][fLedBaselineSample+q])*m->BStruct[i].ScaleFactor;
        Integral /= 2*fIntHalfWidth+1;
	Average[i][j][k] += Integral;
    	Sigma[i][j][k] += pow(Integral,2);
      }

  if (++Count<NumAverages) return false;

  // Acquired number of event requires action
  switch (FBMode) {
    case FB_Active: SlowDataClass->NewEntry("Average");  break;
    case FB_Targets: SlowDataClass->NewEntry("Target");  break;
    case FB_ResponseSecond: SlowDataClass->NewEntry("Response"); SlowDataClass->AddToEntry("%.3f ",DiffVoltage);  break;
    default: break;  // to suppress warning abount not handled enumeration value
  }			

  for (i=m->FirstBoard; i<=m->LastBoard; i++)
    for (j=0; j<kNumberOfChips; j++)
      for (k=0; k<kNumberOfChannels; k++) {
	Average[i][j][k] /= Count;
	Sigma[i][j][k] = sqrt(Sigma[i][j][k]/Count-pow(Average[i][j][k],2))/sqrt(Count);
	switch (FBMode) {
	  case FB_Active:   // Determine correction from response maxtrix and change voltages
    	    Difference = Target[i][j][k] - Average[i][j][k];
	    EffectiveGain = Gain*pow(0.5*(1+(Difference/Sigma[i][j][k]-1)/(Difference/Sigma[i][j][k]+1)),2);
	    Correction = -Difference*Response[i][j][k]*EffectiveGain;
            if(Correction!=0 && Target[i][j][k]!=0 && !PixMap->DRS_to_Pixel(i,j,k).empty()) {
              printf("Average of board %d, chip %d, channel %d is %.2f +/- %.2f    Correction %.3f\n",i,j,k,Average[i][j][k],Sigma[i][j][k],Correction);
	      SlowDataClass->AddToEntry("%d %d %d %.3f %.3f %.3f ", i,j,k,Average[i][j][k],Sigma[i][j][k],Correction);
	      if(fabs(Average[i][j][k]) < 2*Sigma[i][j][k]) printf("Too noisy!\n");
    	      else WriteHVCommand("hvdiff %s %f\n",PixMap->DRS_to_Pixel(i,j,k).c_str(),Correction);
	    }
	    break;
	  case FB_Targets:  // Take average as new targets  
	    Target[i][j][k] = Average[i][j][k];
 	    SlowDataClass->AddToEntry("%d %d %d %.3f %.3f ", i,j,k,Average[i][j][k],Sigma[i][j][k]);
	    break;
      	  case FB_ResponseFirst:  // First point of response measurement done  
	    Buffer[i][j][k] = Average[i][j][k];
	    if(!PixMap->DRS_to_Pixel(i,j,k).empty()) WriteHVCommand("hvdiff %s %f",PixMap->DRS_to_Pixel(i,j,k).c_str(), DiffVoltage);
	    break;
      	  case FB_ResponseSecond: // Determine response from signal variation
	    if(Buffer[i][j][k] == Average[i][j][k]) {
	      m->PrintMessage("HV Feedback: Warning, response singular for board %d, chip %d, channel %d.\n",i,j,k);
	      Response[i][j][k] = 0;
	    }
	    else Response[i][j][k] = DiffVoltage/(Buffer[i][j][k]-Average[i][j][k]);
	    SlowDataClass->AddToEntry("%d %d %d %.3f ", i,j,k,Response[i][j][k]);
	    if(!PixMap->DRS_to_Pixel(i,j,k).empty()) WriteHVCommand("hvdiff %s %f",PixMap->DRS_to_Pixel(i,j,k).c_str(), -DiffVoltage/2);
	    break;
	  default: break;  // to suppress warning abount not handled enumeration value
        }			
      }

  switch (FBMode) {
    case FB_Targets:
      m->PrintMessage("HV Feedback: New targets set, switching off.\n");
      FBMode = FB_Off;
      break;
    case FB_ResponseFirst:
      FBMode = FB_ResponseSecond;
      m->PrintMessage("HV Feedback: Increasing voltages by %f for response measurement, acquiring data.\n", DiffVoltage);
      break;
    case FB_ResponseSecond:
      m->PrintMessage("HV Feedback: Response measurements finished, original voltages set, switching off.\n");
      FBMode = FB_Off;
      break;
    default: break;  // to suppress warning abount not handled enumeration value
  }
  ClearAverages();
  return true;  
}

//
// Clear average values and event counter
//
void HVFeedback::ClearAverages() {
  for (int i=m->FirstBoard; i<=m->LastBoard; i++)
    for (int j=0; j<kNumberOfChips; j++)
      for (int k=0; k<kNumberOfChannels; k++) {
        Average[i][j][k] = 0.0;
	Sigma[i][j][k] = 0.0;
      }
  Count = 0;
}

//
// Number of events to accumulate before correction acts
//
void HVFeedback::SetNumAverages(unsigned int Averages) {
  NumAverages = Averages;
}

//
// Get requested number of events
//
unsigned int HVFeedback::GetNumAverages() {
  return NumAverages;
}

//
// Set feedback gain
//
void HVFeedback::SetGain(float FBGain) {
  Gain = FBGain;
}

//
// Get feedback gain
//
float HVFeedback::GetGain() {
  return Gain;
}

//
// Set feedback mode and clear averages
//
void HVFeedback::SetFBMode(FBState Mode) {
  if(Mode==FB_ResponseFirst || Mode==FB_ResponseFirst)
      m->PrintMessage("Start reponse measurement by calling MeasureResponse().\n");
  else {
    FBMode = Mode;
    ClearAverages();
  }
}

//
// Set feedback mode and clear averages
//
FBState HVFeedback::GetFBMode() {
  switch (FBMode) {
    case FB_Off: m->PrintMessage("Feedback off.\n");   break;
    case FB_Active: m->PrintMessage("Feedback active.\n");   break;
    case FB_Targets: m->PrintMessage("Feedback acquiring new targets.\n");   break;
    case FB_ResponseFirst: m->PrintMessage("Feedback measuring response with first voltage.\n");   break;
    case FB_ResponseSecond: m->PrintMessage("Feedback measuring response with second voltage.\n");   break; 
  }
  return FBMode;			
}

//
// Return current number of events
//
unsigned int HVFeedback::GetCurrentCount() {
  return Count;
}

//
// Set target values
//
void HVFeedback::SetTarget(int Board, int Chip, int Channel, float TargetVal) {
  if(Board<m->NumBoards && Chip<kNumberOfChips && Channel<kNumberOfChannels)
    Target[Board][Chip][Channel] = TargetVal;
  else printf("Invalid board, chip or channel number.\n");
}

//
// Print target values
//
void HVFeedback::GetTargets() {
  for (int i=m->FirstBoard; i<=m->LastBoard; i++)
    for (int j=0; j<kNumberOfChips; j++)
      for (int k=0; k<kNumberOfChannels; k++)
         m->PrintMessage("Board %d, chip %d, channel %d: %.2f\n",i,j,k,Target[i][j][k]);
}

//
// Measure response matrix
//
void HVFeedback::MeasureResponse(float U) {

  if (U==0) m->PrintMessage("HV Feedback: Error, voltage difference must not non-zero.\n");
  else {
    for (int i=m->FirstBoard; i<=m->LastBoard; i++) 
      for (int j=0; j<kNumberOfChips; j++) 
        for (int k=0; k<kNumberOfChannels-2; k++) {
	  if(PixMap->DRS_to_Pixel(i,j,k).empty()) m->PrintMessage("Could not find pixel ID of board %d, chip %d, channel %d\n",i,j,k);
          else WriteHVCommand("hvdiff %s %f\n",PixMap->DRS_to_Pixel(i,j,k).c_str(), -U/2);
        }
    DiffVoltage = U;
    FBMode = FB_ResponseFirst;
    ClearAverages();  
    m->PrintMessage("HV Feedback: Decreasing voltages by %f for response measurement, acquiring data.\n",DiffVoltage/2);
  }
}

//
// Print response values
//
void HVFeedback::GetResponse() {
  for (int i=m->FirstBoard; i<=m->LastBoard; i++)
    for (int j=0; j<kNumberOfChips; j++)
      for (int k=0; k<kNumberOfChannels; k++)
         m->PrintMessage("Board %d, chip %d, channel %d: %.3f\n",i,j,k,Response[i][j][k]);
}

//
// Write commmand to socket
//
bool HVFeedback::WriteHVCommand(const char *Format, ...) {
  char Textbuffer[MAX_COM_SIZE];
  fd_set SelectDescriptor;
  int RetryCount=0;
  
  do {
    va_list ArgumentPointer;  va_start(ArgumentPointer, Format); 
    vsnprintf(Textbuffer, sizeof(Textbuffer), Format, ArgumentPointer);

    // Write command to socket
    if(write(SocketDescriptor, Textbuffer, strlen(Textbuffer)+1)!=(int) strlen(Textbuffer)+1) {
      m->PrintMessage("Error: Could not write (entire) command to HV socket (%s)\n", strerror(errno));
      return false;
    }

    // Wait for command acknowledge from hvcontrol program
    FD_ZERO(&SelectDescriptor);   FD_SET(SocketDescriptor, &SelectDescriptor);
    struct timeval WaitTime = {fMaxCmdAckDelay, 0};
    if (select(((int) SocketDescriptor)+1, &SelectDescriptor, NULL, NULL, &WaitTime)==-1) {
      m->PrintMessage("Error with select() in command acknowledge (%s)\n", strerror(errno));
      return false;
    }
    if (!FD_ISSET(SocketDescriptor, &SelectDescriptor)) { // Time-out expired
      m->PrintMessage("Time-out of %d seconds expired before receiving acknowledge from HV socket.\n", fMaxCmdAckDelay);
      return false;
    }
    if (read(SocketDescriptor, Textbuffer, MAX_COM_SIZE) == -1) {
      m->PrintMessage("Error reading acknowledge from HV socket (%s)\n", strerror(errno));
      return false;
    }
  } while (strstr(Textbuffer,"WC ok")==NULL && ++RetryCount<=MAX_RETRY);
  if(RetryCount==MAX_RETRY) {
    m->PrintMessage("Could not set high voltage after %d tries.\n", MAX_RETRY);
    return false;
  }
  return true;
}

//
// Print feedback configuration
//
void HVFeedback::PrintConfig() {
  m->PrintMessage("LedTrigBoard: %d\t\tLedTrigChip: %d\t\tLedTrigChannel: %d\n"
        "LedTrigSample: %d\tLedTrigThreshold: %.2f\n"
        "LedSignalSample: %d\tLedBaselineSample: %d\tDefaultNumAverage: %d\n"
        "IntHalfWidth:%u\tHVControlServer: %s\tHVControlPort: %d\n"
	"MaxCmdAckDelay: %d\n",
    fLedTrigBoard, fLedTrigChip, fLedTrigChannel, fLedTrigSample,
    fLedTrigThreshold, fLedSignalSample, fLedBaselineSample,
    fDefaultNumAverage, fIntHalfWidth, fHVControlServer, fHVControlPort, fMaxCmdAckDelay);
}