#include "cstddef"
#include "cstdint"
#include "hFramework.h"
#include "hCloudClient.h"
#include "ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "MCP4725.h"

using namespace hFramework;
ros::NodeHandle nh;
MCP4725 dac;

void twistCallback(const geometry_msgs::Twist &twist)
{
	float gas = twist.linear.x;
	float nozzle = twist.linear.y;
	float bucket = twist.linear.z;
	platform.printf("%f [n]", gas);
	platform.ui.label("lb_gas").setText("%f [n]", gas);
	platform.ui.label("lb_nozzle").setText("%f [n]", nozzle);
	platform.ui.label("lb_bucket").setText("%f [n]", bucket);
	platform.printf("%f [N]", gas);
	platform.printf("test [N]");
	LED2.toggle();
}

void printfOnConsoleInWebIDE()
{
	for (;;) {
		platform.printf("asd %d\r\n", sys.getRefTime());
		sys.delay(1000);
	}
}

void cfgHandler()
{
	//uncomment if you want to stream video from your project using smartphone
	platform.ui.video.enableTransmission();
	platform.ui.loadHtml({ Resource::WEBIDE, "/ui.html" });
}

void onKeyEvent(KeyEventType type, KeyCode code)
{
	//press "up key" on your keyboard in your device UI
	if (code == KeyCode::Up) {
		if (type == KeyEventType::Pressed) {
			LED3.on();
		}
		else {
			LED3.off();
		}
	}
}

void onButtonEvent(hId id, ButtonEventType type)
{
	static int cnt = 0;
	if (id == "btn1") {
		UiButton b = platform.ui.button("btn1");
		if (type == ButtonEventType::Pressed) {
			b.setText("pressed %u", cnt++);
		}
		else {
			b.setText("released %u", cnt++);
		}
		LED1.toggle();
	}
}

void checkADC()
{
	hSens4.pin1.enableADC(); // enable ADC on pin1 on hSens4 port
	hExt.pin1.enableADC(); // enable ADC on pin1 on hExt port
	float val1 = hExt.pin3.analogReadVoltage(); // read analog value (voltage in [V])
	float val2 = hExt.pin4.analogReadVoltage(); // read analog value (voltage in [V])
	float val3 = hExt.pin5.analogReadVoltage(); // read analog value (voltage in [V])
	platform.ui.label("lb_adc0").setText("%f [V]", val1);
	platform.ui.label("lb_adc1").setText("%f [V]", val2);
	platform.ui.label("lb_adc2").setText("%f [V]", val3);
	//platform.printf("%f [V]\r\n", val3);
}

void setDAC(int value)
{
	dac.setVoltage(value);
}

//ros::Subscriber<geometry_msgs::Twist> sub("/cmd_vel", &twistCallback);

/*void hMain()
{
platform.begin(&RPi);
nh.getHardware()->initWithDevice(&platform.LocalSerial);
nh.initNode();
nh.subscribe(sub);
LED1.on();
while (true)
{
checkADC();
nh.spinOnce();
sys.delay(100);
}
}*/

void hMain()
{
	ros::Subscriber<geometry_msgs::Twist> sub("/mstx11/cmd_vel", &twistCallback);
	dac.begin(0x60);
	platform.begin(&RPi);
	nh.getHardware()->initWithDevice(&platform.LocalSerial);
	nh.initNode();
	nh.subscribe(sub);
	platform.ui.configHandler = cfgHandler;
	platform.ui.onKeyEvent = onKeyEvent;
	platform.ui.onButtonEvent = onButtonEvent;
	platform.ui.setProjectId("@@@PROJECT_ID@@@");

	//sys.setSysLogDev(&devNull); //turn off sys logs
	//sys.setLogDev(&Serial);     //default console setup - USB Serial
	//Serial.init(115200);        //default baudrate for USB Serial is 460800

	//sys.taskCreate(printfOnConsoleInWebIDE);
	for (;;) {
		//sys.delay(500); //Sign of life
		//printf("\r\nuptime %u", (unsigned int)sys.getRefTime()); //print on default console - USB Serial
		//platform.ui.label("lb_up").setText("uptime %u", (unsigned int)sys.getRefTime());
		//platform.ui.label("lb_bat").setText("%f [V]", sys.getSupplyVoltage());
		//LED2.toggle(); //Sign of life
		checkADC();
		setDAC(2000);
		nh.spinOnce();
		sys.delay(100);
	}
}