/*!
  \example sync_timestamp.cpp タイムスタンプの初期化サンプル

  \author Satofumi KAMIMURA

  $Id$
*/

#include "TimestampManager.h"
#include "BeegoDrive.h"
#include "UrgDevice.h"
#include "ticks.h"
#include <iostream>

using namespace qrk;
using namespace std;


namespace
{
    void printTimestamp(TimestampManager& timestamp_manager,
                        BeegoDrive& run, UrgDevice& urg)
    {
        cout << "run: " << timestamp_manager.timestamp(&run);
        cout << ", " << ticks() << endl;

        cout << "urg: " << timestamp_manager.timestamp(&urg);
        cout << ", " << ticks() << endl;

        cout << endl;
    }
}


int main(int argc, char *argv[])
{
    static_cast<void>(argc);
    static_cast<void>(argv);

    BeegoDrive run;
    if (! run.connect("/dev/ttyUSB0")) {
        cout << "BeegoDrive::connect: " << run.what() << endl;
        return 1;
    }

    UrgDevice urg;
    if (! urg.connect("/dev/ttyACM0")) {
        cout << "UrgDevice::connect: " << urg.what() << endl;
        return 1;
    }

    TimestampManager timestamp_manager;
    timestamp_manager.add(&run);
    timestamp_manager.add(&urg);

    printTimestamp(timestamp_manager, run, urg);
    timestamp_manager.setTimestamp(ticks());
    printTimestamp(timestamp_manager, run, urg);

    return 0;
}
