#include <SPI.h>
#include <Ticker.h>
#include <Wire.h>

#include <ESP8266WiFi.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <SparkFunSX1509.h>

#include <ArduinoJson.h>
#include <AsyncMqttClient.h> //https://github.com/marvinroger/async-mqtt-client/

/**
 * @brief Show information to the serial along with some metadata
 *
 * @param args Things to put, **NOT** separated by spaces.
 */
template <typename... Args>
auto log(const char *function, int line, const Args &...args) {
    Serial.printf("[%s:%04d] ", function, line);

    const auto print = [](const auto &thing) { Serial.print(thing); };

    (print(args), ...);

    Serial.println();
}

/**
 * @brief A macro to use for the `log` function.
 */
#define LOG(...) log(__FUNCTION__, __LINE__, ##__VA_ARGS__);

// i2c SCL
constexpr auto i2c_scl = D1;
// i2c SDL
constexpr auto i2c_sda = D2;

enum class ConnStatus { disconnected, connected, conneting };

void start_datashow_ticker();

/**
 * @brief A timer to use for sending periodic data to the server.
 */
// Ticker datapub_ticker;

/**
 * @brief Put toguether all of system related to wifi connection management
 * variables, functions are defined further down (we do have the prototypes
 * here).
 *
 * The name `wifiman` stands for 'wifi manager'.
 */
namespace wifiman {

/**
 * @brief The possible wifi status.
 */
using WifiStatus = ConnStatus;

/**
 * @brief Store the possible wifi status.
 */
WifiStatus wifi_status{WifiStatus::disconnected};

/**
 * @brief The SSID of the WiFi network to connect to.
 */
constexpr auto wifi_ssid = "Teddy";

/**
 * @brief The password of the WiFi network to connect to.
 */
constexpr auto wifi_password = "casamento";

/**
 * @brief This is a handler for storing our callback for when the connection to
 * the Wifi is achieved.
 */
WiFiEventHandler wifi_connected_handler;

/**
 * @brief This is a handler for storing our callback for when the connection to
 * the Wifi is lost.
 */
WiFiEventHandler wifi_disconnected_handler;

/**
 * @brief A timer to use for reconnecting to wifi.
 */
Ticker wifi_reconnect_ticker;

// Function prototypes

void stop_datashow_ticker();
void set_wifi_status(WifiStatus sts);
void wifi_connected_callback(const WiFiEventStationModeGotIP &event);
void wifi_disconnected_callback(const WiFiEventStationModeDisconnected &event);
void connect_to_wifi();
} // namespace wifiman

/**
 * @brief Put toguether everything that does with simple indicators (like the 2
 * builtin leds).
 */
namespace indicators {

/**
 * @brief The pin with the builtin led close to the antenna.
 */
constexpr auto indicator_led_0 = 2;

/**
 * @brief The pin with the builtin led close to the usb connector.
 */
constexpr auto indicator_led_1 = 16;

/**
 * @brief Ticker for blinking the indicator led 0.
 */
Ticker indicator_0_ticker;

/**
 * @brief Ticker for blinking the indicator led 1.
 */
Ticker indicator_1_ticker;

// function prototypes

void begin();
void blk_indicator_0();
void set_indicator_0(bool value);

void blk_indicator_1();
void set_indicator_1(bool value);
} // namespace indicators

/**
 * @brief Put toguether everything that does with the MQTT telemetry and control
 * system.
 */
namespace mqttman {

using MqttStatus = ConnStatus;

/**
 * @brief Flag for if we are connected to the MQTT server.
 */
MqttStatus mqtt_status = MqttStatus::disconnected;

/**
 * @brief The address of the MQTT server.
 *
 * NOTE: May be changed by the `connect_to_wifi` function and etc for diferent
 * configurations.
 */
IPAddress mqtt_server_address{192, 168, 0, 12};

/**
 * @brief The port of the MQTT server.
 *
 * NOTE: May be changed by the `connect_to_wifi` function and etc for diferent
 * configurations.
 */
constexpr int mqtt_server_port{1883};

/**
 * @brief The instance of the MQTT client.
 */
AsyncMqttClient mqtt_client;

/**
 * @brief This is a timer that we use to try to reconnect to the MQTT server.
 */
Ticker mqtt_reconnect_ticker;

/**
 * @brief Used to allow sending MQTT events.
 */
auto datashow_enabled = true;

// function prototypes

void send_wifidata();
void send_io_data();
void send_pilot_data();
void set_mqtt_status(MqttStatus sts);
void mqtt_connected_callback(bool session_present);
void mqtt_disconnected_callback(AsyncMqttClientDisconnectReason reason);
void mqtt_subscribe_callback(uint16_t packet_id, uint8_t qos);
void connect_to_server();
void begin();
} // namespace mqttman

