#include <iostream>
#include <iomanip>
#include <string>
#include <sstream>
#include <fstream>
#include <cstdio>
#include <cstdlib>
#include <assert.h>
#include <fcntl.h>
#include <unistd.h>
#include "gluex_Canlib.h"
#include <cstring>
#include <math.h>
#include <vector>
#include <map>
#include <iterator>
#include <stdio.h>
#include <time.h>       /* time_t, struct tm, difftime, time, mktime */
#include <algorithm>

using namespace std;

Device *dev[30];
int portNum;
int anaNum;
int nDevices;

void ADC_menu( int id, int nReplies );
void TP_menu(  int id );
void BL_menu(Device* dev[],  int id, int nReplies, bool fcal_anagate );
void HV_menu( Device &dev , int id, int nReplies );
void Power_menu(Device *dev[], int nDevices);
void CheckForMissingIDs(vector<int> all_ids, vector<int> current_ids, string ADC_type);

void uploadModules();
void sortStrands();
void thresholdHV();
void testUpload();
double mean( float x[], int size);
double stddev( float x[], int size);
void orderedTest();
void HVStripes();

string anaIPs[8];
string anaPorts[4];

vector<int> all_ids;

bool exit_monitoring;

int HV_setpoint;

/* int bases[5000], basesTemp[5000];
int CAN_ID[5000], CAN_IDTemp[5000];
int xCoord[5000], xCoordTemp[5000];
int yCoord[5000], yCoordTemp[5000];
int strands[5000];
int PMT_ID[5000], PMT_IDTemp[5000];
float PMT_fit_A[5000], PMT_fit_ATemp[5000];
float PMT_fit_B[5000], PMT_fit_BTemp[5000];
float HV[5000];
 */
 
 

 
//----------------------------------------------------------

