/**
******************************************************************************
* @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      - Максимальное количество кандидатов для обучения


@par Пример использования:
@code
#include "evolve_optimizer.h"
#define N_PARAMS        4
#define N_CANDIDATES    100
#define N_BEST          10
#define IQ_MUTATION     (PARAM_SCALE_Q16(0.1, 0, 1))
int32_t 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_Q16(param_u16,  0,      1000);
params[1] = PARAM_SCALE_Q16(param_f,    0.001f, 0.1f);
params[2] = PARAM_SCALE_Q16(param_u8,   10,     100);
params[3] = PARAM_SCALE_Q16(param_i16,  500,    5000);

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

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

// Взятие следующих для эволюции параметров
param_u16 = PARAM_UNSCALE_Q16(params[0],  0,      1000);
param_f   = PARAM_UNSCALE_Q16(params[1],  0.001f, 0.1f);
param_u8  = PARAM_UNSCALE_Q16(params[2],  10,     100);
param_i16 = PARAM_UNSCALE_Q16(params[3],  500,    5000);
@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] в Q16.16 [0, 65536)
  */
#define PARAM_SCALE_Q16(x, min_val, max_val) \
((int32_t)((((float)(x) - (float)(min_val)) / ((float)(max_val) - (float)(min_val))) * 65536.0f))

/**
  * @brief Обратное линейное масштабирование Q16.16 значения в диапазон [min_val, max_val]
 */
#define PARAM_UNSCALE_Q16(q16_val, min_val, max_val) \
(((float)(q16_val) / 65536.0f) * ((float)(max_val) - (float)(min_val)) + (float)(min_val))

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

/**
  * @brief Структура эволюционного оптимизатора
  */
typedef struct {
  uint16_t n_params;                      ///< Количество параметров
  uint16_t n_cand;                        ///< Количество кандидатов
  uint16_t n_best;                        ///< Количество лучших, усредняемых
  uint16_t iq_mutation;                   ///< Амплитуда мутации в Q16.16
  uint16_t cand_index;                    ///< Индекс кандидата для обработки
  int32_t loss[EVOLVE_MAX_CANDIDATES];    ///< Loss для каждого кандидата
  int32_t candidates[EVOLVE_MAX_CANDIDATES][EVOLVE_MAX_PARAMS];    ///< Параметры кандидатов
  uint16_t sorted_idx[EVOLVE_MAX_CANDIDATES]; ///< Индексы отсортированных кандидатов
} EvolveOptimizer_t;



/** 
 * @cond EVOLVE_INTERNAL
 */
#define Q16_MUL(a,b) ((int32_t)(((int64_t)(a) * (int64_t)(b)) >> 16))

// Вспомогательный указатель для сортировки
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 iq_mutation  Амплитуда мутации в Q16.16
  * @param start_params Начальные параметры (Q16.16)
  * @return 0   — если окей, 
  *         -1  — если ошибка
  */
__STATIC_INLINE int EvolveOptimizer_Init(EvolveOptimizer_t* opt,
                                        uint16_t n_params,
                                        uint16_t n_cand,
                                        uint16_t n_best,
                                        uint16_t iq_mutation,
                                        int32_t* 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(iq_mutation > 32768)
    return -1;  
  opt->iq_mutation  = iq_mutation;

  for (uint16_t i = 0; i < n_cand; i++) {
    for (uint16_t j = 0; j < n_params; j++) {
      opt->candidates[i][j] = start_params[j];
    }
    opt->loss[i] = 0;
  }
  uint32_t seed = local_time() + (ADC1->DR & 0xFF);
  srand(seed);
  
  return 0;
}


/**
  * @brief Один шаг эволюционного оптимизатора.
  * @param opt    Указатель на структуру оптимизатора
  * @param params Массив параметров, которые будут обновлены (на выходе — новые параметры)
  * @param loss   Loss текущего кандидата (Q16.16)
  * @return 0   — если окей, 
  *         -1  — если ошибка
  * @details
  * Сохраняет loss текущего кандидата и формирует параметры следующего кандидата.
  * Если накоплено n_cand кандидатов, генерируется новое поколение.
  * Новое поколение формируется случайным выбором из n_best лучших с добавлением мутации.
  *
  * На выходе params содержит параметры следующего кандидата для измерений.
  * @note Функция использует глобальную внутреннюю переменную для сортировки.
  * Надо убедится что только один экземпляр функции запущен в момент времени
  */
__STATIC_INLINE int EvolveOptimizer_Step(EvolveOptimizer_t* opt,
                                        int32_t* params,
                                        int32_t   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;  
  
  uint16_t mut = opt->iq_mutation;
  if(mut > 32768)
    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;
//    for (uint16_t i = 0; i < n_cand - 1; i++) {
//      for (uint16_t j = i + 1; j < n_cand; j++) {
//        if (opt->loss[j] < opt->loss[i]) {
//          int32_t tmp_loss = opt->loss[i];
//          opt->loss[i] = opt->loss[j];
//          opt->loss[j] = tmp_loss;

//          for (uint16_t k = 0; k < n_params; k++) {
//            int32_t tmp = opt->candidates[i][k];
//            opt->candidates[i][k] = opt->candidates[j][k];
//            opt->candidates[j][k] = tmp;
//          }
//        }
//      }
//    }

    // 3. Генерируем новое поколение: каждый кандидат берется случайно из лучших с мутацией
    uint16_t n_elite = 2; // количество элитных кандидатов, которые сохраняем без изменений
    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; 
      } else {
        // Остальные кандидаты формируются с кроссовером и мутацией
        for (uint16_t i = 0; i < n_params; i++) {
          int32_t noise = (rand() % (2 * mut)) - mut;
          uint16_t parent = opt->sorted_idx[rand() % opt->n_best]; // каждый параметр из случайного лучшего
          opt->candidates[c][i] = opt->candidates[parent][i] + noise;
        }
        opt->loss[c] = 0;
      }
    }
    opt->cand_index = 0;
  }

  // 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; 
  uint16_t iq_mutation; 
  int32_t loss[0]; 
  int32_t candidates[0][0]; 
} EvolveOptimizer_t;
#define EvolveOptimizer_Init(opt, n_params, n_cand, n_best, iq_mutation, start_params)
#define EvolveOptimizer_Step(opt, params, LossFunc)
#define PARAM_SCALE_Q16(x, min_val, max_val)            (x)
#define PARAM_UNSCALE_Q16(q16_val, min_val, max_val)    (q16_val)
#endif // ENABLE_EVOLVE_OPTIMIZATION

#endif // __EVOLVE_OPTIMIZER_H_

/** EVOLVE_OPTIMIZER
  * @}
  */