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

#include <ros.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Duration.h>
//#include <geometry_msgs/Pose.h>
//#include <geometry_msgs/Quaternion.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;
//geometry_msgs::Pose current_pose;
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;
    //drawLCD();
}

/*void poseCb( const geometry_msgs::Pose& a_pose){
    current_pose = a_pose;
}*/

ros::Subscriber<std_msgs::UInt16> score_sub("score", &scoreCb);
ros::Subscriber<std_msgs::Duration> time_sub("remainingTime", &timeCb);
//ros::Subscriber<geometry_msgs::Pose> pose_sub("current_pose", poseCb);

/*int quaternionToEulerZ(geometry_msgs::Quaternion& q)
{
    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    return static_cast<int>(atan2(siny_cosp, cosy_cosp));
}*/

void drawLCD()
{
    char* toto = "tu";

    // Print a message to the LCD.
    lcd.setCursor(7,1);
    lcd.print("Score:");
    print_with_padding(current_score);
    //lcd.print(add_padding(current_score));
    //lcd.print(char_buffer);
    /*lcd.setCursor(0,0);
    lcd.print("X");
    lcd.print(current_pose.position.x);
    lcd.setCursor(6,0);
    lcd.print("Y");
    lcd.print(current_pose.position.y);
    lcd.setCursor(12,0);
    lcd.print("T");
    //lcd.print(quaternionToEulerZ(current_pose.orientation));*/
    lcd.setCursor(0,1);
    lcd.print("T");
    print_with_padding(remaining_time);
    lcd.print("s");
    /*add_padding(remaining_time, &char_buffer);
    lcd.print(char_buffer);*/
}

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 setup()
{ 
    current_score = 0;
    loopId = 255;
    remaining_time = 100;
    //current_pose = geometry_msgs::Pose();
    
    nh.initNode();
    nh.subscribe(score_sub);
    nh.subscribe(time_sub);
    //nh.subscribe(pose_sub);

    lcd.init();                      // initialize the lcd 
    lcd.backlight();
    lcd.setCursor(0,1);
    lcd.print("T");
}

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