#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Float64MultiArray.h"
#include <iostream>
#include <chrono>

using namespace std;

int main(int argc, char **argv)
{
//clock_t clock_time;
cout << "GO" << endl;
ros::init(argc, argv, "producer");
ros::NodeHandle n;

ros::Publisher producer_pub = n.advertise<std_msgs::Float64MultiArray>("producer", 10);

ros::Rate loop_rate(1);

std_msgs::Int32 msg;

    std_msgs::Float64MultiArray array_msg;
    size_t count = 6400;
    array_msg.data.resize(count);
    for (size_t i = 0; i < count; i++)
    {
      float r2 = static_cast <float> (rand()) / (static_cast <float> (RAND_MAX/3.40282e+038));//maximum float value = 3.40282e+038
      array_msg.data[i]= r2;
    }
    
int counter = 0;

while (ros::ok())
  {
//msg.data = counter;

//chrono::system_clock::time_point time_point;
//time_point = chrono::system_clock::now();
//std::time_t now_c = std::chrono::system_clock::to_time_t(time_point);
//    cout << "Time is : " << now_c << setprecision(9); 
//    cout << " sec" << endl; 
// cout << "Now sending : " << counter << endl;



// Record start time
auto start = std::chrono::high_resolution_clock::now();

producer_pub.publish(array_msg);
ROS_INFO("Sending message: [%i]", counter);
ros::spinOnce();
loop_rate.sleep();
counter++;

// Record end time
auto finish = std::chrono::high_resolution_clock::now();

std::chrono::duration<double> elapsed = finish - start;
std::cout << "Elapsed time: " << elapsed.count() << setprecision(10) << " s\n";

/*
std::chrono::seconds s (1);             // 1 second
std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (s);

ms += std::chrono::milliseconds(2500);  // 2500 millisecond

s = std::chrono::duration_cast<std::chrono::seconds> (ms);   // truncated

  std::cout << "ms: " << ms.count() << std::endl;
  std::cout << "s: " << s.count() << std::endl;
*/

// Calculating total time taken by the program. 
  //cout << chrono::duration_cast<std::chrono::seconds>(finish - start).count(); 
//  double time_taken =  chrono::duration_cast<std::chrono::nanoseconds>(finish - start).count(); 
//  time_taken *= 1e-9; 
  
//    cout << "Time taken by program is : " << fixed  
//         << time_taken << setprecision(9); 
//    cout << " sec" << endl; 

  }//end while

return 0;
}//end main

/* EQUIVALENT PYTHON PROGRAM *********************************
import rospy
from std_msgs.msg import Int32
if __name__ == '__main__':
    rospy.init_node("counter_publisher")
    rate = rospy.Rate(5)
    pub = rospy.Publisher("/counter", Int32, queue_size=10)
    counter = 0
    rospy.loginfo("Publisher has been started.")
    while not rospy.is_shutdown():
        counter += 1
        msg = Int32()
        msg.data = counter
        pub.publish(counter)
        rate.sleep()
*/