int main( int argc, char **argv ){

  int cmd = 0;
  bool Exit_Prog = false;
  int nReplies = 0;
  string argument="";
  string my_port="";
  int ext_id=0;
  HV_setpoint=0;
  bool fcal_anagate=false;
  if(argc==2||argc==3) argument = argv[1];
  if(argc==3) my_port = argv[2];
  
  
	anaIPs[0]="129.57.26.115"; //1L
	anaIPs[1]="129.57.26.107"; //1R
	anaIPs[2]="129.57.26.116"; //2L
	anaIPs[3]="129.57.26.109"; //2R
	anaIPs[4]="129.57.26.112"; //3L
	anaIPs[5]="129.57.26.108"; //3R
	anaIPs[6]="129.57.26.114"; //4L

	anaPorts[0]="A";
	anaPorts[1]="B";
	anaPorts[2]="C";
	anaPorts[3]="D";

  
  if(argc==2&&argument=="help") {
	 cout << "To run on a single FCAL anagate (e.g. 1L) and all strands use:" << endl;
	 cout << "> baseControl 1L" << endl << endl;
	 cout << "To run on a single FCAL strand (e.g. 1L port A) use:" << endl;
	 cout << "> baseControl 1L A" << endl << endl;
	 cout << "To run on jlab small test setup use:" << endl;
	 cout << "> baseControl F117" << endl << endl;
	 cout << "To run on larger F-Wing Electronics Room setup use either:" << endl;
	 cout << "> baseControl jlabL [A-D]" << endl << endl;
	 cout << "or" << endl << endl;
	 cout << "> baseControl jlabR [A-D]" << endl << endl;
	 cout << "To run on IU test setup use:" << endl;
	 cout << "> baseControl IU" << endl << endl;

	 cout << "All arguments are case sensitive" << endl;
	 return 0;
  }
  
  else if(argc==3&&argument=="jlabL") {
    nDevices=1;
	  cout << "Enter the number of bases currently connected:" << endl;
	  cin >> nReplies;
    dev[0] = OpenCANBus(const_cast<char*>(my_port.c_str()),"129.57.194.19"); 
	anaIPs[0]="129.57.194.19";
	anaPorts[0]=my_port;
    cout <<"Opening first device with " << nReplies << " bases on bus" << endl;
  }
  
  else if(argc==3&&argument=="jlabR") {
    nDevices=1;
	  cout << "Enter the number of bases currently connected:" << endl;
	  cin >> nReplies;
    dev[0] = OpenCANBus(const_cast<char*>(my_port.c_str()),"129.57.194.22"); 
	anaIPs[0]="129.57.194.22";
	anaPorts[0]=my_port;
    cout <<"Opening first device with " << nReplies << " bases on bus" << endl;
  }

  else if(argc==2&&argument=="F117") {
	  nDevices=1;
	  cout << "Assuming bases are connected to port A..." << endl;
	  cout << "Enter the number of bases currently connected:" << endl;
	  cin >> nReplies;
	  dev[0] = OpenCANBus("A","192.168.1.254");
	anaIPs[0]="192.168.1.254";
	anaPorts[0]="A";
  }
  
  else if(argc==2&&argument=="IU") {
	  nDevices=1;
	  cout << "Assuming bases are connected to port A..." << endl;
	  cout << "Enter the number of bases currently connected:" << endl;
	  cin >> nReplies;
	  dev[0] = OpenCANBus("A","10.79.158.71");
	anaIPs[0]="10.79.158.71";
	anaPorts[0]="A";

  }

  
  else if(argc==2&&argument.size()==2&& (argument[1]=='L' || argument[1]=='R') ) {
	  nDevices=4;
	  nReplies = 100;
	  fcal_anagate=true;
	  string anaIP_single;
	  if(argument=="1L") anaIP_single=anaIPs[0];
	  if(argument=="1R") anaIP_single=anaIPs[1];
	  if(argument=="2L") anaIP_single=anaIPs[2];
	  if(argument=="2R") anaIP_single=anaIPs[3];
	  if(argument=="3L") anaIP_single=anaIPs[4];
	  if(argument=="3R") anaIP_single=anaIPs[5];
	  if(argument=="4L") anaIP_single=anaIPs[6];
	  
	  dev[0] = OpenCANBus("A",const_cast<char*>(anaIP_single.c_str()));
	  dev[1] = OpenCANBus("B",const_cast<char*>(anaIP_single.c_str()));
	  dev[2] = OpenCANBus("C",const_cast<char*>(anaIP_single.c_str()));
	  dev[3] = OpenCANBus("D",const_cast<char*>(anaIP_single.c_str()));
  }

  else if(argc==3&&argument.size()==2&& (argument[1]=='L' || argument[1]=='R') ) {
	  nDevices=1;
	  nReplies = 100;
	  cout << argv[2] <<endl;
	  string anaIP_single;
          fcal_anagate=true;
	  if(argument=="1L") anaIP_single=anaIPs[0];
	  if(argument=="1R") anaIP_single=anaIPs[1];
	  if(argument=="2L") anaIP_single=anaIPs[2];
	  if(argument=="2R") anaIP_single=anaIPs[3];
	  if(argument=="3L") anaIP_single=anaIPs[4];
	  if(argument=="3R") anaIP_single=anaIPs[5];
	  if(argument=="4L") anaIP_single=anaIPs[6];
	  
	  if(my_port=="A") {dev[0] = OpenCANBus("A",const_cast<char*>(anaIP_single.c_str())); cout << "Connecting to Anagate IP " << anaIP_single << endl;}
          if(my_port=="B") {dev[0] = OpenCANBus("B",const_cast<char*>(anaIP_single.c_str())); cout << "Connecting to Anagate IP " << anaIP_single << endl;}
	  if(my_port=="C") {dev[0] = OpenCANBus("C",const_cast<char*>(anaIP_single.c_str())); cout << "Connecting to Anagate IP " << anaIP_single << endl;}
	  if(my_port=="D") {dev[0] = OpenCANBus("D",const_cast<char*>(anaIP_single.c_str())); cout << "Connecting to Anagate IP " << anaIP_single << endl;}

	  }

  
  else if(argc==1) {
	  nDevices=28;
	  nReplies = 100;
          fcal_anagate=true;
	  for(int i=0; i<7; ++i) {
		for(int j=0; j<4; ++j) {
            dev[i*4+j] = OpenCANBus((char*)anaPorts[j].c_str(),(char*)anaIPs[i].c_str());
            cout << "Opening anagate at IP: " << anaIPs[i] << " port " << anaPorts[j] << endl;
		}
	  cout << "No options passed on startup --- addressing full FCAL!!!!" << endl;
	  cout << "To see options for addressing a single anagate, single strand, or test stand use:" << endl;
	  cout << "> baseControl help" << endl << endl;

	}
  }

  else {
	cout << "Invalid initialization, exiting...." << endl;
	return 0;
  }
  
  
  //If using any setup besides FCAL, then get all IDs for later use  
	  if(argument=="IU"||argument=="jlabL"||argument=="F117"||argument=="jlabR") {
		  all_ids = GetMyIDs( *dev[0], ext_id, nReplies,false );
		  map<int,int> HV_status_map = HVControl( *dev[0], ext_id, 0, 0, true, nReplies,true );
		  
		  if(HV_status_map.size()!=all_ids.size()) {
			  cout << "I AM ERROR: PROBLEMS WITH NREPLIES FOR INITIAL CHECKS " << endl;
			  return 0;
		  }
		  		  
//If base doesn't send back HV = all enable, then send HV set = 0 
//This avoids misinterpreting HV setpoint values
		  for( map<int,int>::iterator it = HV_status_map.begin(); it!=HV_status_map.end(); it++) {
			  if(it->second != 212 && it->first!=0) {
				  cout << "Setting HV = 0 base with HV enable OFF (avoids HV setpoint issue): " << it->first << endl;
				  SetVoltage(*dev[0], it->first, 0);
			  }
			  
		  }
		  
		  
	  }
  	  if(argument=="IU"||argument=="jlabL"||argument=="F117"||argument=="jlabR") {
		  if(nReplies != all_ids.size() ) {
			cout << "ERROR: Not all IDs could be determined on startup!!! Exiting... " << endl;
			return 0;
		  }  
	  }
  while(!Exit_Prog) {

    cout << "\t 1.  Set LED red"<<endl;
    cout << "\t 2.  Set LED green"<<endl;
    cout << "\t 3.  Set LED off"<<endl;
    cout << "\t 4.  HV On"<<endl;
    cout << "\t 5.  HV Set"<<endl;
    cout << "\t 6.  HV Off"<<endl;
    cout << "\t 7.  Power Controls"<<endl;
    cout << "\t 8.  Read an ADC"<<endl;
    cout << "\t 9.  Change number of expected replies per strand"<<endl;
    cout << "\t 10. Test pulser control"<<endl;
    cout << "\t 11. HV controls (Experts only!)" << endl;
    cout << "\t 12. Read Firmware Version" <<endl;
    cout << "\t 13. Bootloader Menu" << endl;
    cout << "\t 14. Get CAN IDs" << endl;
    cout << "\t 15. Address Single Base"<<endl;
	cout << "\t 16. Listen for base messages" << endl;
    cout << "To exit program enter 0"<<endl;
    cout << "> ";
    cin >> cmd;

	if(cmd<=-1 || cmd >=17) {
		cout << "Please enter a valid selection" << endl;
		continue;
	}
	
	switch(cmd) {
		
		case(0):
			Exit_Prog=true;
			break;
		
		case(1): //LED RED
			for(int i = 0; i < nDevices; ++i) {
				LEDSwitch(*dev[i], ext_id, LEDRED);
				usleep(50000);
			}
			break;
		
		case(2): //LED GREEN
			for(int i = 0; i < nDevices; ++i) {
				LEDSwitch(*dev[i], ext_id, LEDGREEN);
				usleep(50000);
			}
			break;

		case(3): //LED OFF
			for(int i = 0; i < nDevices; ++i) {
				LEDSwitch(*dev[i], ext_id, LEDOFF);
				usleep(50000);
			}
			break;

		case(4): //HV ON
			for(int i = 0; i < nDevices; ++i) {
				HVControl(*dev[i], ext_id, 0xFF, 0x00, false, nReplies,false);
				usleep(50000);
			}
			break;

		case(5): //HV SET
			double Voltage;
			cout<<"Enter a value from 0 - 2047.5 (in steps of 0.5):\n";
			cout<<"> ";
			cin>>Voltage;
			HV_setpoint=Voltage;
			for(int i = 0; i < nDevices; ++i) {
				SetVoltage(*dev[i], ext_id, Voltage);
				usleep(50000);
			}
			break;
			
		case(6): //HV OFF
			for(int i = 0; i < nDevices; ++i) {
				HVControl(*dev[i], ext_id, 0xF0, 0x00, false, nReplies,false);
				usleep(50000);
			}
			break;
			
			
		case(7): //Power Controls
			Power_menu(dev,nDevices);
			break;
			
		case(8): // Read an ADC
			ADC_menu(ext_id,nReplies);
			break;
			
		case(9): // Change nReplies
			cout << "Enter a new number of bases: \n >";
			cin >> nReplies;
                        break;
			
		case(10): //Test Pulser
			TP_menu(ext_id);
			break;
		
		case(11): //HV Controls 
			for(int i = 0; i < nDevices; ++i) {
				cout << "DON'T DO THIS FOR FULL FCAL!!!" << endl;
				HV_menu( *dev[i], ext_id, nReplies);
				usleep(50000);
			}
			break;

		case(12): //Read firmware
			for(int i = 0; i < nDevices; ++i) {
				ReadVersion( *dev[i], ext_id, nReplies );
			}
			break;
			
		case(13): //Bootloader menu
			BL_menu(dev, ext_id, nReplies, fcal_anagate );
			break;

                case(14): //Get Ids
			for(int i = 0; i < nDevices; ++i) {
                          all_ids = GetMyIDs( *dev[i], ext_id, nReplies,true );
                        }
                          break;
                          
        case(15):           
          cout << "Enter board address (decimal value) or 0 to communicate with all bases again" <<endl;
		  cout << "NOTE: if you are doing this while connected to more than one strand, you will get TIMEOUT errors on all other strands. Ignore these errors." << endl;
          cout <<">"<<endl;
		  cin >> ext_id;
          nReplies=1;
          if(ext_id==0) {
			  cout << "How many bases are connected (per strand)? "<<endl<<">";
			  cin >> nReplies;
          }
		  break;
		  
		case(16): //Read firmware
			for(int i = 0; i < nDevices; ++i) {
				ListenForMessages( *dev[i], ext_id, nReplies );
			}
			break;

		
	}
	  
  }





  return 0;
}


