/**
******************************************************************************
* @file evolve_optimizer.h
* @brief Заголовочный файл для адаптивного подбора параметров
******************************************************************************
* @addtogroup EVOLVE_OPTIMIZER    Evolve optimizer
* @ingroup    MYLIBS_DEFINES
* @brief      Библиотека для эволюционного подбора параметров
* @details
Поддерживает:
- Любое количество параметров
- Генерацию новых параметров на основе лучших кандидатов
- Мутацию для поиска оптимальных параметров
- Несколько независимых оптимизаторов в одной программе


Параметры для конфигурации:
- @ref ENABLE_EVOLVE_OPTIMIZATION - Включить оптимизацию параметров
  Если библиотека отключена @ref ENABLE_EVOLVE_OPTIMIZATION, то вставляются 
  заглушки, никак не влияющие на параметры и остальную программу
- @ref EVOLVE_MAX_PARAMS          - Максимальное количество параметров
- @ref EVOLVE_MAX_CANDIDATES      - Максимальное количество кандидатов для обучения
- (опционально) @ref EVOLVE_MUTATION_MIN_PCT    - Минимальная мутация в процентах от Loss (по умолчанию 10%)
- (опционально) @ref EVOLVE_MUTATION_MAX_PCT    - Максимальная мутация в процентах от Loss (по умолчанию 100%)
- (опционально) @ref ELOVLE_N_ELITE_CANDIDATE   - Количество кандидатов, которые проходят в поколение без изменений

@par Пример использования:
@code
#include "evolve_optimizer.h"
#define N_PARAMS        4
#define N_CANDIDATES    100
#define N_BEST          10
#define MUTATION        0.1f
float params[N_PARAMS];
EvolveOptimizer_t optimizer;

// Формирование параметров
uint16_t  param_u16 = 800;
float     param_f   = 0.01f;
uint8_t   param_u8  = 40;
int16_t   param_i16 = 1600;
params[0] = PARAM_SCALE(param_u16,  0.0f,   1000.0f);
params[1] = PARAM_SCALE(param_f,    0.001f, 0.1f);
params[2] = PARAM_SCALE(param_u8,   10.0f,  100.0f);
params[3] = PARAM_SCALE(param_i16,  500.0f, 5000.0f);

// Инициалиазция
EvolveOptimizer_Init(&optimizer, N_PARAMS, N_CANDIDATES, N_BEST, MUTATION, params);

// Шаг эволюции
float loss = calc_loss(); // расчет эффективности параметров (от 0 до 1)
EvolveOptimizer_Step(&optimizer, params, loss);

// Взятие следующих для эволюции параметров
param_u16 = PARAM_UNSCALE(params[0],  0.0f,   1000.0f);
param_f   = PARAM_UNSCALE(params[1],  0.001f, 0.1f);
param_u8  = PARAM_UNSCALE(params[2],  10.0f,  100.0f);
param_i16 = PARAM_UNSCALE(params[3],  500.0f, 5000.0f);
@endcode
* @{  
*****************************************************************************/
#ifndef __EVOLVE_OPTIMIZER_H_
#define __EVOLVE_OPTIMIZER_H_

#include "mylibs_defs.h"
#include <stdint.h>
#include <stdlib.h>

#ifdef ENABLE_EVOLVE_OPTIMIZATION
/**
  * @brief Линейное масштабирование x из диапазона [min_val, max_val] в диапазон [0, 1)
  */
#define PARAM_SCALE(x, min_val, max_val) \
(((float)(x) - (float)(min_val)) / ((float)(max_val) - (float)(min_val)))

/**
  * @brief Обратное линейное масштабирование значения из [0, 1) в диапазон [min_val, max_val]
  */
#define PARAM_UNSCALE(val, min_val, max_val) \
(((float)(val)) * ((float)(max_val) - (float)(min_val)) + (float)(min_val))

#ifndef local_time
#define local_time()  HAL_GetTick() ///< Локальное время
#endif



#ifndef EVOLVE_MUTATION_MIN_PCT
#define EVOLVE_MUTATION_MIN_PCT   10    ///< Минимальная мутация (в процентах от Loss)
#endif
#ifndef EVOLVE_MUTATION_MAX_PCT
#define EVOLVE_MUTATION_MAX_PCT   100   ///< Максимальная мутация (в процентах от Loss)
#endif
#ifndef ELOVLE_N_ELITE_CANDIDATE
#define ELOVLE_N_ELITE_CANDIDATE  2     ///< Количество кандидатов, которые проходят в поколение без изменений (по умолчанию 2)
#endif


/**
  * @brief Структура эволюционного оптимизатора
  */
