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

  Interface to FACT bias voltage crate
                
\********************************************************************/
#include <utility>

#include "Crate.h"
#include "User.h" // Must not be in header file to avoid problem with declaring class User

using namespace std;

//
// Constructor
//
Crate::Crate(string CrateName, int Number, class User *PIO) {
   
  struct termios tio;

  // Initialize
  InitOK = false;
  Disabled = false;
  File = NULL;
  m = PIO;
  Name = new char [CrateName.size()+1];
  strcpy(Name, CrateName.c_str());
  WrapCount = -1;
  LastReset = 0;

  for (unsigned int i=0; i<MAX_NUM_BOARDS*NUM_CHANNELS; i++) {
	OC[i] = false;
	Present[i] = false;
	Current[i] = 0;
  }
  ResetHit = false;
  WrapOK = true;
  WrapCount = -1;
  ErrorCount = 0;

  // Create DIM services
  stringstream ID;
  ID << setfill('0') << setw(2) << Number;

  NameService = new DimService ((SERVER_NAME"/NAME/ID"+ID.str()).c_str(), Name);
  BiasVolt = new DimService ((char *) (SERVER_NAME"/VOLT/ID"+ID.str()).c_str(), (char *) "D", Volt, MAX_NUM_BOARDS*NUM_CHANNELS*sizeof(double));
  BiasCurrent = new DimService ((char *) (SERVER_NAME"/MICROAMP/ID"+ID.str()).c_str(), (char *) "F", Current, MAX_NUM_BOARDS*NUM_CHANNELS*sizeof(float));

  ClearVoltageArrays();

  // Open device
  if ((fDescriptor = open(("/dev/"+CrateName).c_str(), O_RDWR|O_NOCTTY|O_NDELAY)) == -1) {
    if (errno != 2) m->PrintMessage("Error: Could not open device %s (%s)\n", Name, strerror(errno));
    return;
  }
  
  // Generate FILE pointer
  if ((File = fdopen(fDescriptor, "rb+")) == NULL) {
    m->PrintMessage("Error: fdopen() failed on device %s (%s)\n", Name, strerror(errno));
    return;
  }

  // Get current serial port settings
  if (tcgetattr(fDescriptor, &tio) == -1) {
    m->PrintMessage("Error: tcgetattr() failed on device %s (%s)\n", Name, strerror(errno));
    return;   
  }

  // Set baudrate and raw mode
  if (cfsetspeed(&tio, B115200) == -1) {
	m->PrintMessage("Error: Could not set baud rate of device %s (%s)\n", Name,  strerror(errno));
	return;
  }
  cfmakeraw(&tio);
  if (tcsetattr(fDescriptor, TCSANOW, &tio ) == -1) {
	m->PrintMessage("Error: tcsetattr() failed on device %s (%s)\n", Name, strerror(errno));
	return;
  }
  
  // Synchronize crate
  if (!Synch()) return;

  // Set voltages and zero and reset crate
  if (!GlobalSet(0) || !SystemReset()) return;

  InitOK = true;
}

//
// Destructor (Resets board)
//
Crate::~Crate() {

  if(fDescriptor != -1) {
    if (!GlobalSet(0)) m->Message(m->ERROR, "Could not global set crate %s to zero voltage", Name);

    if (File == NULL) {
	  if (close(fDescriptor) == -1) m->PrintMessage("Error closing device %s (%s)\n", Name, strerror(errno));
	}
    else if (fclose(File) != 0) m->PrintMessage("Error closing device %s\n", Name);
  }

  delete NameService;
  delete BiasVolt;
  delete BiasCurrent;
  delete[] Name;
}