void Power_menu(Device *dev[], int nDevices) {
	
	cout << "\t Power Control Options: " << endl;
	cout << "\t Note: Will only work if anagate has digital I/O hooked up" << endl;
	cout << "\t\t 1. Reset all bases" << endl;
	cout << "\t\t 2. Power off all bases" << endl;
	cout << "\t\t 3. Power on all bases" << endl;
	
	bool exit_now=false;
	int cmd=0;
	
    cin >> cmd;
	
	if(cmd<=0 || cmd >= 4) {
		cout << "Invalid response." << endl;
		return;
	}

	switch(cmd){
	
		case(1):
			for(int i =0; i<nDevices; ++i) {
			    WriteDigital(*dev[i], 0x0f);
				usleep(100000);
				WriteDigital(*dev[i], 0x00);
				usleep(100000);
			}
			break;
			
		case(2):	
			for(int i =0; i<nDevices; ++i) {
			    WriteDigital(*dev[i], 0x0f);
				usleep(100000);
			}
			break;
			
		case(3):	
			for(int i =0; i<nDevices; ++i) {
			    WriteDigital(*dev[i], 0x00);
				usleep(100000);
			}
			break;
	}
	
	return;
}

void BL_menu( Device* dev[], int ext_id, int nReplies,bool fcal_anagate)
{
	std::vector<int> retVals;
  	int position;
	int statByte;
	int linecount = 0;
	string r_u_sure;
	cout<<"Bootloader Menu. \n";
	cout<<"\t 1. Read Firware/Bootloader Status Byte\n";
	cout<<"\t 2. Upload Firmware\n";
	cout<<"\t 3. Clear Firmware\n";
	cout<<"\t 4. Jump to Bootloader Mode\n";
	cout<<"\t 5. Jump to Firmware Mode \n";
	cout<<"\t 6. Light Up Non-Responding Bases\n";
	cout<<"> ";
	cin>>position;
	switch(position){

        case(1):
          for(int i=0; i<nDevices; ++i) {
            ReadStat( *dev[i], ext_id, nReplies, retVals );
          }
          cout <<endl << "1=BL mode, 2=FW mode" << endl << endl;
          break;

	case(2):
	  if(fcal_anagate) {
		  cout << "Use baseUpload_FCAL to do this" << endl;
		  break;
	  }
	  else BootloaderAct( *dev[0], ext_id, nReplies, linecount, retVals, true );
	  break;
	  
	case(3):
	  	if(fcal_anagate) {
		  cout << "Use baseUpload_FCAL executable to do this" << endl;
		  break;
	  }
	  else BootloaderClear( *dev[0], ext_id, nReplies );
	  break;
	  
	case(4):
		cout << "Switching to BL mode and power cycling..." << endl;
		for(int i=0; i<nDevices; ++i) {
			WriteStat( *dev[i], ext_id, nReplies, 1);
			usleep(100000);
			WriteDigital(*dev[i], 0x0f);
			usleep(100000);
			WriteDigital(*dev[i], 0x00);
			usleep(100000);
		}
	  break;
	  
	case(5):
		cout << "WARNING!!! IF FIRMWARE ON BASE IS CORRUPTED OR MISSING THIS WILL BRICK THE BASE!!!!!!" << endl;
		cout << "Are you sure you want to switch to FW mode (y for yes, anything else for no)" << endl << ">";
		cin >> r_u_sure;
		if(r_u_sure=="y") {
			cout << "Switching to FW mode and power cycling..." << endl;
			for(int i=0; i<nDevices; ++i) {
				WriteStat( *dev[i], ext_id, nReplies, 2);
				usleep(100000);
				WriteDigital(*dev[i], 0x0f);
				usleep(100000);
				WriteDigital(*dev[i], 0x00);
				usleep(100000);
			}
		}
	  break;
	  
	case(6):
		for(int j=0; j<nDevices; ++j) {
		  LightBad(*dev[j], ext_id, nReplies, retVals);
		  LEDSwitch(*dev[j], 0x000000, LEDGREEN);
		  for(int i = 0; i < retVals.size(); i++) {
			cout<<i<<": "<<retVals[i]<<"\n";
			LEDSwitch(*dev[j], retVals[i], LEDOFF);
		  }
		  usleep(10000);
		}
 	  break;
	}
}



