#include "Arduino.h"

#include "Things/ControlledMotor.h"
#include "Things/DifferentialDrive.h"
#include "Things/RelativeEncoder.h"
#include "Things/TouchSensor.h"

#include "Arduino/Things/DRV8833.h"
#include "Arduino/Things/DigitalInput.h"
#include "Arduino/Things/UltrasonicSensor.h"
#include "Arduino/ArduinoUtils.h"

#include "Participants/ParticipantUDP.h"

#include "configuration.h"

#include <iostream>

using namespace RoboidControl;
using namespace RoboidControl::Arduino;

ParticipantUDPGeneric* localParticipant;

DifferentialDrive* bb2b;
TouchSensor* touchLeft;
TouchSensor* touchRight;

// RelativeEncoder* encoderLeft;
// RelativeEncoder* encoderRight;

void setup() {
  Serial.begin(115200);
  delay(3000);
  Serial.println("started");

  StartWifi("serrarens", "192.168.76.44");
  localParticipant = new ParticipantUDPGeneric("192.168.77.76");

  bb2b = new DifferentialDrive();
  touchLeft = new TouchSensor(bb2b);
  touchRight = new TouchSensor(bb2b);
  
  // bb2b = new DRV8833::DifferentialDrive(driveConfig);
  // touchLeft = new UltrasonicSensor::TouchSensor(leftTouchConfig, bb2b);
  // touchRight = new UltrasonicSensor::TouchSensor(rightTouchConfig, bb2b);

  touchLeft->name = "Left Touch Sensor";
  touchLeft->SetPosition(Spherical::Degrees(0.15, -30, 0));
  touchRight->name = "Right Touch Sensor";
  touchRight->SetPosition(Spherical::Degrees(0.15, 30, 0));

  // encoderLeft = new DigitalInput::RelativeEncoder(leftEncoderConfig);
  // encoderRight = new DigitalInput::RelativeEncoder(rightEncoderConfig);

  // bb2b->leftWheel = new ControlledMotor(bb2b->leftWheel, encoderLeft, bb2b);
  // bb2b->rightWheel = new ControlledMotor(bb2b->rightWheel, encoderRight, bb2b);
}

void loop() {
  
  // std::cout << touchLeft->touchedSomething << " | "
  //           << touchRight->touchedSomething << std::endl;
  // std::cout << encoderLeft->rotationSpeed << " : "
  //           << encoderRight->rotationSpeed << std::endl;
  // std::cout << bb2b->leftWheel->encoder->rotationSpeed
  //           << " :: " << bb2b->rightWheel->encoder->rotationSpeed << std::endl;

  
  // 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 leftMotorVelocity = (touchRight->IsTouching()) ? -1.0f : 1.0f;
  // The right wheel does the same, but instead is controlled by
  // touches on the left side
  float rightMotorVelocity = (touchLeft->IsTouching()) ? -1.0f : 1.0f;
  // When both sides are touching something, both wheels will turn backward
  // and the roboid will move backwards

  bb2b->leftWheel->SetTargetVelocity(leftMotorVelocity);
  bb2b->rightWheel->SetTargetVelocity(rightMotorVelocity);
  // std::cout << "  " << bb2b->leftWheel->GetTargetVelocity() << " : " << bb2b->rightWheel->GetTargetVelocity() << std::endl;

  // float leftWheelVelocity = (touchRight->touchedSomething) ? -1.0f : 1.0f;
  // float rightWheelVelocity = (touchLeft->touchedSomething) ? -1.0f : 1.0f;
  // bb2b->SetWheelVelocity(leftWheelVelocity, rightWheelVelocity);
  // std::cout << "  " << leftWheelVelocity << " # " << rightWheelVelocity << std::endl;

  // std::cout << leftMotor->actualVelocity << std::endl;
  
  //Serial.println(".");
  // Update the roboid state
  localParticipant->Update();

  // and sleep for 100ms
  delay(10);
}