#include "hFramework.h"
#include "hCloudClient.h"
#include <stdio.h>
#include <iostream>
#include "ros.h"
#include "sensor_msgs/LaserScan.h"
#include "std_msgs/Int64.h"

using namespace hFramework;

bool batteryLow = false;
ros::NodeHandle nh;
std_msgs::Int64 counter;
ros::Publisher counter_pub("/counter", &counter);
int scan_cnt = 0;

void scanCallback(const sensor_msgs::LaserScan &scan)
{
	scan_cnt++;
	Serial.printf("scanCallback # %7d\r\n", scan_cnt);
	counter.data = scan_cnt;
	counter_pub.publish(&counter);
}

void batteryCheck()
{
	int i = 0;
	for (;;)
	{
		if (sys.getSupplyVoltage() > 11.1)
		{
			i--;
		}
		else
		{
			i++;
		}
		if (i > 50)
		{
			batteryLow = true;
			i = 50;
		}
		if (i < -50)
		{
			batteryLow = false;
			i = -50;
		}
		if (batteryLow == true)
		{
			LED1.toggle();
		}
		else
		{
			LED1.on();
		}
		sys.delay(250);
	}
}

ros::Subscriber<sensor_msgs::LaserScan> scan_sub("/scan_throttle", &scanCallback);

void hMain()
{
	platform.begin(&RPi);
	nh.getHardware()->initWithDevice(&platform.LocalSerial);
	nh.initNode();
	nh.subscribe(scan_sub);
    nh.advertise(counter_pub);
	LED1.on();
	sys.taskCreate(batteryCheck);
	sys.setSysLogDev(&devNull);
	Serial.init(115200);
	while (true)
	{
		nh.spinOnce();
		sys.delay(10);
	}
}