#include <thread>
#include <chrono>
#include <sstream>
#include <iostream>

#include "engine.hpp"
#include "ultrasound_sensor.hpp"

using namespace std;

enum class drive_mode
{
	LEFT,
	RIGHT,
	FIND_TURN,
};

int main()
{
	engine right(
			gpio(13, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
			gpio(20, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
			);
	engine left(
			gpio(19, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
			gpio(26, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
			);
	gpio bwright(2, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
	gpio bw1(3, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
	gpio bw2(4, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
	gpio bwleft(17, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);

	ultrasound_sensor ultrasound(23, 24);

	left.set_speed(-50);
	right.set_speed(-50);
	while (ultrasound.get_value() > 200000)
	{
		cout << ultrasound.get_value() << endl;
		this_thread::sleep_for(500ms);
	}
	left.set_speed(0);
	right.set_speed(0);

/*	auto stop_time = chrono::high_resolution_clock::now() + 1min;
	drive_mode mode = drive_mode::LEFT;
	right.set_speed(50);
	left.set_speed(10);
	while (stop_time > chrono::high_resolution_clock::now())
	{
		int found_corners = bwright.get_value() + bwleft.get_value() + bw1.get_value() + bw2.get_value();
		if (mode == drive_mode::FIND_TURN)
		{
			if (!bwright.get_value())
			{
				right.set_speed(100);
				left.set_speed(0);
				mode = drive_mode::LEFT;
			}
			else if (!bwleft.get_value())
			{
				right.set_speed(0);
				left.set_speed(100);
				mode = drive_mode::RIGHT;
			}
		}
		else if (found_corners >= 3 || (bwright.get_value() && bwleft.get_value()))
		{
			left.set_speed(100);
			right.set_speed(100);
			mode = drive_mode::FIND_TURN;
		}
		else if (mode == drive_mode::LEFT)
		{
			if (bwright.get_value())
			{
				left.set_speed(100);
				right.set_speed(0);
				mode = drive_mode::RIGHT;
			}
		}
		else if (mode == drive_mode::RIGHT)
		{
			if (bwleft.get_value())
			{
				left.set_speed(0);
				right.set_speed(100);
				mode = drive_mode::LEFT;
			}
		}
		this_thread::sleep_for(1ms);
	}
	right.set_speed(0);
	left.set_speed(0);
*/
	return 0;
}