#include <iostream>

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

using namespace RoboidControl;

void CollisionSteering();

DifferentialDrive* bb2b = nullptr;
TouchSensor* touchLeft = nullptr;
TouchSensor* touchRight = nullptr;

int main() {
  bb2b = new DifferentialDrive();

  touchLeft = new TouchSensor();
  touchLeft->SetParent(bb2b);

  touchRight = new TouchSensor();
  touchRight->SetParent(bb2b);

  while (true) {
    CollisionSteering();
  }

  return 0;
}

void CollisionSteering() {
  if (touchLeft->touchedSomething)
    bb2b->SetWheelVelocity(bb2b->rightWheel, -0.5f);
  else
    bb2b->SetWheelVelocity(bb2b->rightWheel, 0.5f);

  if (touchRight->touchedSomething)
    bb2b->SetWheelVelocity(bb2b->leftWheel, -0.5f);
  else
    bb2b->SetWheelVelocity(bb2b->leftWheel, 0.5f);
}