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

  Bias feedback

  Oliver Grimm, July 2010
 
\**************************************************************/


#include "Feedback.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"
};

static const struct CL_Struct { const char *Name;    
								void (Feedback::*CommandPointer)();
								const char *Parameters;
								const char *Help;
  } CommandList[] = 
  {{"mode", &Feedback::cmd_mode, "[off|active|target]", "Set or get feedback mode"},
   {"average", &Feedback::cmd_average, "[n]", "Set ot get number of averages for feedback"},
   {"gain", &Feedback::cmd_gain, "[gain]", "Set ot get feedback gain"},
   {"target", &Feedback::cmd_target, "[pixel id]", "Set or get target value"},
   {"response", &Feedback::cmd_response, "[voltage]", "Start or get response measurement"},
   {"clear", &Feedback::cmd_clear, "", "Clear feedback signals"},
   {"data", &Feedback::cmd_data, "", "New feedback signals"},
   {"help", &Feedback::cmd_help, "", "Print help"},
   {"exit", &Feedback::cmd_exit, "", "Exit program"}};

using namespace std;


// ----- Constructor: Initialise feedback 
Feedback::Feedback(): EvidenceServer(SERVER_NAME) {

  PixMap = new PixelMap(PIXMAP_LOCATION, false);
  TimeBarrier = 0;

  // DIM console service used in PrintMessage()
  ConsoleText = NULL;
  ConsoleOut = new DimService(SERVER_NAME"/ConsoleOut", (char *) "");

  // Get pixel ID table
  fIDTable = Tokenize(GetConfig("IDTable"), " \t");
  Multiplicity = new unsigned int [fIDTable.size()];
  
  multiset<string> A;
  char *Buf;
  
  for (unsigned int i=0; i<fIDTable.size(); i++) {
 	if (asprintf(&Buf, "BiasID%d%d%d", PixMap->Pixel_to_HVboard(fIDTable[i]),
	  	PixMap->Pixel_to_HVchain(fIDTable[i]), PixMap->Pixel_to_HVchannel(fIDTable[i])) == -1) Message(FATAL, "asprintf() failed");
	A.insert(Buf);
	free(Buf);
  }
  
  for (unsigned int i=0; i<fIDTable.size(); i++) {
 	if (asprintf(&Buf, "BiasID%d%d%d", PixMap->Pixel_to_HVboard(fIDTable[i]),
	  	PixMap->Pixel_to_HVchain(fIDTable[i]), PixMap->Pixel_to_HVchannel(fIDTable[i])) == -1) Message(FATAL, "asprintf() failed");
	Multiplicity[i] = A.count(Buf);
	free(Buf);
  }

  // Initialise with zero content ???
  Average    = new float [fIDTable.size()];
  Sigma      = new float [fIDTable.size()];
  Response   = new float [fIDTable.size()];
  Target     = new float [fIDTable.size()];
  Buffer     = new float [fIDTable.size()];  

  DIMAverage = new float [fIDTable.size()];
  DIMSigma   = new float [fIDTable.size()];

  // Get remaing configuration data
  fDefaultNumAverage = atoi(GetConfig("DefaultNumAverage").c_str());
  
  vector<string> Token = Tokenize(GetConfig("DefaultResponse"), " \t");
  for (unsigned int i=0; i< Token.size(); i++) {
	if (i < fIDTable.size()) Response[i] = (float) atof(Token[i].c_str());
  }

  Token = Tokenize(GetConfig("DefaultTarget"), " \t");
  for (unsigned int i=0; i<Token.size(); i++) {
	if (i < fIDTable.size()) Target[i] = (float) atof(Token[i].c_str());
  }
  
  // Provide DIM services
  FeedbackAverage = new DimService (SERVER_NAME"/Average", "F", DIMAverage, fIDTable.size() * sizeof(float));
  FeedbackSigma = new DimService (SERVER_NAME"/Sigma", "F", DIMSigma, fIDTable.size() * sizeof(float));
  FeedbackResponse = new DimService (SERVER_NAME"/Response", "F", Response, fIDTable.size() * sizeof(float));
  FeedbackTarget = new DimService (SERVER_NAME"/Target", "F", Target, fIDTable.size() * sizeof(float));
  CountService = new DimService (SERVER_NAME"/Count", Count);
  FeedbackState = new DimService (SERVER_NAME"/State", "I:1;C", NULL, 0);

  // Initial state
  Gain = atof(GetConfig("DefaultGain").c_str());
  SetFBMode(Off);
  NumAverages = fDefaultNumAverage;
  LastServiceUpdate = 0;
  
  // Install DIM command (after all initialized)
  Command = new DimCommand((char *) SERVER_NAME"/Command", (char *) "C", this);
}