/**
 * @brief Put toguether everything that has to do with the OLED display.
 */
namespace displayman {

/**
 * @brief Width of the diplay.
 */
constexpr auto display_width = 128;

/**
 * @brief Height of the display.
 */
constexpr auto display_height = 64;

/**
 * @brief The I2C address of the SSD1306 display.
 */
constexpr auto display_address = 0x3C;

/**
 * @brief Instance of our display.
 */
Adafruit_SSD1306 display{display_width, display_height};

/**
 * @brief Flag to detect when we need to clear the display for new information.
 */
auto display_data_did_change = true;

// function prototypes

void show_status_at(int x, int y, ConnStatus sts);
void show_wifi_status(ConnStatus sts);
void show_wifi_info();
void show_mqtt_status(ConnStatus sts);
void show_io_data();
void show_pilot_nav();
void show_pilot_state();
void clear();
void commit();
void begin();
} // namespace displayman

/**
 * @brief All Input/Output related stuff.
 */
namespace io {

/**
 * @brief The I2C address of the SX1509 IO expander.
 */
auto io_address = 0x3E;

/**
 * @brief The IO expander object.
 */
SX1509 io;

template <typename T>
class TimedRead {
public:
    /**
     * @brief Construct a new Timed Read object.
     */
    TimedRead(T &&inner_, u8 delay_)
        : inner{inner_}, delay_acc{0}, delay{delay_} {}

    /**
     * @brief Update delay timings.
     */
    void update() {
        if (inner.get()) {
            if (delay_acc < delay) delay_acc++;
        } else
            delay_acc = 0;
    }

    bool get() const { return delay_acc == delay; }

    T &get_inner() { return inner; }
    const T &get_inner() const { return inner; }

private:
    /**
     * @brief The inner sensor that we are delaying the read of.
     */
    T inner;

    /**
     * @brief Accumulated delay time.
     */
    u8 delay_acc;

    /**
     * @brief The delay we want to achieve.
     */
    u8 delay;
};

/**
 * @brief Abstraction over tcrt5000 infrared sensor connected to an SX1509 IO
 * expander.
 */
class InfraredSensor {
public:
    /**
     * @brief Construct with the pin of the SX1509 to use to read this from.
     */
    InfraredSensor(u8 p) : pin{p} {}

    /**
     * @brief Initialize the sensor.
     */
    void begin() { io.pinMode(pin, INPUT); }

    /**
     * @brief Read the digital value from GPIO and cache internally.
     *
     * @param io Used to read the value of the sensor from the IO expander.
     */
    void cache() { read(); }

    /**
     * @brief Read the last digital value stored in cache.
     */
    bool get() const { return last_read; }

    /**
     * @brief Read the digital value from GPIO and cache internally.
     *
     * @param io Used to read the value of the sensor from the IO expander.
     * @return The value of the IR sensor (detected line/not detected line).
     */
    bool read() { return last_read = io.digitalRead(pin); }

private:
    /**
     * @brief The pin on the SX1509 to read from.
     */
    u8 pin;

    /**
     * @brief Store the last read.
     */
    bool last_read{};
};

class RisingEdgeDetector {
public:
    RisingEdgeDetector(InfraredSensor s) : sensor{s} {}

    void begin() { sensor.begin(); }

    void cache() {
        const auto previous = sensor.get();
        sensor.cache();

        //          _______
        //    _____|      |____
        // p  00000011111110000
        // c  00000111111100000
        // e  00000100000000000

        if (!previous) {
            if (sensor.get()) edge = true;
        } else if (edge) {
            edge = false;
        }
    }

    bool get() const { return edge; }

private:
    InfraredSensor sensor;

    bool edge;
};

/**
 * @brief Abstraction over a pair of `InfraredSensors` in order to detect if the
 * vehicle should turn left, right or go forward to follow a line.
 */
class LineTracker {
public:
    /**
     * @brief Directions to follow.
     */
    enum class Direction : u8 {
        turn_left,
        turn_right,
        hard_left,
        hard_right,
        forward,
        both
    };

    /**
     * @brief Contruct from a pair of infrared sensors.
     */
    LineTracker(InfraredSensor l, InfraredSensor r, InfraredSensor el,
                InfraredSensor er)
        : left{l}, right{r}, edge_left{el}, edge_right{er} {}

    /**
     * @brief Initialize the sensors.
     */
    void begin() {
        left.begin();
        right.begin();
        edge_left.begin();
        edge_right.begin();
    }

    /**
     * @brief Update the cached values of the Infrared sensors.
     */
    void cache() {
        left.cache();
        right.cache();
        edge_left.cache();
        edge_right.cache();
    }

