#include <chrono>
#include <pico/stdlib.h>

void gpio_init_out(uint gpio) {
	gpio_set_dir(gpio, GPIO_OUT);
	gpio_put(gpio, 0);
	gpio_set_function(gpio, GPIO_FUNC_SIO);
}

void gpio_init_input(uint gpio) {
	gpio_set_dir(gpio, GPIO_IN);
	gpio_pull_up(gpio);
	gpio_set_function(gpio, GPIO_FUNC_SIO);
}

class Valve {
public:
	enum class State {
		Unknown,
		Opening,
		Opened,
		Closing,
		Closed
	};

	Valve(uint gpio_motor1, uint gpio_motor2, uint gpio_start, uint gpio_end)
		: motor1(gpio_motor1)
		, motor2(gpio_motor2)
		, start_pin(gpio_start)
		, end_pin(gpio_end) {
		gpio_init_out(motor1);
		gpio_init_out(motor2);
		gpio_init_input(start_pin);
		gpio_init_input(end_pin);
		start_time = std::chrono::steady_clock::now();
	}

	void open() {
		if (state == State::Opening) {
			return;
		}
		gpio_put(motor2, 0);
		gpio_put(motor1, 1);
		state      = State::Opening;
		start_time = std::chrono::steady_clock::now();
	}

	void close() {
		if (state == State::Closing) {
			return;
		}
		gpio_put(motor1, 0);
		gpio_put(motor2, 1);
		state      = State::Closing;
		start_time = std::chrono::steady_clock::now();
	}

	State process() {
		if ((state == State::Opening) && (gpio_get(start_pin) == 0)) {
			gpio_put(motor1, 0);
			state = State::Opened;
		}
		if ((state == State::Closing) && (gpio_get(end_pin) == 0)) {
			gpio_put(motor2, 0);
			state = State::Closed;
		}
		if (state == State::Opening || state == State::Closing) {
			using namespace std::chrono;
			if (duration_cast<seconds>(steady_clock::now() - start_time) > timeout) {
				gpio_put(motor1, 0);
				gpio_put(motor2, 0);
				state = State::Unknown;
			}
		}
		return state;
	}

	State getState() const { return state; }

private:
	uint motor1, motor2;
	uint start_pin, end_pin;
	State state                  = State::Unknown;
	std::chrono::seconds timeout = std::chrono::seconds(10);
	decltype(std::chrono::steady_clock::now()) start_time;
};

int main() {
	Valve cold_valve{10, 11, 12, 13};
	Valve hot_valve{21, 20, 19, 18};

	const int button_open  = 14;
	const int button_close = 15;
	gpio_init_out(PICO_DEFAULT_LED_PIN);
	gpio_init_input(button_open);
	gpio_init_input(button_close);

	const int tick_ms = 50;
	while (true) {
		if (gpio_get(button_open) == 0) {
			cold_valve.open();
			hot_valve.open();
		}
		if (gpio_get(button_close) == 0) {
			cold_valve.close();
			hot_valve.close();
		}
		sleep_ms(tick_ms);
		const auto cst = cold_valve.process();
		hot_valve.process();
		if (cst == Valve::State::Opened || cst == Valve::State::Closed) {
			gpio_put(PICO_DEFAULT_LED_PIN, 1);
		} else {
			gpio_put(PICO_DEFAULT_LED_PIN, 0);
		}
	}
	return 0;
}