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

#include "../DifferentialDrive.h"
#include "../DistanceSensor.h"
#include "../Roboid.h"
#include "../VectorAlgebra/Angle.h"

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

  DistanceSensor *sensorLeft = new DistanceSensor();
  sensorLeft->position.angle = -30;
  DistanceSensor *sensorRight = new DistanceSensor();
  sensorRight->position.angle = 30;

  Sensor *sensors[] = {sensorLeft, sensorRight};

  Perception *perception = new Perception(sensors, 2);

  DifferentialDrive *propulsion = new DifferentialDrive(motorLeft, motorRight);
  propulsion->SetDimensions(
      1 / Angle::pi, // we use this, such that motor speed 1 -> velocity 1 m/s
      1);

  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;

  EXPECT_FLOAT_EQ(leftMotorSpeed, 1.0F);
  EXPECT_FLOAT_EQ(rightMotorSpeed, 1.0F);

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

  float leftActualSpeed = motorLeft->GetActualSpeed();
  float rightActualSpeed = motorRight->GetActualSpeed();

  EXPECT_FLOAT_EQ(leftActualSpeed, 1.0F);
  EXPECT_FLOAT_EQ(rightActualSpeed, 1.0F);

  Polar velocity =
      diffDrive->GetVelocity(); // this depends on the wheel diameter.
  EXPECT_FLOAT_EQ(velocity.distance, 1.0F);
  EXPECT_FLOAT_EQ(velocity.angle, 0.0F);
}
// #endif