#include "ros.h"
#include "hFramework.h"
#include "hCloudClient.h"

using namespace hFramework;
ros::NodeHandle nh;

void cloudTask()
{
	for (;;)
	{
	    platform.ui.label("Encoder A").setText("A: %d \n\t", hMotA.getEncoderCnt());
	    platform.ui.label("Encoder B").setText("B: %d \n\t", hMotB.getEncoderCnt());
	    platform.ui.label("Encoder C").setText("C: %d \n\t", hMotC.getEncoderCnt());
	    platform.ui.label("Encoder D").setText("D: %d \n\t", hMotD.getEncoderCnt());

		sys.delay(300);
	}
}

void hMain()
{
    RPi.init(500000);
    platform.begin(&RPi);
    nh.getHardware()->initWithDevice(&platform.LocalSerial);
    nh.initNode();
    
    uint32_t t = sys.getRefTime();
	sys.setLogDev(&Serial);
	
//	hMotA.resetEncoderCnt();
// 	hMotB.resetEncoderCnt();
// 	hMotC.resetEncoderCnt();
// 	hMotD.resetEncoderCnt();
    
    hMotA.setMotorPolarity(Polarity::Normal); 
 	hMotB.setMotorPolarity(Polarity::Normal);
 	hMotC.setMotorPolarity(Polarity::Normal);
 	hMotD.setMotorPolarity(Polarity::Normal);

	hMotA.setEncoderPolarity(Polarity::Normal); 
	hMotB.setEncoderPolarity(Polarity::Normal);
 	hMotC.setEncoderPolarity(Polarity::Normal);
	hMotD.setEncoderPolarity(Polarity::Normal);
	
	sys.taskCreate(cloudTask);

	while (1)
	{
		hMotA.setPower(300);
		sys.delay(1000);
		LED1.toggle();
// 		hMotA.setPower(-300);
// 		sys.delay(1000);
// 		LED1.toggle();
		
		hMotB.setPower(300);
		sys.delay(1000);
		LED1.toggle();
// 		hMotB.setPower(-300);
// 		sys.delay(1000);
// 		LED1.toggle();
		
		hMotC.setPower(300);
		sys.delay(1000);
		LED1.toggle();
// 		hMotC.setPower(-300);
// 		sys.delay(1000);
// 		LED1.toggle();
		
		hMotD.setPower(300);
		sys.delay(1000);
		LED1.toggle();
// 		hMotD.setPower(-300);
// 		sys.delay(1000);
// 		LED1.toggle();

        nh.spinOnce();
		sys.delaySync(t, 10);
		
	}
}