    /**
     * @brief Based on the values read from the two sensors, decide what
     * direction to go.
     */
    Direction get_direction() const {
        const auto l = is_left_detected();
        const auto r = is_right_detected();
        const auto el = is_edge_left_detected();
        const auto er = is_edge_right_detected();

        // if (el && er) return Direction::both;
        // if (el) return Direction::hard_left;
        // if (er) return Direction::hard_right;

        if (l && r) return Direction::both;
        if (l) return Direction::turn_left;
        if (r) return Direction::turn_right;

        return Direction::forward;
    }

    /**
     * @brief If the left sensor has detected a line.
     */
    bool is_left_detected() const { return left.get(); }

    /**
     * @brief If the right sensor has detected a line.
     */
    bool is_right_detected() const { return right.get(); }

    bool is_edge_left_detected() const { return edge_left.get(); }

    bool is_edge_right_detected() const { return edge_right.get(); }

private:
    /**
     * @brief The left infrared sensor.
     */
    InfraredSensor left;

    /**
     * @brief The right infrared sensor.
     */
    InfraredSensor right;

    InfraredSensor edge_left;
    InfraredSensor edge_right;
};

/**
 * @brief Abstraction over one DC motor.
 *
 */
class MotorDriver {
public:
    MotorDriver(u8 en, u8 dir1, u8 dir2)
        : enable_pin{en}, dir1_pin{dir1}, dir2_pin{dir2} {}

    /**
     * @brief Initialize the motor.
     *
     * @param io Reference to the SX1509 instance.
     */
    void begin() {
        io.pinMode(enable_pin, ANALOG_OUTPUT);
        io.pinMode(dir1_pin, OUTPUT);
        io.pinMode(dir2_pin, OUTPUT);
        set_speed(0);
        stop();
    }

    /**
     * @brief How fast is the motor going?
     */
    u8 get_speed() const { return speed; }

    /**
     * @brief Set speed to zero.
     *
     * @param io Reference to the SX1509 instance.
     */
    void stop() {
        io.digitalWrite(dir1_pin, LOW);
        io.digitalWrite(dir2_pin, LOW);
    }

    /**
     * @brief Spin the motor forward.
     *
     * @param io Reference to the SX1509 instance.
     */
    void forward() {
        io.digitalWrite(dir1_pin, HIGH);
        io.digitalWrite(dir2_pin, LOW);
    }

    /**
     * @brief Spin the motor backward.
     *
     * @param io Reference to the SX1509 instance.
     */
    void backward() {
        io.digitalWrite(dir1_pin, LOW);
        io.digitalWrite(dir2_pin, HIGH);
    }

    /**
     * @brief Set the speed of the motor.
     *
     * @param io Reference to the SX1509 instance.
     * @param s The new speed.
     */
    void set_speed(u8 s) {
        speed = s;
        // SX1509 actually uses sinking current PWM, so we need to invert the
        // values first (255 is 0% DC and 0 is 100% DC).
        io.analogWrite(enable_pin, 255 - speed);
    }

private:
    u8 enable_pin;
    u8 dir1_pin;
    u8 dir2_pin;
    u8 speed = 0;
};

/**
 * @brief Abstraction over two DC motors in tank steering mode.
 */
class TankDriver {
public:
    TankDriver(MotorDriver l, MotorDriver r, u8 two, u8 one, u8 spin)
        : left{l}, right{r}, when_two_power{two}, when_one_power{one},
          when_slow_power{spin} {}

    std::pair<u8, u8> get_power() const {
        return {left.get_speed(), right.get_speed()};
    }

    /**
     * @brief Inialize the motors.
     */
    void begin() {
        left.begin();
        right.begin();
    }

    /**
     * @brief Move both motors forward.
     */
    void forward() {
        set_two_power();

        left.forward();
        right.forward();
    }

    /**
     * @brief Stop both motors.
     */
    void stop() {
        left.stop();
        right.stop();
    }

    /**
     * @brief Stop the left motor and power the right one.
     */
    void full_left() {
        set_one_power();

        left.stop();
        right.forward();
    }

    /**
     * @brief Stop the right motor and power the left one.
     */
    void full_right() {
        set_one_power();

        left.forward();
        right.stop();
    }

    /**
     * @brief Slow down the left motor and power the right one.
     */
    void mid_left() {
        left.set_speed(80);
        right.set_speed(when_slow_power);

        left.forward();
        right.forward();
    }

    /**
     * @brief Slow down the right motor and power the left one.
     */
    void mid_right() {
        left.set_speed(when_slow_power);
        right.set_speed(80);

        left.forward();
        right.forward();
    }