typedef struct {
  float     stability;                      ///< Коэффициент насколько стабильная популяция (0..1)(@ref n_cand)

  uint16_t  n_params;                       ///< Количество параметров
  uint16_t  n_cand;                         ///< Количество кандидатов в популяции
  uint16_t  n_best;                         ///< Количество лучших, усредняемых
  float     mutation_amp;                   ///< Амплитуда мутации (0..1)
  
  uint16_t  cand_index;                     ///< Индекс кандидата для обработки
  uint16_t  gen_index;                      ///< Индекс популяции  
  
  //INTERNAL
  float     gen_mut;                        ///< Амплитуда мутации у текущей популяции  
  
  float     loss[EVOLVE_MAX_CANDIDATES];    ///< Loss для каждого кандидата
  float     candidates[EVOLVE_MAX_CANDIDATES][EVOLVE_MAX_PARAMS]; ///< Параметры кандидатов
  uint16_t  sorted_idx[EVOLVE_MAX_CANDIDATES]; ///< Индексы отсортированных кандидатов
} EvolveOptimizer_t;



/** 
 * @cond EVOLVE_INTERNAL
 */

// Вспомогательный указатель для сортировки
static EvolveOptimizer_t *g_sort_opt; // глобальный указатель на текущий оптимизатор

// функция условия сортировки
static int cmp_idx(const void *a, const void *b) {
  if (g_sort_opt->loss[*(const uint16_t*)a] < g_sort_opt->loss[*(const uint16_t*)b]) 
    return -1;
  if (g_sort_opt->loss[*(const uint16_t*)a] > g_sort_opt->loss[*(const uint16_t*)b]) 
    return 1;
  return 0;
}
/** @endcond */





/**
  * @brief Инициализация эволюционного оптимизатора.
  * @param opt          Указатель на структуру оптимизатора
  * @param n_params     Количество параметров в одном кандидате
  * @param n_cand       Количество кандидатов
  * @param n_best       Количество лучших, усредняемых
  * @param mutation_amp Амплитуда мутации (в диапазоне 0.0–1.0)
  * @param start_params Начальные параметры (в диапазоне 0.0–1.0)
  * @return 0   — если окей, 
  *         -1  — если ошибка
  */
__STATIC_INLINE int EvolveOptimizer_Init(EvolveOptimizer_t* opt,
                                        uint16_t n_params,
                                        uint16_t n_cand,
                                        uint16_t n_best,
                                        float mutation_amp,
                                        float* start_params)
{
  if((opt == NULL) || (start_params == NULL))
    return -1;

  if(n_params > EVOLVE_MAX_PARAMS)
    return -1;  
  opt->n_params     = n_params;
  
  if(n_cand > EVOLVE_MAX_CANDIDATES)
    return -1;  
  opt->n_cand       = n_cand;
  
  if(n_best > EVOLVE_MAX_CANDIDATES/2)
    return -1;  
  opt->n_best       = n_best;
  
  
  if((mutation_amp > 1) || (mutation_amp < 0))
    return -1;    
  if(mutation_amp <= 0.001f)
    mutation_amp = 0.001f;  
  opt->mutation_amp  = mutation_amp;

  uint32_t seed = local_time();
#ifdef ADC1
  seed += (ADC1->DR & 0xFF);
#endif
  srand(seed);
  
  for (uint16_t i = 0; i < n_cand; i++) {
    for (uint16_t j = 0; j < n_params; j++) {
      // Добавляем случайную мутацию вокруг стартового параметра
      float base = start_params[j];
      float inv_randmax = 1.0f / (float)RAND_MAX;
      float noise = ((float)rand() * inv_randmax * 2.0f - 1.0f) * mutation_amp;
      opt->candidates[i][j] = base + noise;
      if (opt->candidates[i][j] < 0.0f) opt->candidates[i][j] = 0.0f;
      if (opt->candidates[i][j] > 1.0f) opt->candidates[i][j] = 1.0f;
    }
    opt->loss[i] = 0.0f;
  }
  
  opt->cand_index = 0;
  opt->gen_index = 0;
  return 0;
}


/**
  * @brief Один шаг эволюционного оптимизатора.
  * @param opt    Указатель на структуру оптимизатора
  * @param params Массив параметров, которые будут обновлены (на выходе — новые параметры)
  * @param loss   Loss текущего кандидата
  * @return 0   — если окей, 
  *         -1  — если ошибка
  * @details
  * Сохраняет loss текущего кандидата и формирует параметры следующего кандидата.
  * Если накоплено n_cand кандидатов, генерируется новое поколение.
  * Новое поколение формируется случайным выбором из n_best лучших с добавлением мутации.
  *
  * На выходе params содержит параметры следующего кандидата для измерений.
  * @note Функция использует глобальную внутреннюю переменную для сортировки.
  * Надо убедится что только один экземпляр функции запущен в момент времени
  */
