// 아두이노 우노 보드 기준이 아니라 아두이노 R4 보드 기준입니다.!!


#include "UltrasonicArray.h"

namespace
{
    struct SensorPins
    {
        uint8_t trigger;
        uint8_t echo;
    };

    constexpr SensorPins FRONT_LEFT_PINS{2U, 3U};
    constexpr SensorPins FRONT_RIGHT_PINS{4U, 5U};
    constexpr SensorPins REAR_LEFT_PINS{6U, 7U};
    constexpr SensorPins REAR_RIGHT_PINS{8U, 9U};
    constexpr unsigned long UPDATE_INTERVAL_MS = 1000UL;

    UltrasonicSensorManager sensor_manager;
    unsigned long last_update_ms = 0UL;
    unsigned long cycle_start_us = 0UL;
    unsigned long last_cycle_duration_us = 0UL;
    unsigned long previous_cycle_completed_ms = 0UL;
    unsigned long last_cycle_completed_ms = 0UL;
    unsigned long last_cycle_interval_ms = 0UL;
    bool cycle_running = false;

    void start_sensor_cycle()
    {
        cycle_start_us = micros();
        cycle_running = true;
        sensor_manager.start_cycle();
    }

    void record_cycle_completion(unsigned long now_ms)
    {
        if (!cycle_running)
        {
            return;
        }

        const unsigned long now_us = micros();
        last_cycle_duration_us = now_us - cycle_start_us;
        cycle_running = false;

        previous_cycle_completed_ms = last_cycle_completed_ms;
        last_cycle_completed_ms = now_ms;

        if (previous_cycle_completed_ms != 0UL)
        {
            last_cycle_interval_ms = last_cycle_completed_ms - previous_cycle_completed_ms;
        }
        else
        {
            last_cycle_interval_ms = 0UL;
        }
    }
}

void configure_sensors()
{
    sensor_manager.configure_sensor(SensorPosition::FrontLeft, FRONT_LEFT_PINS.trigger, FRONT_LEFT_PINS.echo);
    sensor_manager.configure_sensor(SensorPosition::FrontRight, FRONT_RIGHT_PINS.trigger, FRONT_RIGHT_PINS.echo);
    sensor_manager.configure_sensor(SensorPosition::RearLeft, REAR_LEFT_PINS.trigger, REAR_LEFT_PINS.echo);
    sensor_manager.configure_sensor(SensorPosition::RearRight, REAR_RIGHT_PINS.trigger, REAR_RIGHT_PINS.echo);
}

void print_distance(const __FlashStringHelper *label, SensorPosition position)
{
    Serial.print(label);
    Serial.print(F(": "));

    if (!sensor_manager.has_valid_reading(position))
    {
        Serial.println(F("측정 실패"));
        return;
    }

    const float distance_cm = sensor_manager.latest_distance_cm(position);
    Serial.print(distance_cm, 2);
    Serial.println(F(" cm"));
}

void setup()
{
    Serial.begin(115200);
    configure_sensors();
    sensor_manager.begin_all();
    start_sensor_cycle();
}

void loop()
{
    sensor_manager.update();

    if (!sensor_manager.is_cycle_complete())
    {
        return;
    }

    const unsigned long now = millis();
    record_cycle_completion(now);

    if ((now - last_update_ms) < UPDATE_INTERVAL_MS)
    {
        last_cycle_interval_ms = 0UL;
        start_sensor_cycle();
        return;
    }

    last_update_ms = now;

    // 각 센서 거리 값을 빠르게 확인하기 위해 한번에 출력한다.
    Serial.println(F("--- 초음파 센서 거리 ---"));
    print_distance(F("앞-왼쪽"), SensorPosition::FrontLeft);
    print_distance(F("앞-오른쪽"), SensorPosition::FrontRight);
    print_distance(F("뒤-왼쪽"), SensorPosition::RearLeft);
    print_distance(F("뒤-오른쪽"), SensorPosition::RearRight);

    Serial.print(F("측정 시간: "));
    Serial.print(last_cycle_duration_us);
    Serial.print(F(" us ("));
    Serial.print(static_cast<float>(last_cycle_duration_us) / 1000.0F, 3);
    Serial.println(F(" ms)"));

    Serial.print(F("사이클 간격: "));
    Serial.print(last_cycle_interval_ms);
    Serial.println(F(" ms"));
    Serial.println();

    start_sensor_cycle();
}