#if GTEST
#include <gtest/gtest.h>

#include "DifferentialDrive.h"
#include "DistanceSensor.h"
#include "Roboid.h"

TEST(BB2B, Basics) {
  Motor motorLeft = Motor();
  Motor motorRight = Motor();

  DistanceSensor sensorLeft = DistanceSensor();
  DistanceSensor sensorRight = DistanceSensor();

  Placement sensors[] = {Placement(&sensorLeft, -30),
                         Placement(&sensorRight, 30)};
  Perception *perception = new Perception(sensors);

  DifferentialDrive *propulsion =
      new DifferentialDrive(Placement(&motorLeft, Vector3(-1, 0, 0)),
                            Placement(&motorRight, Vector3(1, 0, 0)));

  Roboid *roboid = new Roboid(perception, propulsion);

  bool obstacleLeft = roboid->perception->ObjectNearby(-30);
  bool obstacleRight = roboid->perception->ObjectNearby(30);

  EXPECT_FALSE(obstacleLeft);
  EXPECT_FALSE(obstacleRight);

  float leftMotorSpeed = obstacleRight ? -1.0F : 1.0F;
  float rightMotorSpeed = obstacleLeft ? -1.0F : 1.0F;
}
#endif