/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <limits>
#include <vector>

#include <gtest/gtest.h>
#include <robot/rate.h>
#include <robot/time.h>

#if !defined(_WIN32)
#include <sys/time.h>
#endif

// Boost tests removed in std-only build

using namespace robot;

/// \todo All the tests in here that use randomized values are not unit tests, replace them

double epsilon = 1e-9;

void seed_rand()
{
  //Seed random number generator with current microseond count
#if !defined(_WIN32)
  timeval temp_time_struct;
  gettimeofday(&temp_time_struct,NULL);
  srand(temp_time_struct.tv_usec);
#else
  srand(time(nullptr));
#endif
};

void generate_rand_times(uint32_t range, uint64_t runs, std::vector<robot::Time>& values1, std::vector<robot::Time>& values2)
{
  seed_rand();
  values1.clear();
  values2.clear();
  values1.reserve(runs);
  values2.reserve(runs);
  for ( uint32_t i = 0; i < runs ; i++ )
  {
    values1.push_back(robot::Time( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
    values2.push_back(robot::Time( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
  }
}

void generate_rand_durations(uint32_t range, uint64_t runs, std::vector<robot::Duration>& values1, std::vector<robot::Duration>& values2)
{
  seed_rand();
  values1.clear();
  values2.clear();
  values1.reserve(runs * 4);
  values2.reserve(runs * 4);
  for ( uint32_t i = 0; i < runs ; i++ )
  {
    // positive durations
    values1.push_back(robot::Duration(  (rand() * range / RAND_MAX),  (rand() * 1000000000ULL/RAND_MAX)));
    values2.push_back(robot::Duration(  (rand() * range / RAND_MAX),  (rand() * 1000000000ULL/RAND_MAX)));
    EXPECT_GE(values1.back(), robot::Duration(0,0));
    EXPECT_GE(values2.back(), robot::Duration(0,0));

    // negative durations
    values1.push_back(robot::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
    values2.push_back(robot::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
    EXPECT_LE(values1.back(), robot::Duration(0,0));
    EXPECT_LE(values2.back(), robot::Duration(0,0));

    // positive and negative durations
    values1.push_back(robot::Duration(  (rand() * range / RAND_MAX),  (rand() * 1000000000ULL/RAND_MAX)));
    values2.push_back(robot::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
    EXPECT_GE(values1.back(), robot::Duration(0,0));
    EXPECT_LE(values2.back(), robot::Duration(0,0));

    // negative and positive durations
    values1.push_back(robot::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
    values2.push_back(robot::Duration(  (rand() * range / RAND_MAX),  (rand() * 1000000000ULL/RAND_MAX)));
    EXPECT_LE(values1.back(), robot::Duration(0,0));
    EXPECT_GE(values2.back(), robot::Duration(0,0));
  }
}

TEST(Time, size)
{
  ASSERT_EQ(sizeof(Time), 8);
  ASSERT_EQ(sizeof(Duration), 8);
}

TEST(Time, Comparitors)
{
  std::vector<robot::Time> v1;
  std::vector<robot::Time> v2;
  generate_rand_times(100, 1000, v1,v2);

  for (uint32_t i = 0; i < v1.size(); i++)
  {
    if (v1[i].sec * 1000000000ULL + v1[i].nsec < v2[i].sec * 1000000000ULL + v2[i].nsec)
    {
      EXPECT_LT(v1[i], v2[i]);
      //      printf("%f %d ", v1[i].toSec(), v1[i].sec * 1000000000ULL + v1[i].nsec);
      //printf("vs %f %d\n", v2[i].toSec(), v2[i].sec * 1000000000ULL + v2[i].nsec);
      EXPECT_LE(v1[i], v2[i]);
      EXPECT_NE(v1[i], v2[i]);
    }
    else if (v1[i].sec * 1000000000ULL + v1[i].nsec > v2[i].sec * 1000000000ULL + v2[i].nsec)
    {
      EXPECT_GT(v1[i], v2[i]);
      EXPECT_GE(v1[i], v2[i]);
      EXPECT_NE(v1[i], v2[i]);
    }
    else
    {
      EXPECT_EQ(v1[i], v2[i]);
      EXPECT_LE(v1[i], v2[i]);
      EXPECT_GE(v1[i], v2[i]);
    }

  }

}

TEST(Time, ToFromDouble)
{
  std::vector<robot::Time> v1;
  std::vector<robot::Time> v2;
  generate_rand_times(100, 1000, v1,v2);

  for (uint32_t i = 0; i < v1.size(); i++)
  {
    EXPECT_EQ(v1[i].toSec(), v1[i].fromSec(v1[i].toSec()).toSec());

  }

}

TEST(Time, RoundingError)
{
  double someInt = 1031.0; // some integer
  double t = std::nextafter(someInt, 0); // someint - epsilon
  // t should be 1031.000000

  robot::Time exampleTime;
  exampleTime.fromSec(t);

  // if rounded incorrectly, sec may be 1030
  // and nsec may be 1e9.
  EXPECT_EQ(exampleTime.sec, 1031);
  EXPECT_EQ(exampleTime.nsec, 0);
}

TEST(Time, OperatorPlus)
{
  Time t(100, 0);
  Duration d(100, 0);
  Time r = t + d;
  EXPECT_EQ(r.sec, 200UL);
  EXPECT_EQ(r.nsec, 0UL);

  t = Time(0, 100000UL);
  d = Duration(0, 100UL);
  r = t + d;
  EXPECT_EQ(r.sec, 0UL);
  EXPECT_EQ(r.nsec, 100100UL);

  t = Time(0, 0);
  d = Duration(10, 2000003000UL);
  r = t + d;
  EXPECT_EQ(r.sec, 12UL);
  EXPECT_EQ(r.nsec, 3000UL);
}

TEST(Time, OperatorMinus)
{
  Time t(100, 0);
  Duration d(100, 0);
  Time r = t - d;
  EXPECT_EQ(r.sec, 0UL);
  EXPECT_EQ(r.nsec, 0UL);

  t = Time(0, 100000UL);
  d = Duration(0, 100UL);
  r = t - d;
  EXPECT_EQ(r.sec, 0UL);
  EXPECT_EQ(r.nsec, 99900UL);

  t = Time(30, 0);
  d = Duration(10, 2000003000UL);
  r = t - d;
  EXPECT_EQ(r.sec, 17UL);
  EXPECT_EQ(r.nsec, 999997000ULL);
}

TEST(Time, OperatorPlusEquals)
{
  Time t(100, 0);
  Duration d(100, 0);
  t += d;
  EXPECT_EQ(t.sec, 200UL);
  EXPECT_EQ(t.nsec, 0UL);

  t = Time(0, 100000UL);
  d = Duration(0, 100UL);
  t += d;
  EXPECT_EQ(t.sec, 0UL);
  EXPECT_EQ(t.nsec, 100100UL);

  t = Time(0, 0);
  d = Duration(10, 2000003000UL);
  t += d;
  EXPECT_EQ(t.sec, 12UL);
  EXPECT_EQ(t.nsec, 3000UL);
}

TEST(Time, OperatorMinusEquals)
{
  Time t(100, 0);
  Duration d(100, 0);
  t -= d;
  EXPECT_EQ(t.sec, 0UL);
  EXPECT_EQ(t.nsec, 0UL);

  t = Time(0, 100000UL);
  d = Duration(0, 100UL);
  t -= d;
  EXPECT_EQ(t.sec, 0UL);
  EXPECT_EQ(t.nsec, 99900UL);

  t = Time(30, 0);
  d = Duration(10, 2000003000UL);
  t -= d;
  EXPECT_EQ(t.sec, 17UL);
  EXPECT_EQ(t.nsec, 999997000ULL);
}

TEST(Time, SecNSecConstructor)
{
  Time t(100, 2000003000UL);
  EXPECT_EQ(t.sec, 102UL);
  EXPECT_EQ(t.nsec, 3000UL);
}

TEST(Time, DontMungeStreamState)
{
  std::ostringstream oss;
  Time t(100, 2000003000UL);
  oss << std::setfill('N');
  oss << std::setw(13);
  oss << t;

  EXPECT_EQ(oss.width(), 13);
  EXPECT_EQ(oss.fill(), 'N');
}

// TEST(Time, ToFromBoost) removed

TEST(Time, CastFromDoubleExceptions)
{
  robot::Time::init();

  Time t1, t2, t3, t4, t5, t6, t7, t8;
  // Valid values to cast, must not throw exceptions
  EXPECT_NO_THROW(t1.fromSec(4294967295.0));
  EXPECT_NO_THROW(t2.fromSec(4294967295.999));
  EXPECT_NO_THROW(t3.fromSec(0.0000001));

  // The next casts all incorrect.
  EXPECT_THROW(t1.fromSec(4294967296.0), std::runtime_error);
  EXPECT_THROW(t2.fromSec(-0.0001), std::runtime_error);
  EXPECT_THROW(t3.fromSec(-4294967296.0), std::runtime_error);
  EXPECT_THROW(t4.fromSec(std::numeric_limits<double>::infinity()), std::runtime_error);
  EXPECT_THROW(t5.fromSec(-std::numeric_limits<double>::infinity()), std::runtime_error);
  EXPECT_THROW(t6.fromSec(std::numeric_limits<double>::quiet_NaN()), std::runtime_error);
  // max int64 value is 9223372036854775807
  EXPECT_THROW(t7.fromSec(9223372036854775808.0), std::runtime_error);
  EXPECT_THROW(t8.fromSec(-9223372036854775809.0), std::runtime_error);
}

TEST(Time, OperatorMinusExceptions)
{
  robot::Time::init();

  Time t1(2147483648, 0);
  Time t2(2147483647, 999999999);
  Time t3(2147483647, 999999998);
  Time t4(4294967295, 999999999);
  Time t5(4294967295, 999999998);
  Time t6(0, 1);

  Duration d1(2147483647, 999999999);
  Duration d2(-2147483648, 0);
  Duration d3(-2147483648, 1);
  Duration d4(0, 1);

  EXPECT_NO_THROW(t1 - t2);
  EXPECT_NO_THROW(t3 - t2);
  EXPECT_NO_THROW(t4 - t5);

  EXPECT_NO_THROW(t1 - d1);
  EXPECT_NO_THROW(t5 - d1);

  EXPECT_THROW(t4 - t6, std::runtime_error);
  EXPECT_THROW(t4 - t3, std::runtime_error);

  EXPECT_THROW(t1 - d2, std::runtime_error);
  EXPECT_THROW(t2 - d2, std::runtime_error);
  EXPECT_THROW(t4 - d3, std::runtime_error);
}

TEST(Time, OperatorPlusExceptions)
{
  robot::Time::init();

  Time t1(2147483648, 0);
  Time t2(2147483647, 999999999);
  Time t4(4294967295, 999999999);
  Time t5(4294967295, 999999998);

  Duration d1(2147483647, 999999999);
  Duration d2(-2147483648, 1);
  Duration d3(0, 2);
  Duration d4(0, 1);

  EXPECT_NO_THROW(t2 + d2);
  EXPECT_NO_THROW(t1 + d1);

  EXPECT_THROW(t4 + d4, std::runtime_error);
  EXPECT_THROW(t4 + d1, std::runtime_error);
  EXPECT_THROW(t5 + d3, std::runtime_error);
}

TEST(Time, Constants)
{
  EXPECT_EQ(Time::MAX.sec, static_cast<uint32_t>(-1));
  EXPECT_EQ(Time::MAX.nsec, 999999999);
  EXPECT_EQ(Time::MIN.sec, 0);
  EXPECT_EQ(Time::MIN.nsec, 1);
  EXPECT_EQ(Time::ZERO.sec, 0);
  EXPECT_EQ(Time::ZERO.nsec, 0);
  EXPECT_EQ(Time::UNINITIALIZED.sec, 0);
  EXPECT_EQ(Time::UNINITIALIZED.nsec, 0);

  EXPECT_EQ(WallTime::MAX.sec, static_cast<uint32_t>(-1));
  EXPECT_EQ(WallTime::MAX.nsec, 999999999);
  EXPECT_EQ(WallTime::MIN.sec, 0);
  EXPECT_EQ(WallTime::MIN.nsec, 1);
  EXPECT_EQ(WallTime::ZERO.sec, 0);
  EXPECT_EQ(WallTime::ZERO.nsec, 0);
  EXPECT_EQ(WallTime::UNINITIALIZED.sec, 0);
  EXPECT_EQ(WallTime::UNINITIALIZED.nsec, 0);

  EXPECT_EQ(SteadyTime::MAX.sec, static_cast<uint32_t>(-1));
  EXPECT_EQ(SteadyTime::MAX.nsec, 999999999);
  EXPECT_EQ(SteadyTime::MIN.sec, 0);
  EXPECT_EQ(SteadyTime::MIN.nsec, 1);
  EXPECT_EQ(SteadyTime::ZERO.sec, 0);
  EXPECT_EQ(SteadyTime::ZERO.nsec, 0);
  EXPECT_EQ(SteadyTime::UNINITIALIZED.sec, 0);
  EXPECT_EQ(SteadyTime::UNINITIALIZED.nsec, 0);
}

/************************************* Duration Tests *****************/

TEST(Duration, Comparitors)
{
  std::vector<robot::Duration> v1;
  std::vector<robot::Duration> v2;
  generate_rand_durations(100, 1000, v1,v2);

  for (uint32_t i = 0; i < v1.size(); i++)
  {
    if (v1[i].sec * 1000000000LL + v1[i].nsec < v2[i].sec * 1000000000LL + v2[i].nsec)
    {
      EXPECT_LT(v1[i], v2[i]);
//      printf("%f %lld ", v1[i].toSec(), v1[i].sec * 1000000000LL + v1[i].nsec);
//      printf("vs %f %lld\n", v2[i].toSec(), v2[i].sec * 1000000000LL + v2[i].nsec);
      EXPECT_LE(v1[i], v2[i]);
      EXPECT_NE(v1[i], v2[i]);
    }
    else if (v1[i].sec * 1000000000LL + v1[i].nsec > v2[i].sec * 1000000000LL + v2[i].nsec)
    {
      EXPECT_GT(v1[i], v2[i]);
//      printf("%f %lld ", v1[i].toSec(), v1[i].sec * 1000000000LL + v1[i].nsec);
//      printf("vs %f %lld\n", v2[i].toSec(), v2[i].sec * 1000000000LL + v2[i].nsec);
      EXPECT_GE(v1[i], v2[i]);
      EXPECT_NE(v1[i], v2[i]);
    }
    else
    {
      EXPECT_EQ(v1[i], v2[i]);
      EXPECT_LE(v1[i], v2[i]);
      EXPECT_GE(v1[i], v2[i]);
    }

  }
}

TEST(Duration, ToFromSec)
{
  std::vector<robot::Duration> v1;
  std::vector<robot::Duration> v2;
  generate_rand_durations(100, 1000, v1,v2);

  for (uint32_t i = 0; i < v1.size(); i++)
  {
    EXPECT_EQ(v1[i].toSec(), v1[i].fromSec(v1[i].toSec()).toSec());
    EXPECT_GE(robot::Duration(v1[i].toSec()).nsec, 0);
  }

  EXPECT_EQ(robot::Duration(-0.5), robot::Duration(-1LL, 500000000LL));
  EXPECT_EQ(robot::Duration(-0.5), robot::Duration(0, -500000000LL));
}

TEST(Duration, FromNSec)
{
  robot::Duration t;
  t.fromNSec(-500000000LL);
  EXPECT_EQ(robot::Duration(-0.5), t);

  t.fromNSec(-1500000000LL);
  EXPECT_EQ(robot::Duration(-1.5), t);

  t.fromNSec(500000000LL);
  EXPECT_EQ(robot::Duration(0.5), t);

  t.fromNSec(1500000000LL);
  EXPECT_EQ(robot::Duration(1.5), t);
}

TEST(Duration, OperatorPlus)
{
  std::vector<robot::Duration> v1;
  std::vector<robot::Duration> v2;
  generate_rand_durations(100, 1000, v1,v2);

  for (uint32_t i = 0; i < v1.size(); i++)
  {
    EXPECT_NEAR(v1[i].toSec() + v2[i].toSec(), (v1[i] + v2[i]).toSec(), epsilon);
    robot::Duration temp = v1[i];
    EXPECT_NEAR(v1[i].toSec() + v2[i].toSec(), (temp += v2[i]).toSec(), epsilon);

  }

}

TEST(Duration, OperatorMinus)
{
  std::vector<robot::Duration> v1;
  std::vector<robot::Duration> v2;
  generate_rand_durations(100, 1000, v1,v2);

  for (uint32_t i = 0; i < v1.size(); i++)
  {
    EXPECT_NEAR(v1[i].toSec() - v2[i].toSec(), (v1[i] - v2[i]).toSec(), epsilon);
    robot::Duration temp = v1[i];
    EXPECT_NEAR(v1[i].toSec() - v2[i].toSec(), (temp -= v2[i]).toSec(), epsilon);

    EXPECT_NEAR(- v2[i].toSec(), (-v2[i]).toSec(), epsilon);

  }

  robot::Time t1(1.1);
  robot::Time t2(1.3);
  robot::Duration time_diff = t1 - t2; //=-0.2

  EXPECT_NEAR(time_diff.toSec(), -0.2, epsilon);
  EXPECT_LE(time_diff, robot::Duration(-0.19));
  EXPECT_GE(time_diff, robot::Duration(-0.21));
}

TEST(Duration, OperatorTimes)
{
  std::vector<robot::Duration> v1;
  std::vector<robot::Duration> v2;
  generate_rand_durations(100, 1000, v1,v2);

  for (uint32_t i = 0; i < v1.size(); i++)
  {
    EXPECT_NEAR(v1[i].toSec() * v2[i].toSec(), (v1[i] * v2[i].toSec()).toSec(), epsilon);
    robot::Duration temp = v1[i];
    EXPECT_NEAR(v1[i].toSec() * v2[i].toSec(), (temp *= v2[i].toSec()).toSec(), epsilon);

  }

}

TEST(Duration, OperatorPlusEquals)
{
  Duration t(100, 0);
  Duration d(100, 0);
  t += d;
  EXPECT_EQ(t.sec, 200L);
  EXPECT_EQ(t.nsec, 0L);

  t = Duration(0, 100000L);
  d = Duration(0, 100L);
  t += d;
  EXPECT_EQ(t.sec, 0L);
  EXPECT_EQ(t.nsec, 100100L);

  t = Duration(0, 0);
  d = Duration(10, 2000003000L);
  t += d;
  EXPECT_EQ(t.sec, 12L);
  EXPECT_EQ(t.nsec, 3000L);
}

TEST(Duration, OperatorMinusEquals)
{
  Duration t(100, 0);
  Duration d(100, 0);
  t -= d;
  EXPECT_EQ(t.sec, 0L);
  EXPECT_EQ(t.nsec, 0L);

  t = Duration(0, 100000L);
  d = Duration(0, 100L);
  t -= d;
  EXPECT_EQ(t.sec, 0L);
  EXPECT_EQ(t.nsec, 99900L);

  t = Duration(30, 0);
  d = Duration(10, 2000003000L);
  t -= d;
  EXPECT_EQ(t.sec, 17L);
  EXPECT_EQ(t.nsec, 999997000L);
}

void alarmHandler(int sig)
{

}

TEST(Duration, sleepWithSignal)
{
#if !defined(_WIN32)
  signal(SIGALRM, alarmHandler);
  alarm(1);
#endif

  Time start = Time::now();
  Duration d(5.0);
  bool rc = d.sleep();
  // std::cout << "rc: " << rc << std::endl;
  Time end = Time::now();

  ASSERT_GT(end - start, d);
  ASSERT_TRUE(rc);
}

TEST(Duration, Constants)
{
  EXPECT_EQ(Duration::MAX.sec, std::numeric_limits<int32_t>::max());
  EXPECT_EQ(Duration::MAX.nsec, 999999999);
  EXPECT_EQ(Duration::MIN.sec, std::numeric_limits<int32_t>::min());
  EXPECT_EQ(Duration::MIN.nsec, 0);
  EXPECT_EQ(Duration::ZERO.sec, 0);
  EXPECT_EQ(Duration::ZERO.nsec, 0);
  EXPECT_EQ(Duration::NANOSECOND.sec, 0);
  EXPECT_EQ(Duration::NANOSECOND.nsec, 1);
  EXPECT_EQ(Duration::MICROSECOND.sec, 0);
  EXPECT_EQ(Duration::MICROSECOND.nsec, 1000);
  EXPECT_EQ(Duration::MILLISECOND.sec, 0);
  EXPECT_EQ(Duration::MILLISECOND.nsec, 1000000);
  EXPECT_EQ(Duration::SECOND.sec, 1);
  EXPECT_EQ(Duration::SECOND.nsec, 0);
  EXPECT_EQ(Duration::MINUTE.sec, 60);
  EXPECT_EQ(Duration::MINUTE.nsec, 0);
  EXPECT_EQ(Duration::HOUR.sec, 60 * 60);
  EXPECT_EQ(Duration::HOUR.nsec, 0);
  EXPECT_EQ(Duration::DAY.sec, 60 * 60 * 24);
  EXPECT_EQ(Duration::DAY.nsec, 0);

  EXPECT_EQ(WallDuration::MAX.sec, std::numeric_limits<int32_t>::max());
  EXPECT_EQ(WallDuration::MAX.nsec, 999999999);
  EXPECT_EQ(WallDuration::MIN.sec, std::numeric_limits<int32_t>::min());
  EXPECT_EQ(WallDuration::MIN.nsec, 0);
  EXPECT_EQ(WallDuration::ZERO.sec, 0);
  EXPECT_EQ(WallDuration::ZERO.nsec, 0);
  EXPECT_EQ(WallDuration::NANOSECOND.sec, 0);
  EXPECT_EQ(WallDuration::NANOSECOND.nsec, 1);
  EXPECT_EQ(WallDuration::MICROSECOND.sec, 0);
  EXPECT_EQ(WallDuration::MICROSECOND.nsec, 1000);
  EXPECT_EQ(WallDuration::MILLISECOND.sec, 0);
  EXPECT_EQ(WallDuration::MILLISECOND.nsec, 1000000);
  EXPECT_EQ(WallDuration::SECOND.sec, 1);
  EXPECT_EQ(WallDuration::SECOND.nsec, 0);
  EXPECT_EQ(WallDuration::MINUTE.sec, 60);
  EXPECT_EQ(WallDuration::MINUTE.nsec, 0);
  EXPECT_EQ(WallDuration::HOUR.sec, 60 * 60);
  EXPECT_EQ(WallDuration::HOUR.nsec, 0);
  EXPECT_EQ(WallDuration::DAY.sec, 60 * 60 * 24);
  EXPECT_EQ(WallDuration::DAY.nsec, 0);
}

TEST(Rate, constructFromDuration){
  Duration d(4, 0);
  Rate r(d);
  EXPECT_EQ(r.expectedCycleTime(), d);
}

TEST(Rate, constructFromDouble){
  Rate r(0.5);
  EXPECT_EQ(r.expectedCycleTime(), robot::Duration(2, 0));
  
  Rate r2(-0.5);
  EXPECT_EQ(r2.expectedCycleTime(), robot::Duration(-2, 0));
  
  Rate r3(std::numeric_limits<double>::infinity());
  EXPECT_EQ(r3.expectedCycleTime(), robot::Duration(0, 0));

  EXPECT_THROW(Rate(0.0), std::runtime_error);
}

TEST(Rate, sleep_return_value_true){
  Rate r(Duration(0.2));
  Duration(r.expectedCycleTime() * 0.5).sleep();
  EXPECT_TRUE(r.sleep());
}

TEST(Rate, sleep_return_value_false){
  Rate r(Duration(0.2));
  Duration(r.expectedCycleTime() * 2).sleep();
  EXPECT_FALSE(r.sleep());  // requested rate cannot be achieved
}

TEST(WallRate, constructFromDuration){
  Duration d(4, 0);
  WallRate r(d);
  WallDuration wd(4, 0);
  EXPECT_EQ(r.expectedCycleTime(), wd);
}

TEST(WallRate, constructFromDouble){
  WallRate r(0.5);
  EXPECT_EQ(r.expectedCycleTime(), robot::WallDuration(2, 0));

  WallRate r2(-0.5);
  EXPECT_EQ(r2.expectedCycleTime(), robot::WallDuration(-2, 0));

  WallRate r3(std::numeric_limits<double>::infinity());
  EXPECT_EQ(r3.expectedCycleTime(), robot::WallDuration(0, 0));

  EXPECT_THROW(WallRate(0.0), std::runtime_error);
}

///////////////////////////////////////////////////////////////////////////////////
// WallTime/WallDuration
///////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////
// SteadyTime/WallDuration
///////////////////////////////////////////////////////////////////////////////////

TEST(SteadyTime, sleep){
  SteadyTime start = SteadyTime::now();
  WallDuration d(2.0);
  bool rc = d.sleep();
  SteadyTime end = SteadyTime::now();

  ASSERT_GT(end - start, d);
  ASSERT_TRUE(rc);
}

TEST(SteadyTime, sleepUntil){
  SteadyTime start = SteadyTime::now();
  SteadyTime end = start + WallDuration(2.0);
  bool rc = SteadyTime::sleepUntil(end);
  SteadyTime finished = SteadyTime::now();

  ASSERT_GT(finished, end);
  ASSERT_TRUE(rc);
}

int main(int argc, char **argv){
  testing::InitGoogleTest(&argc, argv);
  robot::Time::init();
  return RUN_ALL_TESTS();
}