#include "params.h"
#include "can-halal.h"
#include "vehicle.h"

Params params = {0};
static const float param_limits[] = {
    [PF_BBAL] = 100.0f,
    [PF_PLIM] = 120.0f,
    [PF_TORQUE] = 1500.0f,
    [PF_SLIM] = 150.0f,
    [PF_APPS0_MIN] = 5.0f,
    [PF_APPS0_MAX] = 5.0f,
    [PF_APPS1_MIN] = 5.0f,
    [PF_APPS1_MAX] = 5.0f,
    [PF_TV] = 1,
    [PF_TC] = 1,
    [PF_TC_SLIPREF] = 1.0f,
    [PF_TC_MUMAX] = 5.0f,
    [PF_TC_P] = 20.0f,
    [PF_TC_I] = 20.0f,
    [PF_DRS_CLOSED] = 1.0f,
    [PF_DRS_OPEN] = 1.0f,
    // TODO: counted enum for map?
    [PF_MAP] = 7,
};
float param_steps[] = {
    [PF_BBAL] = 1,
    [PF_PLIM] = 1,
    [PF_TORQUE] = 10,
    [PF_SLIM] = 1,
    [PF_APPS0_MIN] = 0.001f,
    [PF_APPS0_MAX] = 0.001f,
    [PF_APPS1_MIN] = 0.001f,
    [PF_APPS1_MAX] = 0.001f,
    [PF_TV] = 1,
    [PF_TC] = 1,
    [PF_TC_SLIPREF] = 0.01f,
    [PF_TC_MUMAX] = 0.1f,
    [PF_TC_P] = 0.1f,
    [PF_TC_I] = 0.1f,
    [PF_DRS_CLOSED] = 0.001f,
    [PF_DRS_OPEN] = 0.001f,
    [PF_MAP] = 1,
};
static const float param_broadcast_factors[] = {
    [PF_BBAL] = 1,
    [PF_PLIM] = 1,
    [PF_TORQUE] = 1,
    [PF_SLIM] = 1,
    [PF_APPS0_MIN] = 0.001f,
    [PF_APPS0_MAX] = 0.001f,
    [PF_APPS1_MIN] = 0.001f,
    [PF_APPS1_MAX] = 0.001f,
    [PF_TV] = 1,
    [PF_TC] = 1,
    [PF_TC_SLIPREF] = 0.001f,
    [PF_TC_MUMAX] = 0.001f,
    [PF_TC_P] = 0.001f,
    [PF_TC_I] = 0.001f,
    [PF_DRS_CLOSED] = 0.001f,
    [PF_DRS_OPEN] = 0.001f,
    [PF_MAP] = 1,
};
#ifdef __cplusplus
_Static_assert(sizeof(param_limits) / sizeof(param_limits[0]) ==
                   ParamType_COUNT,
               "Incorrect number of param limits");
_Static_assert(sizeof(param_steps) / sizeof(param_steps[0]) == ParamType_COUNT,
               "Incorrect number of param steps");
_Static_assert(sizeof(param_broadcast_factors) /
                       sizeof(param_broadcast_factors[0]) ==
                   ParamType_COUNT,
               "Incorrect number of param broadcast factors");
#endif

void params_init() {
  params.bbal = 50;
  params.plim = 20;
}

void params_inc(ParamType param) {
  switch (param) {
  case PF_TV:
  case PF_TC:
  case PF_MAP:
    params.valuesInt[param] += param_steps[param];
    if (params.valuesInt[param] > param_limits[param]) {
      params.valuesInt[param] = param_limits[param];
    }
    break;
  default:
    params.valuesFloat[param] += param_steps[param];
    if (params.valuesFloat[param] > param_limits[param]) {
      params.valuesFloat[param] = param_limits[param];
    }
  }
}

void params_dec(ParamType param) {
  switch (param) {
  case PF_TV:
  case PF_TC:
  case PF_MAP:
    params.valuesInt[param] -= param_steps[param];
    if (params.valuesInt[param] < 0) {
      params.valuesInt[param] = 0;
    }
    break;
  default:
    params.valuesFloat[param] -= param_steps[param];
    if (params.valuesFloat[param] < 0) {
      params.valuesFloat[param] = 0;
    }
  }
}

void params_broadcast(ParamType param) {
  int32_t value;
  switch (param) {
  case PF_TV:
  case PF_TC:
  case PF_MAP:
    value = params.valuesInt[param] / param_broadcast_factors[param];
    break;
  default:
    value = params.valuesFloat[param] / param_broadcast_factors[param];
  }
  vehicle_broadcast_param(param, value);
}