#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <ros/ros.h>
#include <string.h>

#include <std_srvs/Empty.h>
#include <naoqi_bridge_msgs/SpeechWithFeedbackActionGoal.h>
#include <actionlib_msgs/GoalStatusArray.h>
#include <naoqi_bridge_msgs/SetSpeechVocabularyActionGoal.h>
#include <naoqi_bridge_msgs/WordRecognized.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>

using namespace std;

class Nao_control {

protected:

    // ROS node handler
    ros::NodeHandle nh_;

    // Publisher for nao speech
    ros::Publisher speech_pub;

    // Publisher for nao vocabulary parameters
    ros::Publisher voc_params_pub;

    // Client for starting speech recognition
    ros::ServiceClient recog_start_srv;

    // Client for stopping speech recognition
    ros::ServiceClient recog_stop_srv;

    // Subscriber to speech recognition
    ros::Subscriber recog_sub;

    // Publisher for recognized commands
    ros::Publisher command_pub; 

    int speech_id_ctr = 1;
    std::vector<std::string> recognized_words;
    bool commandfound = false;

public:

    Nao_control(){

        ROS_INFO("Constructor");
 
        speech_pub = nh_.advertise<naoqi_bridge_msgs::SpeechWithFeedbackActionGoal>(
                "/speech_action/goal", 1);

        voc_params_pub= nh_.advertise<naoqi_bridge_msgs::SetSpeechVocabularyActionGoal>(
                "/speech_vocabulary_action/goal", 1);

        recog_start_srv=nh_.serviceClient<std_srvs::Empty>("/start_recognition");

        recog_stop_srv=nh_.serviceClient<std_srvs::Empty>("/stop_recognition");

        recog_sub=nh_.subscribe("/word_recognized", 1,
                &Nao_control::speechRecognitionCallback, this);

        command_pub = nh_.advertise<std_msgs::String>("/recieved_commands",5);
        
    }

    ~Nao_control() {

        ROS_INFO("Destructor");
        //stopping recognition

        std_srvs::Empty srv;
       
        if (recog_stop_srv.call(srv)) {
            ROS_INFO("SUCCESSFULLY STOPPED RECOGNITION");
        }
        else {
            ROS_ERROR("COULDN'T STOP RECOGNITION");
        }
    }

    void speechRecognitionCallback(const naoqi_bridge_msgs::WordRecognized::ConstPtr& msg)
    {

        ROS_INFO("A WORD WAS RECOGNIZED");
        std_srvs::Empty srv;

        ROS_INFO("Confidence Value:");
        std::cout << std::to_string(msg->confidence_values[0]) << std::endl;

        for (int i = 0; i < msg->words.size(); i++) {

            std::cout << msg->words[i] << std::endl;

        }
        
        //set pause duration
        double f_pause = 1;

        if (recog_stop_srv.call(srv) && ((msg->words.size())> 0)) {

            ROS_INFO("SUCCESSFULLY STOPPED RECOGNITION");
       
            // Use confidence level to decide wether the recognized word should be published
            
            if (msg->confidence_values[0] > 0.35) {

                ROS_INFO("SPEECH STARTING");
                std::string say = "Ok I understood";
            
                naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg;
                s_msg.goal_id.id = std::to_string(this->speech_id_ctr);
                this->speech_id_ctr += 1;
                s_msg.goal.say = say;
                this->speech_pub.publish(s_msg);
                this->recognized_words.clear();

                // publish here
                ROS_INFO("PUBLISH");

                std_msgs::String c_msg;
                c_msg.data = msg->words[0];
                this->command_pub.publish(c_msg);
                
            }
            else if(msg->confidence_values[0] > 0.05) {    
            
                ROS_INFO("SPEECH STARTING");
                std::string say = "I did not understand. Could you repeat that please";
            
                naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg;
                s_msg.goal_id.id = std::to_string(this->speech_id_ctr);
                this->speech_id_ctr += 1;
                s_msg.goal.say = say;
                this->speech_pub.publish(s_msg);
                this->recognized_words.clear();

                // increase pause duration
                f_pause = 0.4;
            
          }

            this->recognized_words.clear();
        }
        else {
            ROS_ERROR("COULDN'T STOP RECOGNITION");
        }    
        // pause until NAO stops talking
        ros::Rate loop_rate(f_pause);
        loop_rate.sleep();

        // re-start recogntion
        commandRecognition();    
    }

    void initializeVocabulary()
    {
        
        std::vector<std::string> vocabulary;
        vocabulary.push_back("Copy postion");
        vocabulary.push_back("Stop");// Operation ");
        //vocabulary.push_back("hello ");
        vocabulary.push_back("Open your hands");
        vocabulary.push_back("Close your hands");

        naoqi_bridge_msgs::SetSpeechVocabularyActionGoal msg;
        msg.goal.words = vocabulary;
        msg.goal_id.id = std::to_string(speech_id_ctr);
        std::cout << msg << std::endl;
        speech_id_ctr += 1;
        voc_params_pub.publish(msg);
        ROS_INFO("VOCABULARY INITIALIZED");
    }

    void commandRecognition()
    {
        //recognition has to be started and ended once a valid command was found
        
        this->initializeVocabulary();
        ros::Rate loop_rate(1);
        loop_rate.sleep();
        std_srvs::Empty srv;

        if (recog_start_srv.call(srv)) {
            ROS_INFO("SUCCESSFULLY STARTED RECOGNITION");
        }
        else {
            ROS_ERROR("COULDN'T START RECOGNITION");
         }    
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "speech");
    
    Nao_control TermiNAOtor;
    //TermiNAOtor.initializeVocabulary();
    ros::Rate loop_rate(1);
    loop_rate.sleep();
    TermiNAOtor.commandRecognition();
    ROS_INFO("SPIN");
    ros::spin();
    return 0;
}