void ADC_menu( int ext_id, int nReplies )
{
  bool output = false;
	int position;
	int command = 0xFF;
	int value[2];
	float ADCvoltage;
	int ADCchannels;
	cout<<"ADC Menu.\n";
	cout<<"\t 1. Read Medium Voltage Bottom\n";
	cout<<"\t 2. Read Medium Voltage Top\n";
	cout<<"\t 3. Read 1st Dynode\n";
	cout<<"\t 4. Read Photocathode\n";
	cout<<"\t 5. Read DAC voltage\n";
	cout<<"\t 6. Read temperature\n";
	cout<<"\t 7. Read current\n";
	cout<<"\t 8. Read HV sent to base \n";
    cout<<"\t 9. Continuously Read Cathode and Temp And Output To File\n";
	cout<<"> ";
	cin>>position;
	if(position > 0 && position < 10){
	switch(position){
	case(1):
	  command = ADC_MVB;
	  break;
	case(2):
	  command = ADC_MVT;
	  break;
	case(3):
	  command = ADC_DYN;
	  break;
	case(4):
	  command = ADC_CAT;
	  break;
	case(5):
	  command = ADC_DAC;
	  break;
	case(6):
	  command = ADC_TEM;
	  break;
	case(7):
	  command = ADC_CUR;
	  break;
	case(8):
	  command = READ_HV;
	  break;	
	case(9):
    output = true;
	}
	
	  if (!output){
  	  for(int i = 0; i < nDevices; i++){	
  		  ADC( *dev[i], ext_id, command, nReplies );
  		  usleep(1000000);
		}
	  return;
	  }
	}
	
	  if (output){
	    
	    
      std::fstream* outFile = new fstream();  
      
	  string outFileStr;
	  cout << "Output file name: "; cin >> outFileStr; cout << endl;
	  
      cerr << "To exit press ctlr+z" << endl;
      while(1){
        
  	    for(int i = 0; i < nDevices; i++){ 
          //if (i==5&&j==0) continue;
          //if (i==6&&j==3) continue;
  	      time_t rawtime;

          time (&rawtime);
          //printf ( "The current date/time is: %s", ctime (&rawtime) );
  	      
		  
		  //Added 3/9/16: check that HV is enabled and all HV setpoints are above zero
		  //              exit if this is not the case
		  // 3/31/16: Also sets to HV setpoint. Setpoint defaults to zero though
		  //Removed again 4/19/16
		  /*
		  usleep(10000);
		  HVControl(*dev[0], ext_id, 0xFF, 0x00, false, nReplies,true);
		  usleep(10000);
		  exit_monitoring=false;
		  exit_monitoring = CheckSetpoints( *dev[0], ext_id, READ_HV, nReplies );

		  
		  for(int i = 0; i < nDevices; ++i) {
				SetVoltage(*dev[i], ext_id, 1600);
				usleep(50000);
			}
		  
		  if(exit_monitoring) {
			  cout << "HV setpoint = 0 detected. Exiting monitoring..." << endl;
			  return;
		  }
		  */

          outFile->open(outFileStr.c_str(), std::fstream::out | std::fstream::app);
          if (i==0) *outFile << "Time:\t" << ctime (&rawtime);
          
		  vector<int> current_ids;
		      usleep(1000000);
          cerr << "Anagate: " << anaIPs[int(floor(i/4.))] << "\tPort: " << anaPorts[i%4] << endl;
          *outFile << "Anagate: " << anaIPs[int(floor(i/4.))] << "\tPort: " << anaPorts[i%4] << endl;
  	      
		      //cout << "Getting cathode HV" << endl;
    		  current_ids=getADC( *dev[i], ext_id, ADC_CAT, nReplies, outFile);
		      CheckForMissingIDs(all_ids,current_ids,"vmon");
			  current_ids.clear();
        
    		  usleep(1000000);
			  
		      //cout << "Getting temperature" << endl;
    		  current_ids=getADC( *dev[i], ext_id, ADC_TEM, nReplies, outFile);
		      CheckForMissingIDs(all_ids,current_ids,"temp");

			  current_ids.clear();

    		  usleep(1000000);
  		  
		  	  //cout << "Getting current" << endl;
    		  current_ids=getADC( *dev[i], ext_id, ADC_CUR, nReplies, outFile);
		      CheckForMissingIDs(all_ids,current_ids,"imon");
			  current_ids.clear();

    		  usleep(1000000);
    		  
		      //cout << "Getting mvb" << endl;
    		  current_ids=getADC( *dev[i], ext_id, ADC_MVB, nReplies, outFile);
		      CheckForMissingIDs(all_ids,current_ids,"mvb");
			  current_ids.clear();

    		  usleep(1000000);
    		  
		      //cout << "Getting mvt" << endl;
    		  current_ids=getADC( *dev[i], ext_id, ADC_MVT, nReplies, outFile);
		      CheckForMissingIDs(all_ids,current_ids,"mvt");
			  current_ids.clear();
			  
    		  usleep(1000000);
    		  
		      //cout << "Getting dyn" << endl;
    		  current_ids=getADC( *dev[i], ext_id, ADC_DYN, nReplies, outFile);
		      CheckForMissingIDs(all_ids,current_ids,"dyn");
			  current_ids.clear();
			  
    		  usleep(1000000);
    		  
		      //cout << "Getting DAC" << endl;
    		  current_ids=getADC( *dev[i], ext_id, ADC_DAC, nReplies, outFile);
		      CheckForMissingIDs(all_ids,current_ids,"imon");
			  current_ids.clear();
			
    		  usleep(1000000);
			
		      //cout << "Getting HV setpoint" << endl;
    		  current_ids=getADC( *dev[i], ext_id, READ_HV, nReplies, outFile);
		      CheckForMissingIDs(all_ids,current_ids,"hv_setpoint");
			  current_ids.clear();
			
  		    outFile->close ();
    		  usleep(1000000);
    	  }
  	  sleep(1); //Wait for about 1 second.
  	  //sleep(60); //Wait for about 1 minute. Can be modified if desired
  	  }
	  
  }
	else
	 	cout<<"STOP IT!\n";
}





