#include "BCap.h"
#include "bcm2835.h"
#include "Include/bCAPServer/bcap_server.h"
#include <iostream>
#include <tr1/memory>
#include <mutex>

#define LOG_OUTPUT
#ifdef LOG_OUTPUT
#include <time.h>
#endif

//#define TEST

namespace bCap
{
class GPIOVariant : public VARIANT
{
public:
	GPIOVariant() { VariantInit(this); }
	virtual ~GPIOVariant() { VariantClear(this); }
};

// typedef
// AddControllerされた擬似コントローラーの情報を保持
struct ControllerInfo
{
};

struct VariableInfo
{
	uint8_t mode;
	uint8_t pud;
	uint8_t pin;
};

// global fields

// b-Capのファイルデスクリプタ
int fd = 0;
// 内部で採番するIDを保持
int32_t currentId = 1;
// 擬似コントローラーの情報
std::map<int32_t, std::tr1::shared_ptr<ControllerInfo> > controllerMap;
// 変数の情報
std::map<int32_t, std::tr1::shared_ptr<VariableInfo> > variableMap;
// Pinの名前と値の情報
std::map<std::wstring, uint8_t> GPIO_PIN_MAP;
// FunctionSelectの名前と値の情報
std::map<std::wstring, uint8_t> FUNCTION_SELECT_MAP;
// PUDControlの名前と値の情報
std::map<std::wstring, uint8_t> PUD_CONTROL_MAP;

std::mutex accessLock;

volatile bool running = false;

// prototypes
std::tr1::shared_ptr<ControllerInfo> GetControllerInfo(int32_t objectId);
std::tr1::shared_ptr<ControllerInfo> InitControllerInfo(int32_t objectId);
void RemoveControllerInfo(int32_t objectId);
std::tr1::shared_ptr<VariableInfo> GetVariableInfo(int32_t objectId);
std::tr1::shared_ptr<VariableInfo> InitVariableInfo(int32_t objectId);
void RemoveVariableInfo(int32_t objectId);
bool StringEquals(const VARIANT& vntArg, const wchar_t *str);
void GetArg(const VARIANT *vntArgs, int16_t Argc, int index, VARIANT& vntOut);
HRESULT CheckArg(const VARIANT& vnt, VARENUM type);
HRESULT GetObjectId(const VARIANT *vntArgs, int16_t Argc, VARIANT& vntOut);
int32_t CreateObjectId();
HRESULT ReadPinValue(VariableInfo *vInfo, VARIANT *pVal);
HRESULT WritePinValue(VariableInfo *vInfo, VARIANT *pVal);

void InitGpioPinMap();
void InitFunctionSelectMap();
void InitPudControlMap();


// utility functions

// 指定したIDの擬似コントローラーを検索してポインタを返す。
// コントローラーが見つからない場合はNULL
std::tr1::shared_ptr<ControllerInfo> GetControllerInfo(int32_t objectId) {
	std::tr1::shared_ptr<ControllerInfo> pInfo;

	std::map<int32_t, std::tr1::shared_ptr<ControllerInfo> >::iterator it = controllerMap.find(objectId);
	if (it != controllerMap.end()) {
		pInfo = controllerMap[it->first];
	}
	return pInfo;
}

// 指定したIDの擬似コントローラーを初期化してポインタを返す
std::tr1::shared_ptr<ControllerInfo> InitControllerInfo(int32_t objectId) {
	std::tr1::shared_ptr<ControllerInfo> cInfo(new ControllerInfo);
	//TODO 初期化

	controllerMap[objectId] = cInfo;
	return GetControllerInfo(objectId);
}

// 指定したIDの擬似コントローラーを削除する
void RemoveControllerInfo(int32_t objectId) {
	controllerMap.erase(objectId);
}

// 指定したIDの擬似コントローラーを検索してポインタを返す。
// コントローラーが見つからない場合はNULL
std::tr1::shared_ptr<VariableInfo> GetVariableInfo(int32_t objectId) {
	std::tr1::shared_ptr<VariableInfo> pInfo;

	std::map<int32_t, std::tr1::shared_ptr<VariableInfo> >::iterator it = variableMap.find(objectId);
	if (it != variableMap.end()) {
		pInfo = variableMap[it->first];
	}
	return pInfo;
}

// 指定したIDの擬似Variableを初期化してポインタを返す
std::tr1::shared_ptr<VariableInfo> InitVariableInfo(int32_t objectId) {
	std::tr1::shared_ptr<VariableInfo> vInfo(new VariableInfo);
	vInfo->pin = RPI_GPIO_P1_03;
	vInfo->mode = BCM2835_GPIO_FSEL_INPT;
	vInfo->pud = BCM2835_GPIO_PUD_OFF;

	variableMap[objectId] = vInfo;
	return GetVariableInfo(objectId);
}

// 指定したIDの擬似ヴァリアブルを削除する
void RemoveVariableInfo(int32_t objectId) {
	variableMap.erase(objectId);
}

// VARIANTと指定文字列が同じ文字列かどうかを調べる。同じならtrue
bool StringEquals(const VARIANT& vntArg, const wchar_t *str)  {
	if (vntArg.vt == VT_BSTR) {
		return wcscmp((wchar_t*)vntArg.bstrVal, str) == 0;
	} else {
		return false;
	}
}

// コールバック引数から指定位置の引数を取り出す
void GetArg(const VARIANT *vntArgs, int16_t Argc, int index, VARIANT& vntOut) {
	if (index < Argc) {
		VariantCopy(&vntOut, &vntArgs[index]);
	} else {
		VariantClear(&vntOut);
	}
}

// VARIANTが指定の型を保持しているかを調べる
HRESULT CheckArg(const VARIANT& vnt, VARENUM type) {
	return vnt.vt == type ? S_OK : E_INVALIDARG;
}

// 擬似コントローラーやメッセージの要求は先頭にIDが入っているので、それを取り出す
HRESULT GetObjectId(const VARIANT *vntArgs, int16_t Argc, VARIANT& vntOut) {
	GetArg(vntArgs, Argc, 0, vntOut);
	return CheckArg(vntOut, VT_I4);
}


// 新規のコントローラーおよびヴァリアブルのオブジェクトIDを作成する
int32_t CreateObjectId() {
	return currentId++;
}

// ヴァリアブルオブジェクトのデータからPinの値を読み込む
// HIGH : true, LOW : false
HRESULT ReadPinValue(std::tr1::shared_ptr<VariableInfo> vInfo, VARIANT *pVal) {
	if (vInfo->mode == BCM2835_GPIO_FSEL_INPT) {
		uint8_t value;
		{
			std::lock_guard<std::mutex> lock(accessLock);
			// Read some data
			value = bcm2835_gpio_lev(vInfo->pin);
		}
		VariantClear(pVal);
		pVal->vt = VT_BOOL;
		pVal->boolVal = value == 0x00 ? VARIANT_FALSE : VARIANT_TRUE;

		return S_OK;
	} else {
		return E_FAIL;
	}
}

// ヴァリアブルオブジェクトのデータからPinの値を書き込む
// HIGH : true, LOW : false
HRESULT WritePinValue(std::tr1::shared_ptr<VariableInfo> vInfo, VARIANT *pVal) {
	if (vInfo->mode != BCM2835_GPIO_FSEL_INPT && pVal->vt == VT_BOOL) {
		{
			std::lock_guard<std::mutex> lock(accessLock);
			bcm2835_gpio_write(vInfo->pin, pVal->boolVal == VARIANT_FALSE ? LOW : HIGH);
		}

		return S_OK;
	} else {
		return E_FAIL;
	}
}

// handlers
HRESULT ServiceStart(VARIANT *vntArgs, int16_t Argc, VARIANT *vntRet) {
	return S_OK;
}

HRESULT ServiceStop(VARIANT *vntArgs, int16_t Argc, VARIANT *vntRet) {
	return S_OK;
}

HRESULT ControllerConnect(VARIANT *vntArgs, int16_t Argc, VARIANT *vntRet) {
	GPIOVariant vntName;
	GPIOVariant vntProv;
	GPIOVariant vntOption;

	GetArg(vntArgs, Argc, 0, vntName);
	GetArg(vntArgs, Argc, 1, vntProv);
	GetArg(vntArgs, Argc, 3, vntOption);

	vntRet->vt = VT_I4;
	vntRet->lVal = CreateObjectId();

	InitControllerInfo(vntRet->lVal);

#ifdef LOG_OUTPUT
	//TODO log
	fprintf(stdout, "ControllerConnect: %i\n", vntRet->lVal);
#endif
	return S_OK;
};

HRESULT ControllerDisconnect(VARIANT *vntArgs, int16_t Argc, VARIANT *vntRet) {
	GPIOVariant vntId;

	HRESULT hr = GetObjectId(vntArgs, Argc, vntId);
	if (SUCCEEDED(hr)) {
		RemoveControllerInfo(vntId.lVal);

#ifdef LOG_OUTPUT
		//TODO log
		fprintf(stdout, "ControllerDisconnect: %i\n", vntId.lVal);
#endif
	}
	return S_OK;
};

HRESULT ControllerGetVariableNames(VARIANT *vntArgs, int16_t Argc, VARIANT *vntRet) {
	SAFEARRAY *vntVNames = SafeArrayCreateVector(VT_BSTR, 0, GPIO_PIN_MAP.size());
	BSTR *pvData;
    HRESULT hr = SafeArrayAccessData(vntVNames, (void**)&pvData);
    if(FAILED(hr)) {
		SafeArrayDestroy(vntVNames);
		return hr;
	}

	for (std::map<std::wstring, uint8_t>::iterator it = GPIO_PIN_MAP.begin(); it != GPIO_PIN_MAP.end(); it++) {
		BSTR bstr = SysAllocString(it->first.c_str());
		*pvData = bstr;
		pvData++;
	}
 
    hr = SafeArrayUnaccessData(vntVNames);
    if(FAILED(hr)) {
		SafeArrayDestroy(vntVNames);
		return hr;
	}

	VariantClear(vntRet);
	vntRet->vt = VT_ARRAY | VT_BSTR;
	vntRet->parray = vntVNames;

	return S_OK;
}

HRESULT ControllerGetVariable(VARIANT *vntArgs, int16_t Argc, VARIANT *vntRet) {
	GPIOVariant vntPin;
	GPIOVariant vntOpt;
	
	GetArg(vntArgs, Argc, 1, vntPin);
	GetArg(vntArgs, Argc, 2, vntOpt);
	
	// pinの番号を取得
	uint8_t pin;
	if (vntPin.vt == VT_BSTR){
		std::map<std::wstring, uint8_t>::iterator it = GPIO_PIN_MAP.find(vntPin.bstrVal);
		//for (it = GPIO_PIN_MAP.begin(); it != GPIO_PIN_MAP.end(); it++) {
		//	if (StringEquals(vntPin, it->first.c_str())) {
		//		pin = it->second;
		//		break;
		//	}
		//}
		if (it == GPIO_PIN_MAP.end()){
			return E_INVALIDARG;
		}
		pin = it->second;
	} else {
		return E_INVALIDARG;
	}

	// Option文字列の解析
	uint8_t mode;
	uint8_t pud = 0xFF;
	if (vntOpt.vt == VT_BSTR){
		// modeの取得
		GPIOVariant vntMode;
		BSTR modeKey = SysAllocString(L"Mode");
		HRESULT hr = GetOptionValue(vntOpt.bstrVal, modeKey, VT_BSTR, &vntMode);
		SysFreeString(modeKey);

		if (FAILED(hr) || vntMode.vt == VT_EMPTY) {
			mode = BCM2835_GPIO_FSEL_INPT;
		} else {
			std::map<std::wstring, uint8_t>::iterator it = FUNCTION_SELECT_MAP.find(vntMode.bstrVal);
			//for (it = FUNCTION_SELECT_MAP.begin(); it != FUNCTION_SELECT_MAP.end(); it++) {
			//	if (StringEquals(vntMode, it->first.c_str())) {
			//		mode = it->second;
			//		break;
			//	}
			//}
			if (it == FUNCTION_SELECT_MAP.end()){
				return E_INVALIDARG;
			}
			mode = it->second;
		}
		VariantClear(&vntMode);

		if(mode == BCM2835_GPIO_FSEL_INPT){
			// PUDの番号を取得
			GPIOVariant vntPUD;
			BSTR pudKey = SysAllocString(L"PUD");
			HRESULT hr = GetOptionValue(vntOpt.bstrVal, pudKey, VT_BSTR, &vntPUD);
			SysFreeString(pudKey);
			if(FAILED(hr) || vntPUD.vt == VT_EMPTY) {
				pud = 0x00;
			} else if (vntPUD.vt == VT_BSTR) {
				std::map<std::wstring, uint8_t>::iterator it = PUD_CONTROL_MAP.find(vntPUD.bstrVal);
				//for (it = PUD_CONTROL_MAP.begin(); it != PUD_CONTROL_MAP.end(); it++) {
				//	if (StringEquals(vntPUD, it->first.c_str())) {
				//		pud = it->second;
				//		break;
				//	}
				//}
				if (it == PUD_CONTROL_MAP.end()){
#ifdef LOG_OUTPUT
					fprintf(stdout, "ControllerGetVariable: get PUD Nothing\n");
#endif
					return E_INVALIDARG;
				}
				pud = it->second;
			} else {
				return E_INVALIDARG;
			}
		}
	} else {
		return E_INVALIDARG;
	}
	
	int32_t id = CreateObjectId();
	std::tr1::shared_ptr<VariableInfo> vInfo = InitVariableInfo(id);
	vInfo->pin = pin;
	vInfo->mode = mode;
	vInfo->pud = pud;

	// PinSetting
	// Set RPI2 to be an input
    bcm2835_gpio_fsel(vInfo->pin, vInfo->mode);
	if(mode == BCM2835_GPIO_FSEL_INPT){
		// with a pullup off
		bcm2835_gpio_set_pud(vInfo->pin, vInfo->pud);
	}

	VariantClear(vntRet);
	vntRet->vt = VT_I4;
	vntRet->lVal = id;

#ifdef LOG_OUTPUT
	fprintf(stdout, "ControllerGetVariable: %i, Pin : %i, Mode : %i, PUD : %i\n", id, vInfo->pin, vInfo->mode, vInfo->pud);
#endif
	return S_OK;
}

HRESULT VariableGetValue(VARIANT *vntArgs, int16_t Argc, VARIANT *vntRet) {
#ifdef LOG_OUTPUT
	time_t t = time(NULL);
	fprintf(stdout, "%ld : VariableGetValue enter.\n", t);
#endif

	GPIOVariant vntId;
	
	HRESULT hr = GetObjectId(vntArgs, Argc, vntId);
	if (FAILED(hr)) {
		VariantClear(vntRet);
		return hr;
	}

#ifdef TEST
	vntRet->vt = VT_I4;
	vntRet->lVal = 0;
#else

	std::tr1::shared_ptr<VariableInfo> vInfo = GetVariableInfo(vntId.lVal);
	if (vInfo == NULL) {
		return E_INVALIDARG;
	}

	VariantClear(vntRet);
	hr = ReadPinValue(vInfo, vntRet);
#endif

#ifdef LOG_OUTPUT
	fprintf(stdout, "VariableGetValue: %i, Value : %i\n", vntId.lVal, vntRet->boolVal);
#endif
	return hr;
}

HRESULT VariablePutValue(VARIANT *vntArgs, int16_t Argc, VARIANT *vntRet) {
	GPIOVariant vntId;
	
	HRESULT hr = GetObjectId(vntArgs, Argc, vntId);
	if (FAILED(hr)) {
		VariantClear(vntRet);
		return hr;
	}

	std::tr1::shared_ptr<VariableInfo> vInfo = GetVariableInfo(vntId.lVal);
	if (vInfo == NULL) {
		VariantClear(vntRet);
		return E_INVALIDARG;
	}

	hr = WritePinValue(vInfo, vntRet);

#ifdef LOG_OUTPUT
	fprintf(stdout, "VariablePutValue: %i, value : %i\n", vntId.lVal, vntRet->boolVal);
#endif
	return hr;
}

HRESULT VariableRelease(VARIANT *vntArgs, int16_t Argc, VARIANT *vntRet) {
	GPIOVariant vntId;

	HRESULT hr = GetObjectId(vntArgs, Argc, vntId);
	if (SUCCEEDED(hr)) {
		RemoveVariableInfo(vntId.lVal);
#ifdef LOG_OUTPUT
		fprintf(stdout, "VariableRelease: %i\n", vntId.lVal);
#endif
	}
	return S_OK;
}

HRESULT InitBcap(int port) {
    if (!bcm2835_init()) return E_FAIL; 

	running = true;
	InitGpioPinMap();
	InitFunctionSelectMap();
	InitPudControlMap();

	bCap_SetCallFunc(ID_SERVICE_START, ServiceStart);
	bCap_SetCallFunc(ID_SERVICE_STOP, ServiceStop);
	bCap_SetCallFunc(ID_CONTROLLER_CONNECT, ControllerConnect);
	bCap_SetCallFunc(ID_CONTROLLER_DISCONNECT, ControllerDisconnect);

	bCap_SetCallFunc(ID_CONTROLLER_GETVARIABLE, ControllerGetVariable);
	bCap_SetCallFunc(ID_CONTROLLER_GETVARIABLENAMES, ControllerGetVariableNames);
	bCap_SetCallFunc(ID_VARIABLE_RELEASE, VariableRelease);
	bCap_SetCallFunc(ID_VARIABLE_GETVALUE, VariableGetValue);
	bCap_SetCallFunc(ID_VARIABLE_PUTVALUE, VariablePutValue);
	
	std::stringstream ss;
	ss << "tcp:127.0.0.1:0:0.0.0.0:" << port;
	std::string conStr = ss.str();

	HRESULT hr = bCap_Open_Server(conStr.c_str(), INIT_EXEC_TIMEOUT, &fd);
	return hr;
}

void InitGpioPinMap() {
	GPIO_PIN_MAP.clear();

	GPIO_PIN_MAP[L"@RPI_GPIO_P1_03"]        =  0;  /*!< Version 1, Pin P1-03 */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_05"]        =  1;  /*!< Version 1, Pin P1-05 */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_07"]        =  4;  /*!< Version 1, Pin P1-07 */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_08"]        = 14;  /*!< Version 1, Pin P1-08, defaults to alt function 0 UART0_TXD */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_10"]        = 15;  /*!< Version 1, Pin P1-10, defaults to alt function 0 UART0_RXD */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_11"]        = 17;  /*!< Version 1, Pin P1-11 */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_12"]        = 18;  /*!< Version 1, Pin P1-12, can be PWM channel 0 in ALT FUN 5 */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_13"]        = 21;  /*!< Version 1, Pin P1-13 */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_15"]        = 22;  /*!< Version 1, Pin P1-15 */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_16"]        = 23;  /*!< Version 1, Pin P1-16 */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_18"]        = 24;  /*!< Version 1, Pin P1-18 */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_19"]        = 10;  /*!< Version 1, Pin P1-19, MOSI when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_21"]        =  9;  /*!< Version 1, Pin P1-21, MISO when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_22"]        = 25;  /*!< Version 1, Pin P1-22 */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_23"]        = 11;  /*!< Version 1, Pin P1-23, CLK when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_24"]        =  8;  /*!< Version 1, Pin P1-24, CE0 when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_GPIO_P1_26"]        =  7;  /*!< Version 1, Pin P1-26, CE1 when SPI0 in use */

    /* RPi Version 2 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_03"]     =  2;  /*!< Version 2, Pin P1-03 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_05"]     =  3;  /*!< Version 2, Pin P1-05 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_07"]     =  4;  /*!< Version 2, Pin P1-07 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_08"]     = 14;  /*!< Version 2, Pin P1-08, defaults to alt function 0 UART0_TXD */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_10"]     = 15;  /*!< Version 2, Pin P1-10, defaults to alt function 0 UART0_RXD */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_11"]     = 17;  /*!< Version 2, Pin P1-11 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_12"]     = 18;  /*!< Version 2, Pin P1-12, can be PWM channel 0 in ALT FUN 5 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_13"]     = 27;  /*!< Version 2, Pin P1-13 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_15"]     = 22;  /*!< Version 2, Pin P1-15 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_16"]     = 23;  /*!< Version 2, Pin P1-16 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_18"]     = 24;  /*!< Version 2, Pin P1-18 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_19"]     = 10;  /*!< Version 2, Pin P1-19, MOSI when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_21"]     =  9;  /*!< Version 2, Pin P1-21, MISO when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_22"]     = 25;  /*!< Version 2, Pin P1-22 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_23"]     = 11;  /*!< Version 2, Pin P1-23, CLK when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_24"]     =  8;  /*!< Version 2, Pin P1-24, CE0 when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_26"]     =  7;  /*!< Version 2, Pin P1-26, CE1 when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_29"]     =  5;  /*!< Version 2, Pin P1-29 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_31"]     =  6;  /*!< Version 2, Pin P1-31 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_32"]     = 12;  /*!< Version 2, Pin P1-32 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_33"]     = 13;  /*!< Version 2, Pin P1-33 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_35"]     = 19;  /*!< Version 2, Pin P1-35 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_36"]     = 16;  /*!< Version 2, Pin P1-36 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_37"]     = 26;  /*!< Version 2, Pin P1-37 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_38"]     = 20;  /*!< Version 2, Pin P1-38 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P1_40"]     = 21;  /*!< Version 2, Pin P1-40 */

    /* RPi Version 2, new plug P5 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P5_03"]     = 28;  /*!< Version 2, Pin P5-03 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P5_04"]     = 29;  /*!< Version 2, Pin P5-04 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P5_05"]     = 30;  /*!< Version 2, Pin P5-05 */
    GPIO_PIN_MAP[L"@RPI_V2_GPIO_P5_06"]     = 31;  /*!< Version 2, Pin P5-06 */

    /* RPi B+ J8 header, also RPi 2 40 pin GPIO header */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_03"]     =  2;  /*!< B+, Pin J8-03 */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_05"]     =  3;  /*!< B+, Pin J8-05 */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_07"]     =  4;  /*!< B+, Pin J8-07 */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_08"]     = 14;  /*!< B+, Pin J8-08, defaults to alt function 0 UART0_TXD */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_10"]     = 15;  /*!< B+, Pin J8-10, defaults to alt function 0 UART0_RXD */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_11"]     = 17;  /*!< B+, Pin J8-11 */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_12"]     = 18;  /*!< B+, Pin J8-12, can be PWM channel 0 in ALT FUN 5 */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_13"]     = 27;  /*!< B+, Pin J8-13 */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_15"]     = 22;  /*!< B+, Pin J8-15 */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_16"]     = 23;  /*!< B+, Pin J8-16 */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_18"]     = 24;  /*!< B+, Pin J8-18 */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_19"]     = 10;  /*!< B+, Pin J8-19, MOSI when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_21"]     =  9;  /*!< B+, Pin J8-21, MISO when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_22"]     = 25;  /*!< B+, Pin J8-22 */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_23"]     = 11;  /*!< B+, Pin J8-23, CLK when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_24"]     =  8;  /*!< B+, Pin J8-24, CE0 when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_26"]     =  7;  /*!< B+, Pin J8-26, CE1 when SPI0 in use */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_29"]     =  5;  /*!< B+, Pin J8-29,  */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_31"]     =  6;  /*!< B+, Pin J8-31,  */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_32"]     = 12;  /*!< B+, Pin J8-32,  */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_33"]     = 13;  /*!< B+, Pin J8-33,  */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_35"]     = 19;  /*!< B+, Pin J8-35,  */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_36"]     = 16;  /*!< B+, Pin J8-36,  */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_37"]     = 26;  /*!< B+, Pin J8-37,  */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_38"]     = 20;  /*!< B+, Pin J8-38,  */
    GPIO_PIN_MAP[L"@RPI_BPLUS_GPIO_J8_40"]     = 21;  /*!< B+, Pin J8-40,  */

	GPIO_PIN_MAP[L"@GPIO_00"]     =  0;
	GPIO_PIN_MAP[L"@GPIO_01"]     =  1;
	GPIO_PIN_MAP[L"@GPIO_02"]     =  2;
	GPIO_PIN_MAP[L"@GPIO_03"]     =  3;
	GPIO_PIN_MAP[L"@GPIO_04"]     =  4;
	GPIO_PIN_MAP[L"@GPIO_05"]     =  5;
	GPIO_PIN_MAP[L"@GPIO_06"]     =  6;
	GPIO_PIN_MAP[L"@GPIO_07"]     =  7;
	GPIO_PIN_MAP[L"@GPIO_08"]     =  8;
	GPIO_PIN_MAP[L"@GPIO_09"]     =  9;
	GPIO_PIN_MAP[L"@GPIO_10"]     =  10;
	GPIO_PIN_MAP[L"@GPIO_11"]     =  11;
	GPIO_PIN_MAP[L"@GPIO_12"]     =  12;
	GPIO_PIN_MAP[L"@GPIO_13"]     =  13;
	GPIO_PIN_MAP[L"@GPIO_14"]     =  14;
	GPIO_PIN_MAP[L"@GPIO_15"]     =  15;
	GPIO_PIN_MAP[L"@GPIO_16"]     =  16;
	GPIO_PIN_MAP[L"@GPIO_17"]     =  17;
	GPIO_PIN_MAP[L"@GPIO_18"]     =  18;
	GPIO_PIN_MAP[L"@GPIO_19"]     =  19;
	GPIO_PIN_MAP[L"@GPIO_20"]     =  20;
	GPIO_PIN_MAP[L"@GPIO_21"]     =  21;
	GPIO_PIN_MAP[L"@GPIO_22"]     =  22;
	GPIO_PIN_MAP[L"@GPIO_23"]     =  23;
	GPIO_PIN_MAP[L"@GPIO_24"]     =  24;
	GPIO_PIN_MAP[L"@GPIO_25"]     =  25;
	GPIO_PIN_MAP[L"@GPIO_26"]     =  26;
	GPIO_PIN_MAP[L"@GPIO_27"]     =  27;
	GPIO_PIN_MAP[L"@GPIO_28"]     =  28;
	GPIO_PIN_MAP[L"@GPIO_29"]     =  29;
	GPIO_PIN_MAP[L"@GPIO_30"]     =  30;
	GPIO_PIN_MAP[L"@GPIO_31"]     =  31;
}