    /**
     * @brief Move the left motor back and the right motor forward.
     */
    void spin_left() {
        set_two_power();

        left.backward();
        right.forward();
    }

    /**
     * @brief Move the right motor back and the left motor forward.
     */
    void spin_right() {
        set_two_power();

        left.forward();
        right.backward();
    }

private:
    /**
     * @brief Set both motors to the `when_one_power` speed.
     */
    void set_one_power() {
        left.set_speed(when_one_power);
        right.set_speed(when_one_power);
    }

    /**
     * @brief Set both motors to the `when_two_power` speed.
     */
    void set_two_power() {
        left.set_speed(when_two_power);
        right.set_speed(when_two_power);
    }

    /**
     * @brief Set both motors to the `when_slow_power` speed.
     */
    void set_spin_power() {
        left.set_speed(when_slow_power);
        right.set_speed(when_slow_power);
    }

private:
    /**
     * @brief Driver for the left motor.
     */
    MotorDriver left;

    /**
     * @brief Driver for the right motor.
     */
    MotorDriver right;

    /**
     * @brief PWM power for when both motors are running.
     */
    u8 when_two_power;

    /**
     * @brief PWM power for when one motor is running.
     */
    u8 when_one_power;

    /**
     * @brief PWM power for when both motors are running on oposite directions.
     */
    u8 when_slow_power;
};

// sensors
constexpr auto s0 = 8;
constexpr auto s1 = 9;
constexpr auto s2 = 10;
constexpr auto s3 = 11;
constexpr auto s4 = 12;
constexpr auto s5 = 13;

// motor left
constexpr auto motors_len = 7;
constexpr auto motors_ld1 = 5;
constexpr auto motors_ld2 = 6;

// motor right
constexpr auto motors_ren = 4;
constexpr auto motors_rd1 = 3;
constexpr auto motors_rd2 = 2;

// Line trackers
LineTracker center{s2, s3, s1, s4};

/**
 * @brief Detect left edge signals.
 */
RisingEdgeDetector deleft{s0};

/**
 * @brief Detect right edge signals.
 */
RisingEdgeDetector deright{s5};

// Driver to the 4 motors
TankDriver tank{{motors_len, motors_ld1, motors_ld2},
                {motors_ren, motors_rd1, motors_rd2},
                170,
                210,
                190};

// function prototypes

void begin();
const char *to_string(LineTracker::Direction dir);
} // namespace io

/**
 * @brief Everything about piloting the car in the line tracks.
 */
namespace pilot {

/**
 * @brief The main driving state.
 */
enum class MainState {
    stop,
    forward,
    turn_to_right,
    turn_to_left,
    spin_left,
    spin_right,
};

/**
 * @brief The navigation state.
 */
enum class NavigationState {
    ignore_next,
    turn_on_next,
    stop_on_next,
    reverse_on_next,
};

enum class TrackDirection {
    clockwise,
    counter_clockwise,
};

auto main_state = MainState::stop;

auto navigation_state = NavigationState::ignore_next;

auto track_direction = TrackDirection::clockwise;

int timeof_last_state_change{};

int track_part = 0;

// function prototypes

const char *to_string(MainState state);
const char *to_string(NavigationState state);

void set_main_state(MainState state);
void set_navigation_state(NavigationState state);
void set_track_direction(TrackDirection dir);
void reverse_track_direction();

void follow(io::LineTracker tracker);
void update();
void update_move();
void update_spin_left();
void next_track_part();
} // namespace pilot

// Here are the functions for `wifiman`.
namespace wifiman {

/**
 * @brief Stop the periodic timer for sending wifi information via MQTT.
 */
void stop_datashow_ticker() { mqttman::datashow_enabled = true; }

/**
 * @brief Set the value of the `wifi_status` flag.
 */
void set_wifi_status(WifiStatus sts) {
    // Mark that data changed
    displayman::display_data_did_change = true;

    wifi_status = sts;
}

/**
 * @brief A callback to be used to know when we have a Wifi connection.
 *
 * When we have a connection, we will try to connect to the MQTT server.
 */
void wifi_connected_callback(const WiFiEventStationModeGotIP &event) {
    LOG("Wifi connection achieved");
    LOG("local IP: ", WiFi.localIP());

    // Mark that we have connected
    set_wifi_status(WifiStatus::connected);

    // Show that we have connection via indicator 0
    indicators::set_indicator_0(true);

    // Connect to the MQTT server
    mqttman::connect_to_server();
}

/**
 * @brief A callback to be used to knwo when we have lost Wifi connection.
 */
void wifi_disconnected_callback(const WiFiEventStationModeDisconnected &event) {
    LOG("Wifi connection lost");

    // Stop driving
    pilot::set_main_state(pilot::MainState::stop);

    // Mark that we have disconnected
    set_wifi_status(WifiStatus::disconnected);

    // Dont try to reconnect MQTT while without Wifi
    mqttman::mqtt_reconnect_ticker.detach();

    // Try to reconnect to Wifi after 2 seconds
    wifi_reconnect_ticker.once(2, connect_to_wifi);

    // Show that we have lost connection via indicator 0
    indicators::set_indicator_0(false);
}

/**
 * @brief Try to connect to WiFi.
 */
void connect_to_wifi() {
    LOG("Connecting to wifi network '", wifi_ssid, "'");

    // Mark that we are trying to connect
    set_wifi_status(WifiStatus::conneting);

    indicators::blk_indicator_0();

    WiFi.begin(wifi_ssid, wifi_password);
}

/**
 * @brief Run initialization for WiFi connection.
 */
void begin() {
    // Setup some handlers for when we get and loose connection
    wifi_connected_handler = WiFi.onStationModeGotIP(wifi_connected_callback);
    wifi_disconnected_handler =
        WiFi.onStationModeDisconnected(wifi_disconnected_callback);
}
} // namespace wifiman

