#ifndef ECON_SPD
#define ECON_SPD

// Feet
#define MIN_FL   10
#define MAX_FL  430
// Tonnes
#define MIN_WGT 140
#define MAX_WGT 290
// Percent
#define MIN_TIP  60
#define MAX_TIP  90
// Mach
#define MIN_MMO   0.85
#define MAX_MMO   0.87
// 10x Feet
#define FL_STP   20
// 10x Kilogrammes
#define WGT_STP  10
// Percent
#define TIP_STP  30.0

#include "ci2mach_0.85.h"
#include "ci2mach_0.87.h"

#include <algorithm>
#include <cmath>
#include <tuple>

/// @brief Round to n decimal places
/// @param value Value to round
/// @param decimals Number of decimal places
/// @return Rounded value
float roundTo(float value, char decimals) {
  return std::roundf(value * std::pow(10, decimals)) / std::pow(10, decimals);
}

/// @brief Bounding function for altitudes in accordance with data granularity
/// @param altitude Altitude in ft
/// @return Lower bound in FL, upper bound in FL, ratio between bounds that equates to flightLevel
std::tuple<int, int, float> boundAltitude(int altitude) {
  int flightLevel = (int)round(altitude / 100.0);

  float lower = flightLevel - ((flightLevel - MIN_FL) % FL_STP);
  float upper = (flightLevel - MIN_FL) % FL_STP != 0 ? lower + FL_STP : lower;
  float ratio = (flightLevel - lower) / FL_STP;

  return {(int)lower, (int)upper, ratio};
}

/// @brief Bounding function for weight in accordance with data granularity
/// @param weight Weight in kilogrammes
/// @return Lower bound in t, upper bound in t, ratio between bounds that equates to weight
std::tuple<int, int, float> boundWeight(int weight) {
  int wgt = (int)round(weight / 1000.0);

  float lower = wgt - ((wgt - MIN_WGT) % WGT_STP);
  float upper = (wgt - MIN_WGT) % WGT_STP != 0 ? lower + WGT_STP : lower;
  float ratio = ((weight / 1000.0) - lower) / WGT_STP;

  return {(int)lower, (int)upper, ratio};
}

/// @brief Bounding function for MMO based on total fuel
/// @param totalFuel Total fuel in kilogrammes
/// @return Ratio between .85 and .87 MMO
float boundMMO(float totalFuel) {
  float percent = (int)round(1.57613580e-02 * totalFuel - 1.51221174e+02);

  return percent <= MIN_TIP ? 0 :
         percent >= MAX_TIP ? 1 :
         (percent - MIN_TIP) / TIP_STP;
}

/// @brief Conversion from FL to index of file
/// @param flightLevel FL to convert
/// @return Index in file
int flightLevel2Index(int flightLevel) {
  return (flightLevel - MIN_FL) / FL_STP;
}

/// @brief Conversion from tonnes to index of file
/// @param weight Weight in tonnes to convert
/// @return Index in file
int weight2Index(int weight) {
  return (weight - MIN_WGT) / WGT_STP;
}

/// @brief Linear interpolate between lower and upper
/// @param lower Lower interpolation bound
/// @param upper Upper interpolation bound
/// @param ratio Ratio of interpolation between lower and upper
/// @return Value at ratio between lower and upper
float interp(float lower, float upper, float ratio) {
  return lower + (upper - lower) * ratio;
}

