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

  HVFeedback.cc

  Class handling the feedback of GAPD high voltage
    
  Oliver Grimm

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

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

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

static const char* FBState_Description[] = {
	"Feedback off",
	"Feedback active",
	"Feedback acquiring new targets",
	"Feedback measuring response with first voltage",
	"Feedback measuring response with second voltage"
};

using namespace std;

//
// Constructor: Initialise feedback 
//
HVFeedback::HVFeedback(DAQReadout* DAQClass){

  m = DAQClass;
  fNumberOfChannels = m->GetBoard(0)->GetNumberOfChannels();
  fNumberOfChips = m->GetBoard(0)->GetNumberOfChips();  

  PixMap = new PixelMap(PIXMAP_LOCATION, false);

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

  DIMAverage = new float [m->NumBoards][kNumberOfChipsMax][kNumberOfChannelsMax]();
  DIMSigma   = new float [m->NumBoards][kNumberOfChipsMax][kNumberOfChannelsMax]();

  // Get configuration data
  fLedTrigBoard = atoi(m->GetConfig("TrigBoard").c_str());
  fLedTrigChannel = atoi(m->GetConfig("TrigChannel").c_str());
  fLedTrigChip = atoi(m->GetConfig("TrigChip").c_str());
  fLedTrigSample = atoi(m->GetConfig("TrigSample").c_str());
  fLedTrigThreshold = atoi(m->GetConfig("TrigThreshold").c_str());
  fLedSignalSample = atoi(m->GetConfig("SignalSample").c_str());
  fLedBaselineSample = atoi(m->GetConfig("BaselineSample").c_str());
  fIntHalfWidth = atoi(m->GetConfig("IntHalfWidth").c_str());
  fDefaultNumAverage = atoi(m->GetConfig("DefaultNumAverage").c_str());

  vector<string> Token = m->Tokenize(m->GetConfig("DefaultResponse"), " \t");
  unsigned int N = 0;
  for (int i=0; i<m->NumBoards; i++) {
	for (int j=0; j<fNumberOfChips; j++) {
	  for (int k=0; k<fNumberOfChannels; k++) {
		if (N < Token.size()) Response[i][j][k] = (float) atof(Token[N++].c_str());
	  }
	}
  }

  Token = m->Tokenize(m->GetConfig("DefaultTarget"), " \t");
  N = 0;
  for (int i=0; i<m->NumBoards; i++) {
	for (int j=0; j<fNumberOfChips; j++) {
	  for (int k=0; k<fNumberOfChannels; k++) {
		if (N < Token.size()) Target[i][j][k] = (float) atof(Token[N++].c_str());
	  }
	}
  }
  
  // Provide DIM services
  FeedbackAverage = new DimService ("Feedback/Average", "F", DIMAverage, m->NumBoards*kNumberOfChipsMax*kNumberOfChannelsMax*sizeof(float));
  FeedbackSigma = new DimService ("Feedback/Sigma", "F", DIMSigma, m->NumBoards*kNumberOfChipsMax*kNumberOfChannelsMax*sizeof(float));
  FeedbackResponse = new DimService ("Feedback/Response", "F", Response, m->NumBoards*kNumberOfChipsMax*kNumberOfChannelsMax*sizeof(float));
  FeedbackTarget = new DimService ("Feedback/Target", "F", Target, m->NumBoards*kNumberOfChipsMax*kNumberOfChannelsMax*sizeof(float));
  CountService = new DimService ("Feedback/Count", Count);
  FeedbackState = new DimService ("Feedback/State", "I:1;C", NULL, 0);

  // Initial state
  Gain = atof(m->GetConfig("DefaultGain").c_str());
  SetFBMode(FB_Off);
  SetNumAverages(fDefaultNumAverage);
  LastServiceUpdate = 0;
}