// ----- Destructor
Feedback::~Feedback() {

  delete Command;
  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[] Multiplicity;
  delete PixMap;
  
  delete ConsoleOut;
  free(ConsoleText);
}


// ----- Set/get mode of feedback
void Feedback::cmd_mode() {

  if (Match(Parameter[1], "off")) SetFBMode(Off);
  else if (Match(Parameter[1], "active")) SetFBMode(Active);
  else if (Match(Parameter[1], "targets")) SetFBMode(Targets);
  else PrintMessage("%s.\n", FBState_Description[FBMode]);
}


// ----- Set/get current number of events
void Feedback::cmd_average() {

  if (Parameter.size() == 1) {
	PrintMessage("Current feedback events: %u   (acting when %u events reached)\n", Count, NumAverages);
  }
  else if (atoi(Parameter[1].c_str())>=0) NumAverages = atoi(Parameter[1].c_str());
  else PrintUsage();
}


// ----- Set/get feedback gain
void Feedback::cmd_gain() {

  if (Parameter.size() == 2) Gain = atof(Parameter[1].c_str());
  PrintMessage("Feedback gain is %.2f\n", Gain);
}


// ----- Set/get target value
void Feedback::cmd_target() {

  if (Parameter.size() == 1) {
	for (unsigned int i=0; i<fIDTable.size(); i++) {
      PrintMessage("%s: %.2f  ", fIDTable[i].c_str(), Target[i]);
      if (i%5 == 4) PrintMessage("\n\r");
	}
	PrintMessage("\n");
	return;
  }

  if (Parameter.size() != 3) {
	PrintUsage();
	return;
  }

  if (Match(Parameter[1], "all")) {
	for (unsigned int i=0; i<fIDTable.size(); i++) {
	  Target[i] = atof(Parameter[2].c_str());
	}
	FeedbackTarget->updateService();
	return;
  }
  
  for (unsigned int i=0; i<fIDTable.size(); i++) {
	if (Match(Parameter[1], fIDTable[i])) {
	  Target[i] = atof(Parameter[2].c_str());
	  FeedbackTarget->updateService();
	  return;
	}
  }
  
  PrintMessage("Invalid board, chip or channel number.\n");
}


// ----- Start response measurement
void Feedback::cmd_response() {

  if (Parameter.size() == 1) {
    for (unsigned int i=0; i<fIDTable.size(); i++) {
      PrintMessage("%s: %.2f  ", fIDTable[i].c_str(), Response[i]);
      if (i%5 == 4) PrintMessage("\n\r");
	}
	PrintMessage("\n");
  }
  else if (atof(Parameter[1].c_str()) != 0) MeasureResponse(atof(Parameter[1].c_str()));
  else PrintUsage();
}


// ----- Clear accumulated averages
void Feedback::cmd_clear() {

  ClearAverages();
}