void HV_menu( Device &dev , int ext_id, int nReplies)
{
  int command[2];
  int choice,pinchoice;
  bool GOOD_CHOICE=false;
  bool GOODPINCHOICE=false;
  bool HVread=false;

  while(!GOOD_CHOICE){
    cout<<"\n\tVoltage status options:\n";
    cout<<"\t\t1. Read Voltage Status\n";
    cout<<"\t\t2. Enable all Voltage (not JAM)\n";
    cout<<"\t\t3. Disable all Voltage (not JAM)\n";
    cout<<"\t\t4. Enable all High Pins (not JAM)\n";
    cout<<"\t\t5. Disable all High Pins (not JAM)\n";
    cout<<"\t\t6. Enable all Medium Pins\n";
    cout<<"\t\t7. Disable all Medium Pins\n";
    cout<<"\t\t8. Enable Specific Pin\n";
    cout<<"\t\t9. Disable Specific Pin\n";
    cout<<"\tChoose Voltage status option:\n";
    cout<<"> ";
    cin>>choice;

	if(choice<=0 || choice>=10) return;
	
    switch(choice){
    case(1):
      command[0]=0x00;
      command[1]=0x00;
      HVread = true;
      GOOD_CHOICE=true;
      break;
    case(2):
      command[0]=0xFF;
      command[1]=0x00;
      cout<<"\tEnabling All (not JAM)\n";
      GOOD_CHOICE=true;
      break;
    case(3):
      command[0]=0xF0;
      command[1]=0x00;
      cout<<"\tDisabling All (not JAM)\n";
      GOOD_CHOICE=true;
      break;
    case(4):
      command[0]=0xCC;
      command[1]=0x00;
      cout<<"\tEnabling All High Pins (not JAM)\n";
      GOOD_CHOICE=true;
      break;
    case(5):
      command[0]=0xC0;
      command[1]=0x00;
      cout<<"\tDisabling All High Pins (not JAM)\n";
      GOOD_CHOICE=true;
      break;
    case(6):
      command[0]=0x33;
      command[1]=0x00;
      cout<<"\tEnabling All Medium Pins\n";
      GOOD_CHOICE=true;
      break;
    case(7):
      command[0]=0x30;
      command[1]=0x00;
      cout<<"\tDisabling All Medium Pins\n";
      GOOD_CHOICE=true;
      break;
    case(8):
      GOODPINCHOICE=false;
      while(!GOODPINCHOICE){
	cout<<"\n\t\tEnable Pin Options:\n";
	cout<<"\t\t\t1. ENBHVT\n";
	cout<<"\t\t\t2. ENBHVB\n";
	cout<<"\t\t\t3. ENBMVT\n";
	cout<<"\t\t\t4. ENBMVB\n";
	cout<<"\t\t\t5. JAMHVT\n";
	cout<<"\t\t\t6. JAMHVB\n";
	cout<<"\t\tChoose pin:\n";
	cout<<"> ";
	cin>>pinchoice;

	
	
	if(pinchoice==1){
	  command[0]=0x88;
	  command[1]=0x00;
	  GOODPINCHOICE=true;
	}
	else if(pinchoice==2){
	  command[0]=0x44;
	  command[1]=0x00;
	  GOODPINCHOICE=true;
	}
	else if(pinchoice==3){
	  command[0]=0x22;
	  command[1]=0x00;
	  GOODPINCHOICE=true;
	}
	else if(pinchoice==4){
	  command[0]=0x11;
	  command[1]=0x00;
	  GOODPINCHOICE=true;
	}
	else if(pinchoice==5){
	  command[0]=0x00;
	  command[1]=0x0A;
	  GOODPINCHOICE=true;
	}
	else if(pinchoice==6){
	  command[0]=0x00;
	  command[1]=0x05;
	  GOODPINCHOICE=true;
	}
	else{
	  cout<<"\n\tChoose from the list!\n";
	  GOODPINCHOICE=false;
	}
      }

      GOOD_CHOICE=true;
      break;

    case(9):
      GOODPINCHOICE=false;
      while(!GOODPINCHOICE){
	cout<<"\n\t\tDisable Pin Options:\n";
	cout<<"\t\t\t1. ENBHVT\n";
	cout<<"\t\t\t2. ENBHVB\n";
	cout<<"\t\t\t3. ENBMVT\n";
	cout<<"\t\t\t4. ENBMVB\n";
	cout<<"\t\t\t5. JAMHVT\n";
	cout<<"\t\t\t6. JAMHVB\n";
	cout<<"\t\tChoose pin:\n";
	cout<<"> ";
	cin>>pinchoice;
	cout<<"\n";

	if(pinchoice==1){
	  command[0]=0x80;
	  command[1]=0x00;
	  GOODPINCHOICE=true;
	}
	else if(pinchoice==2){
	  command[0]=0x40;
	  command[1]=0x00;
	  GOODPINCHOICE=true;
	}
	else if(pinchoice==3){
	  command[0]=0x20;
	  command[1]=0x00;
	  GOODPINCHOICE=true;
	}
	else if(pinchoice==4){
	  command[0]=0x10;
	  command[1]=0x00;
	  GOODPINCHOICE=true;
	}
	else if(pinchoice==5){
	  command[0]=0x00;
	  command[1]=0x08;
	  GOODPINCHOICE=true;
	}
	else if(pinchoice==6){
	  command[0]=0x00;
	  command[1]=0x04;
	  GOODPINCHOICE=true;
	}
	else{
	  cout<<"\n\tChoose from the list!\n";
	  GOODPINCHOICE=false;
	}
      }
      GOOD_CHOICE=true;
      break;
    default:
      cout<<"\nChoose from the list!\n";
      GOOD_CHOICE=false;
      break;
    }
  }

  HVControl( dev, ext_id, command[0], command[1], HVread, nReplies,false );
  return;

}

