#include "Thing.h"
#include "Things/DifferentialDrive.h"
#include "Things/TouchSensor.h"

#include <chrono>
#include <thread>

using namespace RoboidControl;

int main() {
  // The robot's propulsion is a differential drive
  DifferentialDrive* bb2b = new DifferentialDrive();
  // Is has a touch sensor at the front left of the roboid
  TouchSensor* touchLeft = new TouchSensor(bb2b);
  // and other one on the right
  TouchSensor* touchRight = new TouchSensor(bb2b);

  // Do forever:
  while (true) {
    // The left wheel turns forward when nothing is touched on the right side
    // and turn backward when the roboid hits something on the right
    float leftWheelSpeed = (touchRight->touchedSomething) ? -0.5f : 0.5f;
    // The right wheel does the same, but instead is controlled by
    // touches on the left side
    float rightWheelSpeed = (touchLeft->touchedSomething) ? -0.5f : 0.5f;
    // When both sides are touching something, both wheels will turn backward
    // and the roboid will move backwards
    bb2b->SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);

    // Update the roboid state
    Thing::UpdateThings(Thing::GetTimeMs());
    // would like to do this:
    // bb2b->Update();

    // and sleep for 100ms
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }

  return 0;
}