#ifndef ESP_LIB_GPIO_H
#define ESP_LIB_GPIO_H

#include "../Platforms.h"


#if IS_ESP8266


/*
	#include "fastGPIO.h"

    struct MyGPIO {

		static inline bool get(const uint8_t num) {
			return GPIO_INPUT_GET(num);
		}

		static inline void set(const uint8_t num) {
			GPIO_REG_WRITE(GPIO_OUT_W1TS_ADDRESS, 1 << num);
		}

		static inline void clear(const uint8_t num) {
			GPIO_REG_WRITE(GPIO_OUT_W1TC_ADDRESS, 1 << num);
		}

		static inline void setOutput(const uint8_t num) {
			switch(num) {
			    case 0: GPIO0_OUTPUT_SET; break;
			    case 1: GPIO1_OUTPUT_SET; break;
			    case 2: GPIO2_OUTPUT_SET; break;
			    case 3: GPIO3_OUTPUT_SET; break;
			    case 4: GPIO4_OUTPUT_SET; break;
			    case 5: GPIO5_OUTPUT_SET; break;
				//case 6: GPIO6_OUTPUT_SET; break;
				//case 7: GPIO7_OUTPUT_SET; break;
				//case 8: GPIO8_OUTPUT_SET; break;
			    case 9: GPIO9_OUTPUT_SET; break;
			    case 10: GPIO10_OUTPUT_SET; break;
				//case 11: GPIO11_OUTPUT_SET; break;
			    case 12: GPIO12_OUTPUT_SET; break;
			    case 13: GPIO13_OUTPUT_SET; break;
			    case 14: GPIO14_OUTPUT_SET; break;
			    case 15: GPIO15_OUTPUT_SET; break;
			}
		}

		static inline void setInput(const uint8_t num) {
			switch(num) {
			    case 0: GPIO0_INPUT_SET; break;
			    case 1: GPIO1_INPUT_SET; break;
			    case 2: GPIO2_INPUT_SET; break;
			    case 3: GPIO3_INPUT_SET; break;
			    case 4: GPIO4_INPUT_SET; break;
			    case 5: GPIO5_INPUT_SET; break;
				//case 6: GPIO6_INPUT_SET; break;
				//case 7: GPIO7_INPUT_SET; break;
				//case 8: GPIO8_INPUT_SET; break;
			    case 9: GPIO9_INPUT_SET; break;
			    case 10: GPIO10_INPUT_SET; break;
				//case 11: GPIO11_INPUT_SET; break;
			    case 12: GPIO12_INPUT_SET; break;
			    case 13: GPIO13_INPUT_SET; break;
			    case 14: GPIO14_INPUT_SET; break;
			    case 15: GPIO15_INPUT_SET; break;
			}
		}

		static inline void setPullUp(const uint8_t num) {
			switch(num) {
			    case 0: GPIO0_INPUT_PULLUP_SET; break;
			    case 1: GPIO1_INPUT_PULLUP_SET; break;
			    case 2: GPIO2_INPUT_PULLUP_SET; break;
			    case 3: GPIO3_INPUT_PULLUP_SET; break;
			    case 4: GPIO4_INPUT_PULLUP_SET; break;
			    case 5: GPIO5_INPUT_PULLUP_SET; break;
				//case 6: GPIO6_INPUT_PULLUP_SET; break;
				//case 7: GPIO7_INPUT_PULLUP_SET; break;
				//case 8: GPIO8_INPUT_PULLUP_SET; break;
			    case 9: GPIO9_INPUT_PULLUP_SET; break;
			    case 10: GPIO10_INPUT_PULLUP_SET; break;
				//case 11: GPIO11_INPUT_PULLUP_SET; break;
			    case 12: GPIO12_INPUT_PULLUP_SET; break;
			    case 13: GPIO13_INPUT_PULLUP_SET; break;
			    case 14: GPIO14_INPUT_PULLUP_SET; break;
			    case 15: GPIO15_INPUT_PULLUP_SET; break;
			}
		}

		static void toggleBuiltInLED() {
			static bool level = false;
			setOutput(2);
			level = !level;
			if (level) {set(2);} else {clear(2);}
		}

	};
*/

	#include "driver/gpio.h"
	#include "esp8266/gpio_struct.h"

    struct MyGPIO {

		static inline bool get(const uint8_t num) {
			return get((gpio_num_t)num);
		}
		static inline bool get(const gpio_num_t num) {
			return gpio_get_level(num);				// TODO faster access like READ_PERI_REG??
		}

		template <typename T> static inline void setOrClear(const uint8_t num, const T val) {
			if (val) {set(num);} else {clear(num);}
			//WRITE_PERI_REG(GPIO_OUT_W1TS_REG, (val != 0) << num);	// branchless but usually slower
			//WRITE_PERI_REG(GPIO_OUT_W1TC_REG, (val == 0) << num);
		}

		__attribute__((always_inline)) static inline void set(const uint8_t num) {
			//gpio_set_level((gpio_num_t)num, 1);				// TODO: faster
			GPIO.out_w1ts |= (0x1 << num);
        }

		__attribute__((always_inline)) static inline void clear(const uint8_t num) {
			//gpio_set_level((gpio_num_t)num, 0);				// TODO: faster
			GPIO.out_w1tc |= (0x1 << num);
		}

		static inline void setOutput(const uint8_t num) {
			setOutput((gpio_num_t)num);
		}
		static inline void setOutput(const gpio_num_t num) {
			//gpio_set_direction(num, GPIO_MODE_OUTPUT);		// does not always suffice?!
			gpio_config_t io_conf;
			io_conf.intr_type = GPIO_INTR_DISABLE;
			io_conf.mode = GPIO_MODE_OUTPUT;
			io_conf.pin_bit_mask = (1<<num);
			io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
			io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
			ESP_ERROR_CHECK(gpio_config(&io_conf));
		}

		static inline void setInput(const uint8_t num) {
			setInput((gpio_num_t)num);
		}
		static inline void setInput(const gpio_num_t num) {
			//gpio_set_direction(num, GPIO_MODE_INPUT);		// does not always suffice?!
			gpio_config_t io_conf;
			io_conf.intr_type = GPIO_INTR_DISABLE;
			io_conf.mode = GPIO_MODE_INPUT;
			io_conf.pin_bit_mask = (1<<num);
			io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
			io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
			ESP_ERROR_CHECK(gpio_config(&io_conf));
		}
		
		static inline void setInputPullUp(const uint8_t num) {
			setInputPullUp((gpio_num_t)num);
		}
		static inline void setInputPullUp(const gpio_num_t num) {
			gpio_config_t io_conf;
			io_conf.intr_type = GPIO_INTR_DISABLE;
			io_conf.mode = GPIO_MODE_INPUT;
			io_conf.pin_bit_mask = (1<<num);
			io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
			io_conf.pull_up_en = GPIO_PULLUP_ENABLE;		// here
			ESP_ERROR_CHECK(gpio_config(&io_conf));
		}

		static void toggleBuiltInLED() {
			static bool level = false;
			setOutput(GPIO_NUM_2);
			level = !level;
			if (level) {set(GPIO_NUM_2);} else {clear(GPIO_NUM_2);}
		}

	};


