File fsm_cut_at_fov_end.hpp

// Copyright 2025 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include "nebula_core_decoders/scan_cutter/types.hpp"

#include <stdexcept>

namespace nebula::drivers
{

class FsmCutAtFovEnd
{
public:
  using buffer_index_t = scan_cutter::buffer_index_t;
  using ChannelBufferState = scan_cutter::ChannelBufferState;
  using ChannelFovState = scan_cutter::ChannelFovState;
  using TransitionActions = scan_cutter::TransitionActions;

  template <typename T>
  using AllSame = scan_cutter::AllSame<T>;
  using Different = scan_cutter::Different;

  enum class State : uint8_t {
    O0,    // All channels in buffer 0, all outside FoV
    F0,    // All channels in buffer 0, at least one in FoV
    C0_1,  // Crossing from buffer 0 to buffer 1
    O1,    // All channels in buffer 1, all outside FoV
    F1,    // All channels in buffer 1, at least one in FoV
    C1_0   // Crossing from buffer 1 to buffer 0
  };

  [[nodiscard]] static State determine_state(
    const ChannelBufferState & buffer_state, const ChannelFovState & fov_state,
    buffer_index_t current_buffer)
  {
    // Check if channels are split between buffers (Crossing state)
    if (std::holds_alternative<Different>(buffer_state)) {
      return current_buffer == 0 ? State::C0_1 : State::C1_0;
    }

    // All channels are in the same buffer
    buffer_index_t buffer_index = std::get<AllSame<buffer_index_t>>(buffer_state).value;

    // Determine FoV-based state: F if any channel is in FoV, O if all outside
    bool any_in_fov{};
    if (std::holds_alternative<Different>(fov_state)) {
      // Mixed: some in FoV, some outside -> at least one in FoV
      any_in_fov = true;
    } else {
      any_in_fov = std::get<AllSame<bool>>(fov_state).value;
    }

    if (any_in_fov) {
      return buffer_index == 0 ? State::F0 : State::F1;
    }
    // All outside FoV
    return buffer_index == 0 ? State::O0 : State::O1;
  }

  [[nodiscard]] static TransitionActions get_transition_actions(
    State state_before, State state_after)
  {
    TransitionActions actions{std::nullopt, std::nullopt};

    // No transition - no action
    if (state_before == state_after) {
      return actions;
    }

    // Transition table implementation for buffer 0 -> buffer 1 transitions
    switch (state_before) {
      case State::O0:
        switch (state_after) {
          case State::F0:
          case State::C0_1:
            // O0 -> F0/C0_1: T0 (reset timestamp of buffer 0)
            actions.reset_timestamp_buffer = 0;
            break;
          case State::O1:
            // O0 -> O1: T1, E0 (reset timestamp 1, emit buffer 0)
            actions.reset_timestamp_buffer = 1;
            actions.emit_scan_buffer = 0;
            break;
          default:
            // O0 -> F1/C1_0: invalid (empty cells)
            break;
        }
        break;

      case State::F0:
        switch (state_after) {
          case State::O1:
            // F0 -> O1: E0 (emit buffer 0)
            actions.emit_scan_buffer = 0;
            break;
          case State::F1:
            // F0 -> F1: T1, E0 (reset timestamp 1, emit buffer 0)
            actions.reset_timestamp_buffer = 1;
            actions.emit_scan_buffer = 0;
            break;
          default:
            // F0 -> O0/C0_1/C1_0: invalid or no action
            break;
        }
        break;

      case State::C0_1:
        switch (state_after) {
          case State::O1:
            // C0_1 -> O1: E0 (emit buffer 0)
            actions.emit_scan_buffer = 0;
            break;
          case State::F1:
            // C0_1 -> F1: T1, E0 (reset timestamp 1, emit buffer 0)
            actions.reset_timestamp_buffer = 1;
            actions.emit_scan_buffer = 0;
            break;
          case State::C1_0:
            // C0_1 -> C1_0: ⛔ Invalid transition
            throw std::runtime_error("Invalid FSM transition: C0_1 -> C1_0");
          default:
            // C0_1 -> O0/F0: invalid
            break;
        }
        break;

      // Symmetric transitions for buffer 1 states
      case State::O1:
        switch (state_after) {
          case State::F1:
          case State::C1_0:
            // O1 -> F1/C1_0: T1 (reset timestamp of buffer 1)
            actions.reset_timestamp_buffer = 1;
            break;
          case State::O0:
            // O1 -> O0: T0, E1 (reset timestamp 0, emit buffer 1)
            actions.reset_timestamp_buffer = 0;
            actions.emit_scan_buffer = 1;
            break;
          default:
            // O1 -> F0/C0_1: invalid (empty cells)
            break;
        }
        break;

      case State::F1:
        switch (state_after) {
          case State::O0:
            // F1 -> O0: E1 (emit buffer 1)
            actions.emit_scan_buffer = 1;
            break;
          case State::F0:
            // F1 -> F0: T0, E1 (reset timestamp 0, emit buffer 1)
            actions.reset_timestamp_buffer = 0;
            actions.emit_scan_buffer = 1;
            break;
          default:
            // F1 -> O1/C1_0/C0_1: invalid or no action
            break;
        }
        break;

      case State::C1_0:
        switch (state_after) {
          case State::O0:
            // C1_0 -> O0: E1 (emit buffer 1)
            actions.emit_scan_buffer = 1;
            break;
          case State::F0:
            // C1_0 -> F0: T0, E1 (reset timestamp 0, emit buffer 1)
            actions.reset_timestamp_buffer = 0;
            actions.emit_scan_buffer = 1;
            break;
          case State::C0_1:
            // C1_0 -> C0_1: ⛔ Invalid transition
            throw std::runtime_error("Invalid FSM transition: C1_0 -> C0_1");
          default:
            // C1_0 -> O1/F1: invalid
            break;
        }
        break;
    }

    return actions;
  }

  [[nodiscard]] static TransitionActions step(
    const ChannelBufferState & buffer_state_before, const ChannelBufferState & buffer_state_after,
    const ChannelFovState & fov_state_before, const ChannelFovState & fov_state_after,
    buffer_index_t current_buffer)
  {
    State state_before = determine_state(buffer_state_before, fov_state_before, current_buffer);
    State state_after = determine_state(buffer_state_after, fov_state_after, current_buffer);
    return get_transition_actions(state_before, state_after);
  }
};

}  // namespace nebula::drivers