using namespace std;

#define BUFFER_MAX 3
#define DIRECTION_MAX 35
#define VALUE_MAX 35
#define INPUT  0
#define OUTPUT 1

#define LOW  0
#define HIGH 1
static uint64_t epochMilli;
static uint64_t epochMicro;

// extern void delay(unsigned int howLong);

/*
 * initialiseEpoch:
 *	Initialise our start-of-time variable to be the current unix
 *	time in milliseconds and microseconds.
 *********************************************************************************
 */


void logError(short errorSourceID, std::string sensor, std::string msg) {  // used system wide
	// std::cout << "logError-------------" << endl;
	// add error to database
	connectMysql();
	std::string query = "INSERT INTO errors(errorSourceID,sensor,message) VALUES('" + to_string(errorSourceID) + "','" + sensor + "','" + msg + "')";
	mysql_query(conn, query.c_str());  // run query
	mysql_disconnect(); 
}

static void initialiseEpoch (void)  // taken from wiringPi library
{
	#ifdef	OLD_WAY
		struct timeval tv;

		gettimeofday (&tv, NULL);
		epochMilli = (uint64_t)tv.tv_sec * (uint64_t)1000    + (uint64_t)(tv.tv_usec / 1000);
		epochMicro = (uint64_t)tv.tv_sec * (uint64_t)1000000 + (uint64_t)(tv.tv_usec);
	#else
		struct timespec ts;

		clock_gettime (CLOCK_MONOTONIC_RAW, &ts);
		epochMilli = (uint64_t)ts.tv_sec * (uint64_t)1000    + (uint64_t)(ts.tv_nsec / 1000000L);
		epochMicro = (uint64_t)ts.tv_sec * (uint64_t)1000000 + (uint64_t)(ts.tv_nsec /    1000L);
	#endif
}

/*
 * delay:
 *	Wait for some number of milliseconds
 *********************************************************************************
 */

void delay(unsigned int howLong) {  // taken from wiringPi library
    struct timespec sleeper, dummy;

    sleeper.tv_sec  = (time_t)(howLong / 1000);
    sleeper.tv_nsec = (long)(howLong % 1000) * 1000000;

    nanosleep (&sleeper, &dummy);
}

/*
 * delayMicroseconds:
 *	This is somewhat intersting. It seems that on the Pi, a single call
 *	to nanosleep takes some 80 to 130 microseconds anyway, so while
 *	obeying the standards (may take longer), it's not always what we
 *	want!
 *
 *	So what I'll do now is if the delay is less than 100uS we'll do it
 *	in a hard loop, watching a built-in counter on the ARM chip. This is
 *	somewhat sub-optimal in that it uses 100% CPU, something not an issue
 *	in a microcontroller, but under a multi-tasking, multi-user OS, it's
 *	wastefull, however we've no real choice )-:
 *
 *      Plan B: It seems all might not be well with that plan, so changing it
 *      to use gettimeofday () and poll on that instead...
 *********************************************************************************
 */

void delayMicrosecondsHard(unsigned int howLong) {  // taken from wiringPi library
	struct timeval tNow, tLong, tEnd;

	gettimeofday(&tNow, NULL);
	tLong.tv_sec  = howLong / 1000000;
	tLong.tv_usec = howLong % 1000000;
	timeradd(&tNow, &tLong, &tEnd);

	while (timercmp(&tNow, &tEnd, <))
		gettimeofday(&tNow, NULL);
}

void delayMicroseconds(unsigned int howLong) {  // taken from wiringPi library
	struct timespec sleeper;
	unsigned int uSecs = howLong % 1000000;
	unsigned int wSecs = howLong / 1000000;

	if (howLong ==   0)
		return;
	else if (howLong  < 100)
		delayMicrosecondsHard(howLong);
	else
	{
		sleeper.tv_sec  = wSecs;
		sleeper.tv_nsec = (long)(uSecs * 1000L);
		nanosleep(&sleeper, NULL);
	}
}


/*
 * millis:
 *	Return a number of milliseconds as an unsigned int.
 *	Wraps at 49 days.
 *********************************************************************************
 */

unsigned int millis(void) {  // generate milliseconds
	// uint64_t now;

	// #ifdef	OLD_WAY
	// 	struct timeval tv;
	// 	gettimeofday(&tv, NULL);
	// 	now  = (uint64_t)tv.tv_sec * (uint64_t)1000 + (uint64_t)(tv.tv_usec / 1000);

	// #else
	// 	struct  timespec ts;
	// 	clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
	// 	now  = (uint64_t)ts.tv_sec * (uint64_t)1000 + (uint64_t)(ts.tv_nsec / 1000000L);
	// #endif

  	// return(uint32_t)(now - epochMilli);

    time_point<Clock> endMillis = Clock::now();
    milliseconds diff = duration_cast<milliseconds>(endMillis - startMillis);
    // std::cout << diff.count() << "ms" << std::endl;
	return(diff.count());
}

/*
 * micros:
 *	Return a number of microseconds as an unsigned int.
 *	Wraps after 71 minutes.
 *********************************************************************************
 */

unsigned int micros(void) {  // taken from wiringPi library
	uint64_t now;
	#ifdef	OLD_WAY
		struct timeval tv;
		gettimeofday(&tv, NULL);
		now  = (uint64_t)tv.tv_sec * (uint64_t)1000000 + (uint64_t)tv.tv_usec;
	#else
		struct  timespec ts;
		clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
		now  = (uint64_t)ts.tv_sec * (uint64_t)1000000 + (uint64_t)(ts.tv_nsec / 1000);
	#endif


	return(uint32_t)(now - epochMicro);
}