//
// Destructor
//
HVFeedback::~HVFeedback() {

  delete FeedbackState;
  delete CountService;
  delete FeedbackAverage;
  delete FeedbackSigma;
  delete FeedbackResponse;
  delete FeedbackTarget;

  delete[] Average;   	delete[] Response;
  delete[] DIMAverage; 	delete[] DIMSigma;
  delete[] Sigma;
  delete[] Target;   	delete[] Buffer;
  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;

  // 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<fNumberOfChips; j++) {
	  for (k=0; k<fNumberOfChannels; 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->GetBoard(i)->GetPrecision();
		}
        Integral /= 2*fIntHalfWidth+1;
		Average[i][j][k] += Integral;
    	Sigma[i][j][k] += pow(Integral,2);
      }
	}
  }
  
  // Update DIM count service regularly
  if (time(NULL)-LastServiceUpdate > 2) {
    LastServiceUpdate = time(NULL);
	CountService->updateService();
  }
  // Check if acquired number of event requires action
  if (++Count<NumAverages) return false;

  // Feedback action
  std::stringstream Cmd;
  
  for (i=m->FirstBoard; i<=m->LastBoard; i++) {
  for (j=0; j<fNumberOfChips; j++) {
  for (k=0; k<fNumberOfChannels; k++) {
    // Calculate average
	Average[i][j][k] /= Count;
	Sigma[i][j][k] = sqrt(Sigma[i][j][k]/Count-pow(Average[i][j][k],2))/sqrt(Count);
	DIMAverage[i][j][k] = Average[i][j][k];
	DIMSigma[i][j][k] = Sigma[i][j][k];
	
	switch (FBMode) {
	  case FB_Active:
		// Determine correction from response maxtrix and change voltages
	    Correction = -(Target[i][j][k] - Average[i][j][k])*Response[i][j][k]*Gain;
    	// Limit voltage steps
		if (fabs(Correction) > 0.1) Correction = fabs(Correction)/Correction*0.1;   // Limit changes to 100 mV
		// Check if voltage change command possible
		if(Correction==0 || Target[i][j][k]==0 || PixMap->DRS_to_Pixel(i,j,k).empty()) break;
		// Add voltage change command if not too noisy
		if(fabs(Average[i][j][k]) > 2*Sigma[i][j][k]) {
		  Cmd << PixMap->DRS_to_Pixel(i,j,k)+" " << std::showpos << Correction << " ";
		}
	    break;

	  case FB_Targets:  // Take average as new targets  
	    Target[i][j][k] = Average[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()) {
		  Cmd << PixMap->DRS_to_Pixel(i,j,k) << " " << std::showpos << 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]);
	    if(!PixMap->DRS_to_Pixel(i,j,k).empty()) {
		  Cmd << PixMap->DRS_to_Pixel(i,j,k) << " " << std::showpos << -DiffVoltage/2 << " ";		  		  
		}
	    break;

	  default: break;  // to suppress warning abount not handled enumeration value
        }			
  } // for() channels
  } // for() chips
  } // for() boards

  // Update DIM service
  FeedbackAverage->updateService();
  FeedbackSigma->updateService();

  // Send command
  if (!Cmd.str().empty()) {
	DimClient::sendCommand("Bias/Command", ("hv "+Cmd.str()).c_str());
  }

  switch (FBMode) {
    case FB_Targets:
	  FeedbackTarget->updateService();
      m->PrintMessage("HV Feedback: New targets set, switching off\n");
      SetFBMode(FB_Off, true);
      break;
    case FB_ResponseFirst:
      SetFBMode(FB_ResponseSecond, true);
      m->PrintMessage("HV Feedback: Increasing voltages by %f for response measurement, acquiring data\n", DiffVoltage);
      break;
    case FB_ResponseSecond:
	  FeedbackResponse->updateService();
      m->PrintMessage("HV Feedback: Response measurements finished, original voltages set, switching off\n");
      SetFBMode(FB_Off, true);
      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<fNumberOfChips; j++)
      for (int k=0; k<fNumberOfChannels; k++) {
        Average[i][j][k] = 0.0;
		Sigma[i][j][k] = 0.0;
      }
  Count = 0;
  CountService->updateService();
}

//
// 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, bool Internal) {
  if((Mode==FB_ResponseFirst || Mode==FB_ResponseFirst) && !Internal)
      m->PrintMessage("Start reponse measurement by calling MeasureResponse()\n");
  else {
    FBMode = Mode;
	if (Mode != FB_ResponseFirst) m->PrintMessage("%s\n", FBState_Description[FBMode]);
	else m->PrintMessage("%s (voltage difference %.3f)\n", FBState_Description[FBMode], DiffVoltage);
    ClearAverages();
	
	// Update state service
	State.State = FBMode;
	strncpy(State.Text, FBState_Description[FBMode], sizeof(State.Text));
	FeedbackState->updateService(&State, sizeof(State));
  }
}

//
// Get feedback mode
//
FBState HVFeedback::GetFBMode() {

  m->PrintMessage("%s.\n", FBState_Description[FBMode]);
  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<fNumberOfChips && Channel<fNumberOfChannels) {
    Target[Board][Chip][Channel] = TargetVal;
	FeedbackTarget->updateService();
  }
  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<fNumberOfChips; j++) {
      m->PrintMessage("Board %d, chip %d:",i,j);
      for (int k=0; k<fNumberOfChannels; k++) m->PrintMessage(" %.2f",Target[i][j][k]);
      m->PrintMessage("\n\r");
    }
}

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

  std::stringstream Cmd;

  if (U==0) {
    m->PrintMessage("HV Feedback: Error, voltage difference must not non-zero.\n");
    return;
  }

  for (int i=m->FirstBoard; i<=m->LastBoard; i++) { 
  for (int j=0; j<fNumberOfChips; j++) { 
  for (int k=0; k<fNumberOfChannels; k++) {
	if(!PixMap->DRS_to_Pixel(i,j,k).empty()) {
	  Cmd << PixMap->DRS_to_Pixel(i,j,k) << " " << std::showpos << -U/2 << " ";		  		  
    }
  }
  }
  }
  
  // Send command
  if (!Cmd.str().empty()) {
	DimClient::sendCommand("Bias/Command", ("hv "+Cmd.str()).c_str());
  }

  DiffVoltage = U;
  SetFBMode(FB_ResponseFirst, true);
  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<fNumberOfChips; j++) {
      m->PrintMessage("Board %d, chip %d:",i,j);
      for (int k=0; k<fNumberOfChannels; k++) m->PrintMessage(" %.3f",Response[i][j][k]);
      m->PrintMessage("\n\r");
    }
  }
}

//
// 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\n",
    fLedTrigBoard, fLedTrigChip, fLedTrigChannel, fLedTrigSample,
    fLedTrigThreshold, fLedSignalSample, fLedBaselineSample,
    fDefaultNumAverage, fIntHalfWidth);
}