//
// Communicate with crate (return empty vector in case of time-out or not a multiple of 3 bytes was returned) 
//
vector<unsigned char> Crate::Communicate(string Buf) {

  int N;
  fd_set SelectDescriptor;
  float TimeOut = atof(m->GetConfig("TimeOut").c_str());
  struct timeval WaitTime = {(long) TimeOut, (long) ((TimeOut-(long) TimeOut)*1e6)};
  char Buffer[10000];
  vector<unsigned char> Data;

  // Crate disabled because of too many errors?
  if (Disabled) return Data;
  
  // === Lock device ===
  flockfile(File);

  // === Write data ===
  if ((N = write(fDescriptor, Buf.data(), Buf.size())) < (int) Buf.size()) {
    if (N == -1) m->Message(m->ERROR, "Could not write data to crate %s (%s)", Name, strerror(errno));
    else m->Message(m->ERROR, "Could write only %d of %d bytes to crate %s", N, Buf.size(), Name);
    ErrorCount++;
	goto ExitCommunicate;
  }

  // === Try to read back data with time-out ===
  do {
	FD_ZERO(&SelectDescriptor);   FD_SET(fDescriptor, &SelectDescriptor);
	if (select(fDescriptor+1, &SelectDescriptor, NULL, NULL, &WaitTime)==-1) {
	  if (errno == EINTR) goto ExitCommunicate;  // in case program is exiting
      m->Message(m->FATAL, "Error with select() (%s)", strerror(errno));
	}

	// Time-out expired?
	if (!FD_ISSET(fDescriptor, &SelectDescriptor)) {
	  goto ExitCommunicate;
	}

	// Read data   
	if ((N = read(fDescriptor, Buffer, sizeof(Buffer))) == -1) {
      m->Message(m->ERROR, "Read error from crate %s (%s)", Name, strerror(errno));
      ErrorCount++;
	  goto ExitCommunicate;
	}

	// Add data to buffer
	for (int i=0; i<N; i++) Data.push_back(Buffer[i]);
  } while(Data.size() < Buf.size());
  
  // === Check if multiple of three bytes were returned ===
  if (Data.size() % 3 != 0) {
    Data.clear();
	goto ExitCommunicate;
  }

  // === Check/update wrap counter in all received data packages of three bytes ===  
  for (unsigned int i=0; i<Data.size(); i+=3) {
	if (WrapCount != -1) {
	  if ((WrapCount+1)%8 == ((Data[i]>>4) & 7)) WrapOK = true;
	  else WrapOK = false;
	}
	WrapCount = (Data[i]>>4) & 7;
  }

 // Check wrap counter
 if (!WrapOK) {
  	m->Message(m->WARN, "Wrap counter mismatch of crate %s", Name);
	ErrorCount++;
 }
 
  // === UnLock file descriptor ===
  ExitCommunicate:
  funlockfile(File);

  if (ErrorCount > MAX_ERR_COUNT) {
  	m->Message(m->ERROR, "Crate %s has more than %d errors, disabled", Name, MAX_ERR_COUNT);   
	Disabled = true;
  }

  return Data;
}

//
// System reset of bias crate
//
bool Crate::SystemReset() {

  // Check if minimum requested period elapsed since last reset  
  if (time(NULL) - LastReset < max(atoi(m->GetConfig("MinResetPeriod").c_str()),1)) return false;
  LastReset = time(NULL);

  // Send reset and check if 3 bytes returned  
  if (Communicate(string(3, 0)).size() != 3) return false;

  ResetHit = false;
  Disabled = false;
  ErrorCount = 0;
  return true;
}

//
// Read all channels status
//
bool Crate::ReadAll() {

  string Buf;

  // Prepare command to read all channels
  for (unsigned int i=0; i<MAX_NUM_BOARDS; i++) for (int unsigned j=0; j<NUM_CHANNELS; j++) {
	Buf.push_back(1<<5 | i<<1 | (j&16)>>4);
	Buf.push_back(j<<4);
	Buf.push_back(0);
  }
  
  // Execute command
  vector<unsigned char> Data = Communicate(Buf);
    
  if (Data.size() != Buf.size()) return false;

  // Evaluate data returned from crate (1 count for current -> 1.22 uA)
  int Count = 0;
  for (unsigned int i=0; i<MAX_NUM_BOARDS*NUM_CHANNELS; i++) {
	Current[i] = (Data[Count+1] + (Data[Count] & 0x0f)*256) * 1.22;
	OC[i] = Data[Count] & 128;
	Present[i] = (Data[Count+2] & 0x70) == 0 ? true : false;
	ResetHit |= (Data[Count+2] & 0x80) == 0 ? false : true;
	Count += 3;
  }
  return true;
}