static int GPIOExport(int pin) {  // initiate gpio
	char buffer[BUFFER_MAX];
	ssize_t bytes_written;
	int fd;

	fd = open("/sys/class/gpio/export", O_WRONLY);
	if (-1 == fd) {
		fprintf(stderr, "Failed to open export for writing!\n");
		return(-1);
	}

	bytes_written = snprintf(buffer, BUFFER_MAX, "%d", pin);
	write(fd, buffer, bytes_written);
	close(fd);
	return(0);
}

static int GPIOUnexport(int pin) {  // free gpio
	char buffer[BUFFER_MAX];
	ssize_t bytes_written;
	int fd;

	fd = open("/sys/class/gpio/unexport", O_WRONLY);
	if (-1 == fd) {
		fprintf(stderr, "Failed to open unexport for writing!\n");
		return(-1);
	}

	bytes_written = snprintf(buffer, BUFFER_MAX, "%d", pin);
	write(fd, buffer, bytes_written);
	close(fd);
	return(0);
}

static int pinMode(int pin, int dir) {  // set pin as input or output
	static const char s_directions_str[]  = "in\0out";

	char path[DIRECTION_MAX];
	int fd;

	snprintf(path, DIRECTION_MAX, "/sys/class/gpio/gpio%d/direction", pin);
	fd = open(path, O_WRONLY);
	if (-1 == fd) {
		fprintf(stderr, "Failed to open gpio direction for writing!\n");
		return(-1);
	}

	if (-1 == write(fd, &s_directions_str[INPUT == dir ? 0 : 3], INPUT == dir ? 2 : 3)) {
		fprintf(stderr, "Failed to set direction!\n");
		return(-1);
	}

	close(fd);
	return(0);
}

int digitalRead(int pin) {  // read digital pin
	char path[VALUE_MAX];
	char value_str[3];
	int fd;

	snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
	fd = open(path, O_RDONLY);
	if (-1 == fd) {
		std::cout <<"Failed to open gpio value for reading - " << pin << endl;
		return(-1);
	}

	if (-1 == read(fd, value_str, 3)) {
		std::cout <<"Failed to read value - " << pin << endl;
		return(-1);
	}

	close(fd);

	// std::cout <<"GPIO = " << atoi(value_str) << endl;
	// if (atoi(value_str) == 0) {return LOW;}
	// else {return HIGH;}
	return(atoi(value_str));
}

static int digitalWrite(int pin, int value) {  // write to digital pin
	static const char s_values_str[] = "01";

	char path[VALUE_MAX];
	int fd;

	snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
	fd = open(path, O_WRONLY);
	if (-1 == fd) {
		fprintf(stderr, "Failed to open gpio value for writing!\n");
		return(-1);
	}

	if (1 != write(fd, &s_values_str[LOW == value ? 0 : 1], 1)) {
		fprintf(stderr, "Failed to write value!\n");
		return(-1);
	}
	close(fd);
	
	// verify GPIO pin was updated
	bool f = 0;  // flag to exit while loop
	while (f == 0)  // check if GPIO received the signal
	{
		if (digitalRead(pin) == value) {f = 1;}  // GPIO was updated correctly
		else {  // GPIO wasn't updated, try again
    		std::cout << "TRY AGAIN                 - " << std::endl;
			logError(4, to_string(pin), "GPIO did not update");
			delay(50);
			snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
			fd = open(path, O_WRONLY);
			if (-1 == fd) {
				fprintf(stderr, "Failed to open gpio value for writing!\n");
				return(-1);
			}

			if (1 != write(fd, &s_values_str[LOW == value ? 0 : 1], 1)) {
				fprintf(stderr, "Failed to write value!\n");
				return(-1);
			}
			close(fd);
			delay(50);
		}
	}
	// std::cout << "pin -------- " << pin << " - " << digitalRead(pin) << std::endl;
	return(0);
}

static int sysFds [64] =
{
  -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
  -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
  -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
  -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
} ;
int digitalRead2 (int pin)
{
  char c ;
//   struct wiringPiNodeStruct *node = wiringPiNodes ;
//   if ((pin & PI_GPIO_MASK) == 0)		// On-Board Pin
//   {
    // /**/ if (wiringPiMode == WPI_MODE_GPIO_SYS)	// Sys mode
    // {
      if (sysFds [pin] == -1)
	return LOW ;

      lseek  (sysFds [pin], 0L, SEEK_SET) ;
      read   (sysFds [pin], &c, 1) ;
      return (c == '0') ? LOW : HIGH ;
    // }
    // else if (wiringPiMode == WPI_MODE_PINS)
    //   pin = pinToGpio [pin] ;
    // else if (wiringPiMode == WPI_MODE_PHYS)
    //   pin = physToGpio [pin] ;
    // else if (wiringPiMode != WPI_MODE_GPIO)
    //   return LOW ;

    // if ((*(gpio + gpioToGPLEV [pin]) & (1 << (pin & 31))) != 0)
    //   return HIGH ;
    // else
    //   return LOW ;
//   }
//   else
//   {
//     if ((node = wiringPiFindNode (pin)) == NULL)
//       return LOW ;
//     return node->digitalRead (node, pin) ;
//   }
}
