// Copyright (C) 2026 Hector van der Aa <hector@h3cx.dev>
// Copyright (C) 2026 Pierre Barbier <pierrebarbier741@gmail.com>
// Copyright (C) 2026 Association Exergie <association.exergie@gmail.com>
// SPDX-License-Identifier: GPL-3.0-or-later

#include "cmsis_os2.h"
#include "global_state.h"
#include "macros.h"
#include "main.h"
#include "ring_buffer.h"
#include "stm32h7xx_hal_tim.h"
#include "tasks.h"
#include "timer_utils.h"
#include <stdint.h>

void crankHandler(void *argument) {
  for (;;) {
    osThreadFlagsWait(0x01, osFlagsWaitAny, osWaitForever);
    DEBUG_LOG("Crank pulse detected at: %lu\n\r",
              ringBufferRead(&state_g.crank_RB, 0));

    /* WINDOW FILTER
     *
     * This filter limits incoming signals to a certain max percentage deviation
     * to blank out false triggers that would be impossible in real life
     *
     * It computes the percantage difference between d1a and d1b then if this
     * has an absolute value over a certain threshold the signal is discarded
     */
    if (state_g.sync_state == SYNC_OK) {
      uint32_t d1a = CRANK(0) - CRANK(1);
      uint32_t d1b = CRANK(1) - CRANK(2);

      int32_t delta_percentage =
          ((int64_t)d1a - (int64_t)d1b) * 100 / (int64_t)d1b;

      DEBUG_LOG("%ld\n\r", delta_percentage);

      if (delta_percentage > 40 || delta_percentage < -40) {
        // Updated to use clean API
        // state_g.crank_RB.w_head -= 1;
        ringBufferRevert(&state_g.crank_RB, 1);
        continue;
      }
    }

    // INCREMENT SWITCH
    switch (state_g.crank_state) {
    case CYCLE_COMPRESSION: {
      if (state_g.cam_state != CAM_TRIGD) {
        DEBUG_LOG("Cam miss detected, sync dynamite incoming\n\r");
        state_g.cam_miss_ctr++;
        if (state_g.cam_miss_ctr > MAX_CAM_MISS) {
          state_g.sync_state = SYNC_NOT_OK;
          state_g.crank_state = CYCLE_UNKNOWN;
          break;
        }
      }
      state_g.crank_state = CYCLE_COMBUSTION;
      DEBUG_LOG("Crank state incremented\n\r");
      state_g.cam_state = CAM_IDLE;
      break;
    }
    case CYCLE_COMBUSTION:
    case CYCLE_EXHAUST:
    case CYCLE_INTAKE: {
      if (state_g.cam_state == CAM_TRIGD) {
        state_g.sync_state = SYNC_NOT_OK;
        state_g.crank_state = CYCLE_UNKNOWN;
        state_g.cam_state = CAM_IDLE;
        break;
      }
      state_g.crank_state++;
      DEBUG_LOG("Crank state incremented\n\r");
      break;
    }
    case CYCLE_UNKNOWN: {
      if (state_g.sync_state == SYNC_PENDING &&
          state_g.cam_state == CAM_TRIGD) {
        state_g.crank_state = CYCLE_COMBUSTION;
        state_g.cam_state = CAM_IDLE;
      }
      break;
    }
    }

    // SPARK SCHEDULE
    if (state_g.sync_state == SYNC_OK) {
      switch (state_g.crank_state) {
      case CYCLE_INTAKE:
      case CYCLE_COMPRESSION: {
        uint32_t d1a = CRANK(0) - CRANK(1);
        uint32_t d_spark = 0;
        if (state_g.crank_state == CYCLE_INTAKE) {
          d_spark += d1a;
        }
        d_spark += d1a * (45 - SPARK_ADVANCE) / 180;
        uint32_t t_spark = CRANK(0) + d_spark;
        uint32_t t_spark_charge = t_spark - SPARK_CHARGE_TIME;
        if (state_g.crank_state == CYCLE_INTAKE) {
          if (t_spark_charge > CRANK(0) + 500 + ((d1a * 102U) / 100U)) {
            state_g.charge_defer = true;
          } else {
            state_g.charge_defer = false;
            if (timer_safe_to_arm(t_spark_charge, &htim2)) {
              state_g.current_spark_state = SPARK_PENDING_CHARGE;
              TIM2_CH3_SetOCMode(TIM2_CH3_OC_ACTIVE);
              __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, t_spark_charge);
            } else {
              state_g.current_spark_state = SPARK_IDLE;
              state_g.spark_errors.overspeed_error_ctr++;
            }
          }
          break;
        }

        if (state_g.charge_defer) {
          state_g.spark_defer = true;
          state_g.t_defer_spark = t_spark;
          state_g.charge_defer = false;
          if (timer_safe_to_arm(t_spark_charge, &htim2)) {
            state_g.current_spark_state = SPARK_PENDING_CHARGE;
            TIM2_CH3_SetOCMode(TIM2_CH3_OC_ACTIVE);
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, t_spark_charge);
          } else {
            state_g.spark_errors.defer_charge_error_ctr++;
          }
          break;
        }

        switch (state_g.current_spark_state) {
        case SPARK_PENDING_CHARGE: {
          if (timer_safe_to_arm(t_spark_charge, &htim2)) {
            state_g.current_spark_state = SPARK_PENDING_CHARGE;
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, t_spark_charge);
          } else {
            state_g.spark_errors.revised_charge_error_ctr++;
          }
          state_g.spark_defer = true;
          state_g.t_defer_spark = t_spark;
          break;
        }
        case SPARK_CHARGING: {
          if (timer_safe_to_arm(t_spark, &htim2)) {

            state_g.current_spark_state = SPARK_PENDING_RELEASE;
            TIM2_CH3_SetOCMode(TIM2_CH3_OC_INACTIVE);
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, t_spark);
            state_g.spark_defer = false;
          } else {
            // TODO: Implement SSD on ignition IC if charging and complete
            // recovery
            state_g.spark_errors.task_release_error_ctr++;
          }
          break;
        }
        default: {
          state_g.spark_errors.state_error_ctr++;
          break;
        }
        }

        break;
      }

      case CYCLE_EXHAUST: {
        // TODO: Schedule injection
        break;
      }
      default:
        break;
      }
    }
  }
}