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

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

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

  DistanceSensor *sensorLeft = new DistanceSensor();
  DistanceSensor *sensorRight = new DistanceSensor();

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

  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;

  DifferentialDrive *diffDrive = (DifferentialDrive *)roboid->propulsion;
  diffDrive->SetTargetSpeeds(leftMotorSpeed, rightMotorSpeed);

  // cannot chek verlocity in this branch?
}
#endif