/// @brief Calculate mach for a given CI and aircraft state
/// @param altitude Altitude in feet
/// @param weight Weight in kilogrammes
/// @param totalFuel Total fuel in kilogrammes EXCLUDING BALLAST
/// @param ci CI
/// @return Mach corresponding to CI. Returns -1 if not possible
float ci2mach(float altitude, float weight, float totalFuel, int ci) {
  // Some fallback assumptions for extreme cases
  // Clamp weight, for <140 we use 140
  weight = std::clamp(weight, 140000.0f, 290000.0f);
  // Clamp altitude, for > 43000 we use 43000, for <1000 we use 1000
  altitude = std::clamp(altitude, 1000.0f, 43000.0f);
  // Safety CI
  ci = std::clamp(ci, 0, 999);

  auto bAlt = boundAltitude(altitude);
  int lowerFl = std::get<0>(bAlt);
  int upperFl = std::get<1>(bAlt);
  float ratioFl = std::get<2>(bAlt);

  auto bWgt = boundWeight(weight);
  int lowerWgt = std::get<0>(bWgt);
  int upperWgt = std::get<1>(bWgt);
  float ratioWgt = std::get<2>(bWgt);

  float ratioMMO = boundMMO(totalFuel);

  int lowerFlIndex = flightLevel2Index(lowerFl);
  int upperFlIndex = flightLevel2Index(upperFl);
  int lowerWgtIndex = weight2Index(lowerWgt);
  int upperWgtIndex = weight2Index(upperWgt);

  // Outside of the maximum indicies, safeguard us from out-of-bounds
  if (lowerFlIndex > 21 || upperFlIndex > 21 || lowerWgtIndex > 15 || upperWgtIndex > 15 ||
      lowerFlIndex < 0 || upperFlIndex < 0 || lowerWgtIndex < 0 || upperWgtIndex < 0) {
    return -1;
  }

  const float* lowerFlLowerWgtCis85 = ci2Mach_85->values[lowerFlIndex][lowerWgtIndex];
  const float* lowerFlUpperWgtCis85 = ci2Mach_85->values[lowerFlIndex][upperWgtIndex];
  const float* upperFlLowerWgtCis85 = ci2Mach_85->values[upperFlIndex][lowerWgtIndex];
  const float* upperFlUpperWgtCis85 = ci2Mach_85->values[upperFlIndex][upperWgtIndex];
  const float* lowerFlLowerWgtCis87 = ci2Mach_87->values[lowerFlIndex][lowerWgtIndex];
  const float* lowerFlUpperWgtCis87 = ci2Mach_87->values[lowerFlIndex][upperWgtIndex];
  const float* upperFlLowerWgtCis87 = ci2Mach_87->values[upperFlIndex][lowerWgtIndex];
  const float* upperFlUpperWgtCis87 = ci2Mach_87->values[upperFlIndex][upperWgtIndex];

  float lowerFlLowerWgtMach85 = lowerFlLowerWgtCis85[ci];
  float lowerFlUpperWgtMach85 = lowerFlUpperWgtCis85[ci];
  float upperFlLowerWgtMach85 = upperFlLowerWgtCis85[ci];
  float upperFlUpperWgtMach85 = upperFlUpperWgtCis85[ci];
  float lowerFlLowerWgtMach87 = lowerFlLowerWgtCis87[ci];
  float lowerFlUpperWgtMach87 = lowerFlUpperWgtCis87[ci];
  float upperFlLowerWgtMach87 = upperFlLowerWgtCis87[ci];
  float upperFlUpperWgtMach87 = upperFlUpperWgtCis87[ci];

  // Outside operational limits
  if (upperFlUpperWgtMach87 < 0 || upperFlUpperWgtMach85 < 0 || upperFlLowerWgtMach87 < 0 || upperFlLowerWgtMach85 < 0 ||
      lowerFlUpperWgtMach87 < 0 || lowerFlUpperWgtMach85 < 0 || lowerFlLowerWgtMach87 < 0 || lowerFlLowerWgtMach85 < 0) {
    return -1;
  }

  float ratioedLowerFlMach85 = interp(lowerFlLowerWgtMach85, lowerFlUpperWgtMach85, ratioWgt);
  float ratioedUpperFlMach85 = interp(upperFlLowerWgtMach85, upperFlUpperWgtMach85, ratioWgt);
  float ratioedMach85 = interp(ratioedLowerFlMach85, ratioedUpperFlMach85, ratioFl);
  float ratioedLowerFlMach87 = interp(lowerFlLowerWgtMach87, lowerFlUpperWgtMach87, ratioWgt);
  float ratioedUpperFlMach87 = interp(upperFlLowerWgtMach87, upperFlUpperWgtMach87, ratioWgt);
  float ratioedMach87 = interp(ratioedLowerFlMach87, ratioedUpperFlMach87, ratioFl);

  return roundTo(interp(ratioedMach85, ratioedMach87, ratioMMO), 3);
}

#endif