// Here are the functions for `indicators`
namespace indicators {

/**
 * @brief Initialize the indicator leds.
 */
void begin() {
    // LEDs are outputs (duh)
    pinMode(indicator_led_0, OUTPUT);
    pinMode(indicator_led_1, OUTPUT);

    // Start powered off
    set_indicator_0(false);
    set_indicator_1(false);
}

/**
 * @brief Start blinking the led indicator 0.
 */
void blk_indicator_0() {
    indicator_0_ticker.attach_ms(200, [] {
        digitalWrite(indicator_led_0, !digitalRead(indicator_led_0));
    });
}

/**
 * @brief Set the value indicator 0 led.
 *
 * @param value The value to set the led to.
 */
void set_indicator_0(bool value) {
    // Stop blinking
    indicator_0_ticker.detach();

    // _not_ the value because the LED is _active low_
    digitalWrite(indicator_led_0, !value);
}

/**
 * @brief Start blinking the led indicator 1.
 */
void blk_indicator_1() {
    indicator_1_ticker.attach_ms(200, [] {
        digitalWrite(indicator_led_1, !digitalRead(indicator_led_1));
    });
}

/**
 * @brief Set the value indicator 1 led.
 *
 * @param value The value to set the led to.
 */
void set_indicator_1(bool value) {
    // _not_ the value because the LED is _active low_
    digitalWrite(indicator_led_1, !value);
}
} // namespace indicators