// ***** Global set *****
bool Crate::GlobalSet(double Voltage) {

  // Limit voltage
  Voltage = min(Voltage, atof(m->GetConfig("VoltageLimit").c_str()));
  Voltage = max(Voltage, 0.0);

  // Calculate DAC value
  unsigned int SetPoint = (unsigned int) (Voltage/90.0*0x0fff);

  // Execute command
  string Buf;
  Buf = (((Buf + char(1<<6)) + char(SetPoint>>8)) + char(SetPoint));
  vector<unsigned char> Data = Communicate(Buf);

  if (Data.size() != 3) return false;
  
  for (unsigned int i=0; i<MAX_NUM_BOARDS*NUM_CHANNELS; i++) {
	Volt[i] = Voltage;
	RefVolt[i] = Voltage;
  }
  return true;
}


// ***** Set channel voltages *****
bool Crate::SetChannels(map<unsigned int, double> V) {
  
  string Buf;

  if (V.empty()) return true;

  // Build and execute commands
  for (map<unsigned int, double>::iterator it = V.begin(); it != V.end(); ++it) {
    // Limit voltage
	it->second = min(it->second, atof(m->GetConfig("VoltageLimit").c_str()));
	it->second = max(it->second, 0.0);

    // If DAC value unchanged, do not send command
	if (GetDAC(it->first) == it->second/90.0*0x0fff) continue;

	// Add command to buffer
	Buf += char(3<<5) |  char(it->first/NUM_CHANNELS<<1) | char((it->first%NUM_CHANNELS&16)>>4 & 1);
	Buf += char(it->first%NUM_CHANNELS<<4) | ((((unsigned int) (it->second/90.0*0x0fff))>>8) & 0x0f);
	Buf += char(it->second/90.0*0x0fff);
  }
  vector<unsigned char> Data = Communicate(Buf);

  // Store new voltage values of successful
  if (Data.size() != Buf.size()) return false;
  
  for (map<unsigned int, double>::const_iterator it = V.begin(); it != V.end(); ++it) {
	Volt[it->first] = it->second;
	RefVolt[it->first] = it->second;
  }

  return true;
}


// ***** Synchronize board *****
bool Crate::Synch() {
  
  int Trial = 0;
  vector<unsigned char> Data;

  while(++Trial <= 3) {
    Data = Communicate(string(1, 0));
    if (Data.size() == 3) return true;
  }
  return false;
}


// ***** Set all voltages of board to zero *****
void Crate::ClearVoltageArrays() {

  for (unsigned int i=0; i<MAX_NUM_BOARDS*NUM_CHANNELS; i++) {
	Volt[i] = 0;  
	RefVolt[i] = 0;  
  }

  UpdateDIM();
}


// ***** Return calibrated voltage of given channel *****
double Crate::GetVoltage(unsigned int Channel) {

  if (Channel >= MAX_NUM_BOARDS*NUM_CHANNELS) return 0;
  else return Volt[Channel]; 
}


// ***** Return DAC value of given channel *****
unsigned int Crate::GetDAC(unsigned int Channel) {

  if (Channel >= MAX_NUM_BOARDS*NUM_CHANNELS) return 0;
  else return (unsigned int) (Volt[Channel]/90.0*0x0fff);
}


// ***** Return current of given channel *****
float Crate::GetCurrent(unsigned int Channel) {

  if (Channel >= MAX_NUM_BOARDS*NUM_CHANNELS) return 0;
  else return Current[Channel]; 
}


// ***** Update DIM services *****
void Crate::UpdateDIM() {

  BiasVolt->updateService();
}


// ***** Set reference current for dynamic mode *****
void Crate::SetRefCurrent() {

  for (unsigned int i=0; i<MAX_NUM_BOARDS*NUM_CHANNELS; i++) RefCurrent[i] = Current[i];  
}


// ***** Correct voltages according to current *****
void Crate::AdaptVoltages() {

  static int LastUpdate = 0;
   
  map<unsigned int, double> Voltages;

  for (unsigned int i=0; i<MAX_NUM_BOARDS*NUM_CHANNELS; i++) {
    if (RefVolt[i] == 0) continue;
	Voltages[i] = RefVolt[i] + (RefCurrent[i]-Current[i])*RESISTOR/1e6;  
  }  
  SetChannels(Voltages);
  
  if (time(NULL)-LastUpdate > 5) {
    LastUpdate = time(NULL);
	UpdateDIM();
  }
}