// 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 "stm32h747xx.h"
#include "stm32h7xx.h"
#include "stm32h7xx_hal_tim.h"
#include "tasks.h"
#include <stdint.h>

static inline void TIM2_CH3_SetOCMode(uint32_t oc_mode) {
  MODIFY_REG(TIM2->CCMR2, TIM_CCMR2_OC3M | TIM_CCMR2_OC3M_3, oc_mode);
}

void crankHandler(void *argument) {
  for (;;) {
    osThreadFlagsWait(0x01, osFlagsWaitAny, osWaitForever);
    DEBUG_LOG("Crank pulse detected at: %lu\n\r",
              ringBufferRead(&state_g.crank_RB, 0));
    // FILTER
    float delta_percentage = ((float)(D1A - D1B)) / D1B;
    if (delta_percentage > 0.4 || delta_percentage < -0.4) {
      state_g.sync_state = SYNC_NOT_OK;
      state_g.crank_state = CYCLE_UNKNOWN;
      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.crank_state == CYCLE_COMPRESSION &&
        state_g.sync_state == SYNC_OK) {
      DEBUG_LOG("Spark schedule reached, congrats\n\r");
      uint32_t d1a = CRANK(0) - CRANK(1);
      // TODO: Map interpolation rather than SPARK_ADVANCE CONSTANT
      uint32_t d_spark = d1a * (45 - SPARK_ADVANCE) / 180;
      uint32_t t_spark = CRANK(0) + d_spark;
      // TODO: schedule spark
      // __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, t_spark);
      // TIM2_CH3_SetOCMode(t_spark);
    } else if (state_g.crank_state == CYCLE_EXHAUST) {
        uint32_t t_injection = CRANK(0) + (45+INJECTION_PHASE) * (CRANK(4) - CRANK(0)) / 720;
        //TODO Schedule injection
    }
  }
}