// Here are the functions for `mqttman`
namespace mqttman {

/**
 * @brief Publish the wifi data.
 */
void send_wifidata() {
    StaticJsonDocument<128> doc;
    doc["id"] = 0;
    doc["ssid"] = WiFi.SSID();
    doc["rssi"] = WiFi.RSSI();

    static std::array<char, 256> buffer;
    const auto size = serializeJson(doc, buffer.data(), buffer.size());

    mqtt_client.publish("shelfbot/wifi", 0, false, buffer.data(), size);
}

void send_io_data() {
    const auto [ml, mr] = io::tank.get_power();

    StaticJsonDocument<512> doc;
    doc["id"] = 0;
    doc["ml"] = ml;
    doc["mr"] = mr;
    doc["dir"] = io::to_string(io::center.get_direction());
    doc["spot"] = pilot::track_part;
    doc["sens"][0] = io::deleft.get();
    doc["sens"][2] = io::deright.get();

    static std::array<char, 512> buffer;
    const auto size = serializeJson(doc, buffer.data(), buffer.size());

    mqtt_client.publish("shelfbot/io", 0, false, buffer.data(), size);
}

void send_pilot_data() {
    StaticJsonDocument<128> doc;
    doc["id"] = 0;
    doc["main_state"] = pilot::to_string(pilot::main_state);
    doc["nav_state"] = pilot::to_string(pilot::navigation_state);

    static std::array<char, 256> buffer;
    const auto size = serializeJson(doc, buffer.data(), buffer.size());

    mqtt_client.publish("shelfbot/pilot", 0, false, buffer.data(), size);
}

/**
 * @brief Set the status of the `mqtt_status` flag.
 */
void set_mqtt_status(MqttStatus sts) {
    // Mark that data changed
    displayman::display_data_did_change = true;

    mqtt_status = sts;
}

/**
 * @brief Callback for when the MQTT client manages to connect to the server
 *
 * @param session_present IDK what is this.
 */
void mqtt_connected_callback(bool session_present) {
    LOG("Connected to MQTT server (sp=", session_present, ")");

    // Signal that we have connection
    indicators::set_indicator_1(true);

    set_mqtt_status(MqttStatus::connected);

    // Start the wifi send data timer
    start_datashow_ticker();

    // subscribe to things and stuff
    const auto pid = mqtt_client.subscribe("shelfbot/control", 2);
    LOG("subscribed with QoS 2: pid=", pid);
}

/**
 * @brief Callback for when we loose connection to the MQTT server.
 *
 * @param reason The reason why we lost connection (I think).
 */
void mqtt_disconnected_callback(AsyncMqttClientDisconnectReason reason) {
    LOG("Disconnected from MQTT server (reason=", static_cast<uint8_t>(reason),
        ")");

    // Stop driving
    pilot::set_main_state(pilot::MainState::stop);

    // Signal that we have no connection
    indicators::set_indicator_1(false);

    // Stop sending data
    wifiman::stop_datashow_ticker();

    set_mqtt_status(MqttStatus::disconnected);

    // We want to try to reconnect if we have wifi
    if (WiFi.isConnected()) mqtt_reconnect_ticker.once(2, connect_to_server);
}

/**
 * @brief Callback from when we get an ACK for subscribing.
 */
void mqtt_subscribe_callback(uint16_t packet_id, uint8_t qos) {
    LOG("Subscribe ACK (pi=", packet_id, ", qos=", qos, ")");
}

/**
 * @brief Callback from when we get an ACK for unsubscribing.
 */
void mqtt_unsubscribe_callback(uint16_t packet_id) {
    LOG("Unsubscribe ACK (pi=", packet_id, ")");
}

/**
 * @brief Callback for when we receive an MQTT message.
 *
 * @param topic
 * @param payload
 * @param properties
 * @param len
 * @param index
 * @param total
 */
void mqtt_message_callback(const char *topic, const char *payload,
                           AsyncMqttClientMessageProperties properties,
                           size_t len, size_t index, size_t total) {
    LOG("Message: topic='", topic, "', qos=", properties.qos,
        ", dup=", properties.dup, ", retain=", properties.retain, ", len=", len,
        ", index=", index, ", total=", total);

    if (strcmp(topic, "shelfbot/control") == 0) {
        StaticJsonDocument<128> doc;

        deserializeJson(doc, payload, len);
        const auto id = doc["id"].as<int>();
        const auto nav = doc["nav"].as<String>();

        LOG("control: id=", id, ", nav=", nav);

        if (nav == "stop")
            pilot::set_main_state(pilot::MainState::stop);
        else if (nav == "forward")
            pilot::set_main_state(pilot::MainState::forward);
        else if (nav == "turn_next")
            pilot::set_navigation_state(pilot::NavigationState::turn_on_next);
        else if (nav == "stop_next")
            pilot::set_navigation_state(pilot::NavigationState::stop_on_next);
        else if (nav == "reverse_next")
            pilot::set_navigation_state(
                pilot::NavigationState::reverse_on_next);
        else if (nav == "spin_left")
            pilot::set_main_state(pilot::MainState::spin_left);
        else
            LOG("Invalid nav: '", nav, "'");
    }
}

/**
 * @brief Callback from when we get an ACK for publishing.
 *
 * @param packet_id
 */
void mqtt_publish_callback(uint16_t packet_id) {
    LOG("Publish ACK (pi=", packet_id, ")");
}

/**
 * @brief Try to connect to the MQTT server.
 */
void connect_to_server() {
    LOG("Connecting to MQTT server at ", mqtt_server_address, ":",
        mqtt_server_port);

    set_mqtt_status(MqttStatus::conneting);

    mqtt_client.connect();
}

/**
 * @brief Initialize the MQTT client with needed callbacks.
 */
void begin() {
    mqtt_client.onConnect(mqtt_connected_callback);
    mqtt_client.onDisconnect(mqtt_disconnected_callback);
    mqtt_client.onSubscribe(mqtt_subscribe_callback);
    mqtt_client.onUnsubscribe(mqtt_unsubscribe_callback);
    mqtt_client.onMessage(mqtt_message_callback);
    mqtt_client.onPublish(mqtt_publish_callback);

    mqtt_client.setServer(mqtt_server_address, mqtt_server_port);
}
} // namespace mqttman