// ----- Accumulate feedback data and calculate voltage change if required number of events reached.
void Feedback::cmd_data() {

  float Correction;

  // Refect data is feedback off or timestamp too early
  if (FBMode == Off || getCommand()->getTimestamp() < TimeBarrier) return;
  TimeBarrier = 0;

  // Calculate average signal  
  for (unsigned int i=0; i<Parameter.size()-1, i<fIDTable.size(); i++) {
	Average[i] += atof(Parameter[i+1].c_str());
	Sigma[i] += pow(atof(Parameter[i+1].c_str()), 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;

  // Feedback action
  std::stringstream Cmd;

  for (unsigned int i=0; i<fIDTable.size(); i++) {
    // Calculate average
	Average[i] /= Count;
	Sigma[i] = sqrt(Sigma[i]/Count - pow(Average[i],2))/sqrt(Count);
	DIMAverage[i] = Average[i];
	DIMSigma[i] = Sigma[i];
	
	switch (FBMode) {
	  case Active:
		// Determine correction from response maxtrix and change voltages
	    Correction = -(Target[i] - Average[i])*Response[i]*Gain;
    	// Limit voltage steps
		if (fabs(Correction) > 0.1) Correction = fabs(Correction)/Correction*0.1;   // Limit changes to 100 mV
		if (Correction==0 || Target[i]==0) break;
		// Add voltage change command if not too noisy
		if(fabs(Average[i]) > 2*Sigma[i]) {
		  Cmd << fIDTable[i] << " " << std::showpos << Correction/Multiplicity[i] << " ";
		}
	    break;

	  case Targets:  // Take average as new targets  
	    Target[i] = Average[i];
	    break;

	  case ResponseFirst:  // First point of response measurement done  
	    Buffer[i] = Average[i];
		Cmd << fIDTable[i] << " " << std::showpos << DiffVoltage/Multiplicity[i] << " ";		  
	    break;

	  case ResponseSecond: // Determine response from signal variation
	    if (Buffer[i] == Average[i]) {
	      PrintMessage("Warning, response singular for pixel %s\n", fIDTable[i].c_str());
	      Response[i] = 0;
	    }
	    else Response[i] = DiffVoltage/(Buffer[i] - Average[i]);

		Cmd << fIDTable[i] << " " << std::showpos << -DiffVoltage/2/Multiplicity[i] << " ";		  		  
	    break;

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

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

  // Send command (non-blocking since in handler thread)
  if (!Cmd.str().empty()) {
	DimClient::sendCommandNB("Bias/Command", (char *) ("hv "+Cmd.str()).c_str());
  }

  switch (FBMode) {
    case Targets:
	  FeedbackTarget->updateService();
      PrintMessage("New targets set, switching off\n");
      SetFBMode(Off);
      break;
    case ResponseFirst:
      SetFBMode(ResponseSecond);
      PrintMessage("Increasing voltages by %f for response measurement, acquiring data\n", DiffVoltage);
      break;
    case ResponseSecond:
	  FeedbackResponse->updateService();
      PrintMessage("Response measurements finished, original voltages set, switching off\n");
      SetFBMode(Off);
      break;
    default: break;  // to suppress warning abount not handled enumeration value
  }
  ClearAverages();
  
  return;
}


// ----- Print help
void Feedback::cmd_help() {

  char Buffer[BUF_LENGTH];

  for(unsigned int i=0; i<sizeof(CommandList)/sizeof(CL_Struct); i++) {
    snprintf(Buffer, sizeof(Buffer), "%s %s", CommandList[i].Name, CommandList[i].Parameters);
    PrintMessage("%-28s%s\n", Buffer, CommandList[i].Help);
  }     
}


// ----- Exit programm
void Feedback::cmd_exit() {

  ExitRequest = true;
}


// ----- Clear average values and event counter
void Feedback::ClearAverages() {

  for (unsigned int i=0; i<fIDTable.size(); i++) {
	Average[i] = 0.0;
	Sigma[i] = 0.0;
  }
  Count = 0;
  CountService->updateService();
}


// ----- Set feedback mode and clear averages
void Feedback::SetFBMode(FBState Mode) {

  FBMode = Mode;
  if (Mode != ResponseFirst) PrintMessage("%s\n", FBState_Description[FBMode]);
  else 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));

  // Reject feedback signals received earlier than this time
  TimeBarrier = time(NULL) + 2;
}


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

  std::stringstream Cmd;

  if (U == 0) {
    PrintMessage("Error, voltage difference must be non-zero.\n");
    return;
  }

  // Build command
  for (unsigned int i=0; i<fIDTable.size(); i++) { 
	Cmd << fIDTable[i] << " " << std::showpos << -U/2/Multiplicity[i] << " ";		  		  
  }
  
  // Send command
  if (!Cmd.str().empty()) {
	DimClient::sendCommand("Bias/Command", ("hv "+Cmd.str()).c_str());
  }

  DiffVoltage = U;
  SetFBMode(ResponseFirst);
  PrintMessage("HV Feedback: Decreasing voltages by %f for response measurement, acquiring data.\n",DiffVoltage/2);
}


// ----- Print usage text for command
void Feedback::PrintUsage() {

  for (unsigned int i=0; i<sizeof(CommandList)/sizeof(CL_Struct); i++) {
    if (Match(Parameter[0], CommandList[Count].Name)) {
	  PrintMessage("Usage: %s %s\n", Parameter[0].c_str(), CommandList[Count].Parameters);
	}
  }
}


// ----- Print message to console only
void Feedback::PrintMessage(const char *Format, ...) {

  static char Error[] = "vasprintf() failed in PrintMessage()";
  char *Text;

  // Evaluate arguments
  va_list ArgumentPointer;
  va_start(ArgumentPointer, Format);
  if (vasprintf(&Text, Format, ArgumentPointer) == -1) Text = Error;
  va_end(ArgumentPointer);
  
  // Print to console
  if(strlen(Text)>0 && Text[strlen(Text)-1]=='\n') printf("\r%sCmd> ", Text); // New prompt
  else printf("%s", Text);
  fflush(stdout);

  // Send to DIM text service
  ConsoleOut->updateService(Text); 

  // Free old text
  if (ConsoleText != Error) free(ConsoleText);
  ConsoleText = Text; 
}


// ----- DIM command handler
void Feedback::commandHandler() {

  string Command = ToString("C", getCommand()->getData(), getCommand()->getSize());
  
  // Parse command into tokens
  Parameter.clear();
  Parameter = Tokenize(Command, " \t");
  if (Parameter.empty()) return;

  // Search for command
  for (unsigned int Count=0; Count<sizeof(CommandList)/sizeof(CL_Struct); Count++) {
    if (Match(Parameter[0], CommandList[Count].Name)) {
	  (this->*CommandList[Count].CommandPointer)();
	  return;
	}
  }
  
  // Command not found
  PrintMessage("Unknown command '%s'\n", Command.c_str());
}


// -----  Check if two strings match (min 1 character must match)
bool Feedback::Match(string str, string cmd) {
  return strncasecmp(str.c_str(),cmd.c_str(),strlen(str.c_str())==0 ? 1:strlen(str.c_str())) ? false:true;
}


// ================
//   Main program
// ================

int main() {

  char *Command;

  system("clear");
  printf("\n*** Bias feedback (built %s, %s, revision %s) *** \n\n",__DATE__, __TIME__, REVISION);

  // Readline library uses getc() (allows interruption by signal)
  rl_getc_function = getc;

  // Construct main instance (static ensures destructor is called with exit())
  static Feedback M;

  // Command loop
  while (!M.ExitRequest) {
    // Read Command
    Command = readline("\rCmd> ");
    if (Command == NULL) continue;
    if(strlen(Command)>0) add_history(Command);

    // Process command
	DimClient::sendCommand(SERVER_NAME"/Command", Command);
    free(Command);
  }
}