#elif IS_ESP32

	#include "driver/gpio.h"

	// NOTE from the manual
	// GPIO 6-11 are usually used for SPI flash.
	// GPIO 34-39 can only be set as input mode and do not have software pullup or pulldown functions.


    struct MyGPIO {

		static inline bool get(const uint8_t num) {
			return get((gpio_num_t)num);
		}
		static inline bool get(const gpio_num_t num) {
			return gpio_get_level(num);				// TODO faster access like READ_PERI_REG??
		}

		template <typename T> static inline void setOrClear(const uint8_t num, const T val) {
			if (val) {set(num);} else {clear(num);}
			//WRITE_PERI_REG(GPIO_OUT_W1TS_REG, (val != 0) << num);	// branchless but usually slower
			//WRITE_PERI_REG(GPIO_OUT_W1TC_REG, (val == 0) << num);
		}

		static inline void set(const uint8_t num) {
			WRITE_PERI_REG(GPIO_OUT_W1TS_REG, 1 << num);
			//gpio_set_level((gpio_num_t)num, 1);
		}

		static inline void clear(const uint8_t num) {
			WRITE_PERI_REG(GPIO_OUT_W1TC_REG, 1 << num);
			//gpio_set_level((gpio_num_t)num, 0);
		}

		static inline void setOutput(const uint8_t num) {
			setOutput((gpio_num_t)num);
		}
		static inline void setOutput(const gpio_num_t num) {
			//gpio_set_direction(num, GPIO_MODE_OUTPUT);		// does not always suffice?!
			gpio_config_t io_conf;
			io_conf.intr_type = GPIO_INTR_DISABLE;
			io_conf.mode = GPIO_MODE_OUTPUT;
			io_conf.pin_bit_mask = (1<<num);
			io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
			io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
			ESP_ERROR_CHECK(gpio_config(&io_conf));
		}

		static inline void setInput(const uint8_t num) {
			setInput((gpio_num_t)num);
		}
		static inline void setInput(const gpio_num_t num) {
			//gpio_set_direction(num, GPIO_MODE_INPUT);		// does not always suffice?!
			gpio_config_t io_conf;
			io_conf.intr_type = GPIO_INTR_DISABLE;
			io_conf.mode = GPIO_MODE_INPUT;
			io_conf.pin_bit_mask = (1<<num);
			io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
			io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
			ESP_ERROR_CHECK(gpio_config(&io_conf));
		}
		
		static inline void setInputPullUp(const uint8_t num) {
			setInputPullUp((gpio_num_t)num);
		}
		static inline void setInputPullUp(const gpio_num_t num) {
			gpio_config_t io_conf;
			io_conf.intr_type = GPIO_INTR_DISABLE;
			io_conf.mode = GPIO_MODE_INPUT;
			io_conf.pin_bit_mask = (1<<num);
			io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
			io_conf.pull_up_en = GPIO_PULLUP_ENABLE;		// here
			ESP_ERROR_CHECK(gpio_config(&io_conf));
		}

		static void toggleBuiltInLED() {
			static bool level = false;
			setOutput(GPIO_NUM_2);
			level = !level;
			if (level) {set(GPIO_NUM_2);} else {clear(GPIO_NUM_2);}
		}

	};

#elif IS_TEENSY

	struct MyGPIO {
		
	public:
	
		static inline void setOutput(uint8_t pin)	{pinMode(pin, OUTPUT);}
		
		static inline void setInput(uint8_t pin)	{pinMode(pin, INPUT);}
		
		static inline void clear(uint8_t pin)		{digitalWriteFast(pin, 0);}
		
		static inline void set(uint8_t pin)			{digitalWriteFast(pin, 1);}
		
		static inline void setOrClear(uint8_t pin, uint8_t val) {digitalWriteFast(pin, val);}
		
		static inline uint8_t get(uint8_t pin)		{return digitalReadFast(pin);}
		
		
		
	};
	
#else

	#error "GPIO: unsupported platform";

#endif


#endif // ESP_LIB_GPIO_H