__STATIC_INLINE int EvolveOptimizer_Step(EvolveOptimizer_t* opt,
                                        float* params,
                                        float loss)
{
  if((opt == NULL) || (params == NULL))
    return -1;

  uint16_t n_params = opt->n_params; 
  if(n_params > EVOLVE_MAX_PARAMS)
    return -1;  
  
  uint16_t n_cand = opt->n_cand; 
  if(n_cand > EVOLVE_MAX_CANDIDATES)
    return -1;  
  
  uint16_t n_best = opt->n_best; 
  if(n_best > EVOLVE_MAX_CANDIDATES/2)
    return -1;
  
  float    mut      = opt->mutation_amp;
  if((mut > 1) ||(mut < 0))
    return -1;

  // 1. Сохраняем loss текущего кандидата
  opt->loss[opt->cand_index] = loss;
  opt->cand_index++;

  if (opt->cand_index >= n_cand) {
    // 2. Сортируем текущее поколение по loss
    for(uint16_t i = 0; i < opt->n_cand; i++)
      opt->sorted_idx[i] = i;
    
    g_sort_opt = opt;
    qsort(opt->sorted_idx, opt->n_cand, sizeof(uint16_t), cmp_idx);
    g_sort_opt = NULL;

    // --- Адаптивная мутация в зависимости от Loss ---
    float best_loss = opt->loss[opt->sorted_idx[0]];
    float worst_loss = opt->loss[opt->sorted_idx[opt->n_cand - 1]];
    float diff = worst_loss - best_loss;

    float sum_loss = 0.0f;
    for (uint16_t i = 0; i < n_cand; i++)
      sum_loss += opt->loss[i];
    float avg_loss = sum_loss / (float)n_cand;
    
    
    float loss_ratio = (diff > 0.0f) ? ((avg_loss - best_loss) / diff) : 0.5f;
    if (loss_ratio < 0.0f) loss_ratio = 0.0f;
    if (loss_ratio > 1.0f) loss_ratio = 1.0f;
    
    // Записываем стабильность популяции в структуру
    if(diff < 0.0f) diff = 0.0f;
    if(diff > 1.0f) diff = 1.0f;
    opt->stability = (1.0f - worst_loss) * (1.0f - (worst_loss - best_loss));
    if(opt->stability < 0.0f) opt->stability = 0.0f;
    if(opt->stability > 1.0f) opt->stability = 1.0f;

    float mut_pct = EVOLVE_MUTATION_MIN_PCT +
                    (EVOLVE_MUTATION_MAX_PCT - EVOLVE_MUTATION_MIN_PCT) * loss_ratio;
    float adaptive_mut = mut * (mut_pct / 100.0f);
    if (adaptive_mut < 0.0001f) adaptive_mut = 0.0001f;
    opt->gen_mut = adaptive_mut;

    // 3. Генерируем новое поколение
    uint16_t n_elite = ELOVLE_N_ELITE_CANDIDATE;
    for (uint16_t c = 0; c < n_cand; c++) {
      if (c < n_elite) {
        for (uint16_t i = 0; i < n_params; i++)
          opt->candidates[c][i] = opt->candidates[opt->sorted_idx[c]][i];
        opt->loss[c] = 0.0f;
      } else {
        for (uint16_t i = 0; i < n_params; i++) {
          float inv_randmax = 1.0f / (float)RAND_MAX;
          float noise = ((float)rand() * inv_randmax * 2.0f - 1.0f) * adaptive_mut;
          uint16_t parent = opt->sorted_idx[rand() % opt->n_best];
          opt->candidates[c][i] = opt->candidates[parent][i] + noise;
          if (opt->candidates[c][i] < 0.0f) opt->candidates[c][i] = 0.0f;
          if (opt->candidates[c][i] > 1.0f) opt->candidates[c][i] = 1.0f;
        }
        opt->loss[c] = 0.0f;
      }
    }
    opt->cand_index = 0;
    opt->gen_index++;
  }

  // 4. Возвращаем параметры следующего кандидата
  for (uint16_t i = 0; i < opt->n_params; i++)
    params[i] = opt->candidates[opt->cand_index][i];
  
  return 0;
}
#else // ENABLE_EVOLVE_OPTIMIZATION
//заглушки
typedef struct {
  uint16_t n_params; 
  uint16_t n_cand; 
  uint16_t n_best; 
  float mutation_amp; 
  float loss[0]; 
  float candidates[0][0]; 
} EvolveOptimizer_t;
#define EvolveOptimizer_Init(opt, n_params, n_cand, n_best, mutation_amp, start_params)
#define EvolveOptimizer_Step(opt, params, LossFunc)
#define PARAM_SCALE(x, min_val, max_val)         (x)
#define PARAM_UNSCALE(val, min_val, max_val)     (val)
#endif // ENABLE_EVOLVE_OPTIMIZATION

#endif // __EVOLVE_OPTIMIZER_H_

/** EVOLVE_OPTIMIZER
  * @}
  */