// Here are the functions for `displayman`
namespace displayman {

/**
 * @brief Show the diferent possible wifi status on the display.
 */
void show_status_at(int x, int y, ConnStatus sts) {
    display.setCursor(x, y);

    switch (sts) {
    case ConnStatus::disconnected: display.print("D"); break;
    case ConnStatus::connected: display.print("C"); break;
    case ConnStatus::conneting: display.print("/"); break;
    }
}

/**
 * @brief Show the diferent possible wifi status on the display.
 */
void show_wifi_status(ConnStatus sts) { show_status_at(0, 0, sts); }

/**
 * @brief Show Wifi ip address.
 */
void show_wifi_info() {
    display.setCursor(0, 8);
    display.print("IP: ");
    display.print(WiFi.localIP());

    display.setCursor(0, 16);
    display.print("RSSI: ");
    display.print(WiFi.RSSI());
    display.print("dBm");
}

/**
 * @brief Show the diferent possible wifi status on the display.
 */
void show_mqtt_status(ConnStatus sts) { show_status_at(8, 0, sts); }

/**
 * @brief Show data from IO.
 */
void show_io_data() {
    display.setCursor(0, 40);
    display.printf("Detect: [%d %d]", io::deleft.get(), io::deright.get());
}

/**
 * @brief Show the diferent possible tracking lanes on the display.
 */
void show_pilot_nav() {
    display.setCursor(0, 24);
    display.printf("Nav [%d]:  ", pilot::track_part);
    display.print(to_string(pilot::navigation_state));
}

/**
 * @brief Show the diferent possible pilot states on the display.
 */
void show_pilot_state() {
    display.setCursor(0, 32);
    display.print("State: ");
    display.print(to_string(pilot::main_state));
}

/**
 * @brief Clear all of the pixels of the display.
 */
void clear() { display.clearDisplay(); }

/**
 * @brief Actually show the contents of the backbuffer on the display.
 */
void commit() { display.display(); }

/**
 * @brief Initialize the display, in case this fails, restart until possible.
 */
void begin() {
    LOG("Initialize display");

    // Initialize the display
    if (!display.begin(SSD1306_SWITCHCAPVCC, display_address)) {
        LOG("Failed to initialize display.");

        // Restart after 3 seconds
        delay(3000);
        ESP.restart();
        delay(5000);
    }

    // Show default contents (the adafruit logo)
    commit();

    // Some configs for fonts
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.cp437(true);
}
} // namespace displayman

namespace io {

const char *to_string(LineTracker::Direction dir) {
    switch (dir) {
    case LineTracker::Direction::turn_left: return "turn_left";
    case LineTracker::Direction::turn_right: return "turn_right";
    case LineTracker::Direction::hard_left: return "hard_left";
    case LineTracker::Direction::hard_right: return "hard_right";
    case LineTracker::Direction::forward: return "forward";
    case LineTracker::Direction::both: return "both";
    default: return "Direction?";
    }
}

/**
 * @brief Initialize Input/Output.
 */
void begin() {
    LOG("Initialize IO");

    // Initialize the IO expander
    if (!io.begin(io_address)) {
        LOG("Failed to initialize IO");

        // Restart after 3 seconds
        delay(3000);
        ESP.restart();
        delay(5000);
    }

    // Initialize motors
    tank.begin();

    // Initialize sensors
    center.begin();
    deleft.begin();
    deright.begin();
}
} // namespace io