void TP_menu( int ext_id )
{
	int position, amount;
	int  command = 0xFF;
	cout<<"Amount to Address\n";
	cout<<"\t 1. Single Base\n";
	cout<<"\t 2. All Bases\n";
	cin>>amount;
	if(amount==1)	cout << "Removed.. See commented code in TP_menu function to re-enable" << endl;

	
	cout<<"Test Pulser Menu.\n";
	cout<<"\t 1. Fire the test pulser\n";
	cout<<"\t 2. Enable external sync of test pulser\n";
	cout<<"\t 3. Disable external sync of test pulser\n";
	cout<<"> ";
	cin >> position;
	
	if(amount < 1 || amount > 2 || position < 1 || position > 3)
	{
		cout<<"NO!\n";
		exit(EXIT_SUCCESS);
	}

	switch( position ){
	case(1):
	  command = TP_FIRE;
	  break;
	case(2):
	  command = TP_ENBSYNC;
	  break;
	case(3):
	  command = TP_DISSYNC;
	  break;
	}
	
	if(amount == 1)
	{	

/* 		int xVal, yVal;
		cout<<"X Coordinate: "<<xVal;
		cout<<"Y Coordinate: "<<yVal;
		for(int i = 1; i < 2802; i++)
		{
			if(xVal == xCoord[i] && yVal == yCoord[i])
			{
				cout<<"Base found: #"<<bases[i]<<" at "<<xCoord[i]<<"/"<<yCoord[i]<<" on strand "<<strands[i]<<"\n";	
				TestPulser( *dev[strands[i]-1], CAN_ID[i], command );
				cout<<"Pulser Modified\n\n";
			}
			else if (i == 2801)
			{
				cout<<"Coordinates Not Found!\n";
				break;
			}
		} */
	}
	else if(amount == 2)
	{
		for(int i = 0; i < nDevices; i++){
			TestPulser( *dev[i], ext_id, command );
			usleep(100000);
		}
		cout<<"All Pulsers Modified\n\n";
	}
}

void CheckForMissingIDs(vector<int> all_ids, vector<int> current_ids, string ADC_type){
				
			for(int j = 0; j!=all_ids.size(); ++j) {
				const int my_all_id = all_ids[j];
				bool found = false;
				for(int k=0; k!=current_ids.size();++k){
					const int my_curr_id = current_ids[k];
					if(my_all_id==my_curr_id) found = true;
				}

				if(!found) {
					cout << endl << "Missing base id: " << my_all_id  << endl;
					cout << "ADC being read: " << ADC_type << endl << endl;
			}

}
}