#include <iostream>
#include <fstream>
#include <termios.h>
#include <ros/ros.h>

using namespace std;

int getch()
{
  static struct termios oldt, newt;
  tcgetattr( STDIN_FILENO, &oldt);           // save old settings
  newt = oldt;
  newt.c_lflag &= ~(ICANON);                 // disable buffering
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

  int c = getchar();  // read character (non-blocking)

  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
  return c;
}


int main(int argc, char** argv)
{
    fstream plik;
    plik.open( "../catkin_aruco/src/aruco_test/dane.txt", ios::in );
    if( plik.good() )
    {
        string x,y,z,yaw,time;
        float x1,y1,z1,yaw1;
        unsigned int time1;
        while( !plik.eof() )
                {
            getline(plik,x,',');
            getline(plik,y,',');
            getline(plik,z,',');
            getline(plik,yaw,',');
            getline(plik,time,'\n');
            x1=stof(x);
            y1=stof(y);
            z1=stof(z);
            yaw1=stof(yaw);
            time1=stoi(time);
            cout<<x1<<" "<<y1<<" "<<z1<< " "<<yaw1<<" "<<time1<<endl;
            sleep(time1);
                }

        plik.close();
    } else cout << "Error! Nie udalo otworzyc sie pliku!" << endl;

    getch();
    return( 0 );
}