//YWROBOT
//Compatible with the Arduino IDE 1.0
//Library version:1.1
#include <ros.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <std_msgs/UInt32.h>

LiquidCrystal_I2C lcd(0x27,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display
ros::NodeHandle nh;


void scoreCb( const std_msgs::UInt32& score){
  lcd.setCursor(13,1);
  uint32_t hundreds = (score.data%1000 - score.data%100)/100;
  uint32_t units = score.data%10;
  uint32_t tens = (score.data%1000 - hundreds*100 - units)/10;
  lcd.print(hundreds);
  lcd.print(tens);
  lcd.print(units);
}

ros::Subscriber<std_msgs::UInt32> score_sub("score", &scoreCb);


void setup()
{
  
  lcd.init();                      // initialize the lcd 
  lcd.init();
  // Print a message to the LCD.
  lcd.backlight();
  lcd.setCursor(7,1);
    lcd.print("Score:");
  lcd.setCursor(0,0);
    lcd.print("X");
      lcd.setCursor(6,0);
    lcd.print("Y");
      lcd.setCursor(12,0);
    lcd.print("T");
      lcd.setCursor(0,1);
    lcd.print("T");
 
      nh.initNode();
      nh.subscribe(score_sub);

}


void loop()
{
  nh.spinOnce();
  delay(10);
}