void InitFunctionSelectMap() {
	FUNCTION_SELECT_MAP.clear();

	FUNCTION_SELECT_MAP[L"INPUT"]  = 0x00;   /*!< Input 0b000 */
    FUNCTION_SELECT_MAP[L"OUTPUT"]  = 0x01;   /*!< Output 0b001 */
    FUNCTION_SELECT_MAP[L"ALT0"]  = 0x04;   /*!< Alternate function 0 0b100 */
    FUNCTION_SELECT_MAP[L"ALT1"]  = 0x05;   /*!< Alternate function 1 0b101 */
    FUNCTION_SELECT_MAP[L"ALT2"]  = 0x06;   /*!< Alternate function 2 0b110, */
    FUNCTION_SELECT_MAP[L"ALT3"]  = 0x07;   /*!< Alternate function 3 0b111 */
    FUNCTION_SELECT_MAP[L"ALT4"]  = 0x03;   /*!< Alternate function 4 0b011 */
    FUNCTION_SELECT_MAP[L"ALT5"]  = 0x02;   /*!< Alternate function 5 0b010 */
    FUNCTION_SELECT_MAP[L"MASK"]  = 0x07;    /*!< Function select bits mask 0b111 */
}

void InitPudControlMap() {
	PUD_CONTROL_MAP.clear();

	PUD_CONTROL_MAP[L"OFF"]  = 0x00;   /*!< Off ? disable pull-up/down 0b00 */
    PUD_CONTROL_MAP[L"DOWN"]  = 0x01;   /*!< Enable Pull Down control 0b01 */
    PUD_CONTROL_MAP[L"UP"]  = 0x02;   /*!< Enable Pull Up control 0b10  */
}


HRESULT DisposeBcap() {
	HRESULT hr = bCap_Close_Server(&fd);
	bcm2835_close();

	running = false;
	return hr;
}
}