// Here are the functions for `pilot`
namespace pilot {

/**
 * @brief Convert the `MainState` enum to a string.
 */
const char *to_string(MainState state) {
    switch (state) {
    case MainState::stop: return "stop";
    case MainState::forward: return "forward";
    case MainState::turn_to_right: return "turn_to_right";
    case MainState::turn_to_left: return "turn_to_left";
    case MainState::spin_left: return "spin_left";
    case MainState::spin_right: return "spin_right";
    }

    return "<err>";
}

/**
 * @brief Convert the `NavigationState` enum to a string.
 */
const char *to_string(NavigationState state) {
    switch (state) {
    case NavigationState::ignore_next: return "ignore_next";
    case NavigationState::turn_on_next: return "turn_on_next";
    case NavigationState::stop_on_next: return "stop_on_next";
    case NavigationState::reverse_on_next: return "reverse_on_next";
    }

    return "<err>";
}

/**
 * @brief Set the main state of the pilot machine.
 */
void set_main_state(MainState state) {
    // Mark that data changed
    displayman::display_data_did_change = true;
    main_state = state;

    timeof_last_state_change = millis();
}

/**
 * @brief Set the navigation state of the pilot machine.
 */
void set_navigation_state(NavigationState state) {
    // Mark that data changed
    displayman::display_data_did_change = true;
    navigation_state = state;
}

/**
 * @brief Set the direction of the pilot machine.
 */
void set_track_direction(TrackDirection dir) {
    // Mark that data changed
    displayman::display_data_did_change = true;
    track_direction = dir;
}

void reverse_track_direction() {
    // Mark that data changed
    displayman::display_data_did_change = true;
    track_direction = track_direction == TrackDirection::clockwise
                          ? TrackDirection::counter_clockwise
                          : TrackDirection::clockwise;
}

/**
 * @brief Follow the line wrapped by `tracker`.
 */
void follow(io::LineTracker tracker) {
    using Direction = io::LineTracker::Direction;

    switch (tracker.get_direction()) {
    case Direction::turn_left: io::tank.full_left(); break;
    case Direction::turn_right: io::tank.full_right(); break;
    case Direction::hard_left: io::tank.spin_left(); break;
    case Direction::hard_right: io::tank.spin_right(); break;
    case Direction::forward: io::tank.forward(); break;
    case Direction::both: io::tank.stop(); break;
    }
}

/**
 * @brief Update the internal logic to follow the line.
 */
void update() {
    // Cache the values of all sensors
    io::center.cache();
    io::deleft.cache();
    io::deright.cache();

    switch (main_state) {
    case MainState::stop: {
        // Stop everything
        io::tank.stop();
    } break;
    case MainState::forward: {

        update_move();
    } break;
    case MainState::turn_to_left: {
        // Turn left until we detect the line again
        io::tank.full_left();
        if (millis() - timeof_last_state_change > 200) io::tank.mid_left();
        if (io::center.is_left_detected()) {
            set_main_state(MainState::forward);
        }
    } break;
    case MainState::turn_to_right: {
        // Turn right until we detect the line again
        io::tank.full_right();
        if (millis() - timeof_last_state_change > 200) io::tank.mid_right();
        if (io::center.is_right_detected()) {
            set_main_state(MainState::forward);
        }
    } break;
    case MainState::spin_left: {
        update_spin_left();
    } break;
    case MainState::spin_right: {
        // update_spin_left();
        set_main_state(MainState::stop);
    } break;
    }
}

/**
 * @brief Logic for `State::move`, when we follow a line.
 */
void update_move() {
    // Follow the center line
    follow(io::center);

    if (io::deleft.get() || io::deright.get()) {
        next_track_part();

        if (navigation_state == NavigationState::turn_on_next) {
            set_navigation_state(NavigationState::ignore_next);

            if (io::deleft.get())
                set_main_state(MainState::turn_to_left);
            else
                set_main_state(MainState::turn_to_right);
        } else if (navigation_state == NavigationState::stop_on_next) {
            set_navigation_state(NavigationState::ignore_next);
            set_main_state(MainState::stop);
        } else if (navigation_state == NavigationState::reverse_on_next) {
            set_navigation_state(NavigationState::ignore_next);
            set_main_state(MainState::spin_left);
            reverse_track_direction();
        }
    }
}

void update_spin_left() {
    if (io::center.is_left_detected()) {
        // Stop when we find the main line again
        set_main_state(MainState::stop);
        return;
    }

    io::tank.spin_left();
}

/**
 * @brief Increment or decrement the `track_part` depending on the
 * `TrackDirection`.
 */
void next_track_part() {
    if (track_direction == TrackDirection::clockwise)
        track_part++;
    else
        track_part--;
}
} // namespace pilot

/**
 * @brief Start the periodic timer that will send wifi information via
 * MQTT.
 */
void start_datashow_ticker() { mqttman::datashow_enabled = true; }

/**
 * @brief Arduino setup.
 */
void setup() {
    // Initialize serial
    Serial.begin(115200);

    // Some padding on top of the terminal
    Serial.println();
    Serial.println();

    // Initialize I2C
    Wire.begin(i2c_sda, i2c_scl);

    LOG("Starting system");

    // Initialize indicator leds
    indicators::begin();

    // Initialize the OLED display
    displayman::begin();
    displayman::clear();

    // Initialize the IO expander
    io::begin();

    // Initialize and connect to Wifi
    wifiman::begin();
    wifiman::connect_to_wifi();

    // Initialize MQTT client
    mqttman::begin();

    LOG("setup ended");
}

int pilot_last_update{};
int datashow_last_send{};

/**
 * @brief Arduino loop.
 */
void loop() {
    // Clear the display if needed
    if (displayman::display_data_did_change) {
        displayman::display_data_did_change = false;

        // LOG("Display data changed, updating");

        displayman::clear();

        // Show data
        displayman::show_wifi_status(wifiman::wifi_status);
        displayman::show_wifi_info();
        displayman::show_mqtt_status(mqttman::mqtt_status);
        displayman::show_pilot_nav();
        displayman::show_pilot_state();
        displayman::show_io_data();

        // Present
        displayman::commit();
    }

    if (millis() - pilot_last_update > 15) {
        pilot_last_update = millis();

        // Drive!
        pilot::update();

        displayman::display_data_did_change = true;
    }

    if (millis() - datashow_last_send > 500) {
        datashow_last_send = millis();

        // MQTT publish
        mqttman::send_wifidata();
        mqttman::send_pilot_data();
        mqttman::send_io_data();
    }
}