/****************************************************************************
* Author : Victor Dubois
****************************************************************************/

#include <ros.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Duration.h>
#include <math.h>

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

ros::NodeHandle nh;
bool stopped = true;
uint16_t current_score;
uint16_t remaining_time;
int loopId = 0;

void timeCb( const std_msgs::Duration& a_remaining_time){
    remaining_time = a_remaining_time.data.toSec();
}

void scoreCb( const std_msgs::UInt16& score){
    current_score = score.data;
}

ros::Subscriber<std_msgs::UInt16> score_sub("score", &scoreCb);
ros::Subscriber<std_msgs::Duration> time_sub("remainingTime", &timeCb);

void drawLCD()
{
    // Print a message to the LCD.
    lcd.setCursor(7,1);
    lcd.print("Score:");
    print_with_padding(current_score);
    lcd.setCursor(0,1);
    lcd.print("T");
    print_with_padding(remaining_time);
    lcd.print("s");
}

void print_with_padding(uint16_t number)
{
    uint16_t hundreds = (number%1000 - number%100)/100;
    uint16_t units = number%10;
    uint16_t tens = (number%1000 - hundreds*100 - units)/10;
    if(hundreds == 0)
    {
        lcd.print(" ");
    }
    else
    {
        lcd.print(hundreds);
    };
    if(tens == 0 && hundreds == 0)
    {
        lcd.print(" ");
    }
    else
    {
        lcd.print(tens);
    };
    lcd.print(units);
}

void createCrab()
{
    byte Crab1[8] = {
        0b01000,
        0b11001,
        0b00110,
        0b01110,
        0b01110,
        0b00110,
        0b11001,
        0b01000
    };
    byte Crab2[8] = {
        0b01000,
        0b11001,
        0b00110,
        0b01111,
        0b01111,
        0b00110,
        0b11001,
        0b01000
    };
    lcd.createChar(0, Crab1);
    lcd.createChar(1, Crab2);
}

void setup()
{ 
    current_score = 0;
    loopId = 255;
    remaining_time = 100;
    
    nh.initNode();
    nh.subscribe(score_sub);
    nh.subscribe(time_sub);

    lcd.init();                      // initialize the lcd 
    lcd.backlight();
    createCrab();
    lcd.setCursor(0,0);
lcd.write(byte(0));
lcd.write(byte(0));
lcd.write(byte(0));
    
    lcd.setCursor(4,0);
    lcd.print("Kraboss");
    lcd.setCursor(13,0);
lcd.write(byte(0));
lcd.write(byte(1));
lcd.write(byte(0));
    
}

void loop()
{
    nh.spinOnce();
    if(loopId == 0)
    {
        loopId = 1024;
        drawLCD();
    }
    loopId--;
}