#ifndef ECON_SPD
#define ECON_SPD

// Feet
#define MIN_FL  250
#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 <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 FL
/// @return Lower bound in FL, upper bound in FL, ratio between bounds that equates to flightLevel
std::tuple<int, int, float> boundAltitude(float altitude) {
  int flightLevel = (int)(altitude / 1000) * 10;

  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)(weight / 1000);

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

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

float boundMMO(int tipTankPercent) {
  return tipTankPercent <= MIN_TIP ? 0 :
         tipTankPercent >= MAX_TIP ? 1 :
         (tipTankPercent - 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 tipTankPercent Lowest tip tank quantity (left/right) in percent
/// @param ci CI
/// @return Mach corresponding to CI. Returns -1 if not possible
float ci2mach(float altitude, float weight, float tipTankPercent, int ci) {
  auto [lowerFl, upperFl, ratioFl] = boundAltitude(altitude);
  auto [lowerWgt, upperWgt, ratioWgt] = boundWeight(weight);
  float ratioMMO = boundMMO(tipTankPercent);

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

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

  if (lowerFlLowerWgtCis85 == NULL || lowerFlUpperWgtCis85 == NULL || upperFlLowerWgtCis85 == NULL || upperFlUpperWgtCis85 == NULL ||
      lowerFlLowerWgtCis87 == NULL || lowerFlUpperWgtCis87 == NULL || upperFlLowerWgtCis87 == NULL || upperFlUpperWgtCis87 == NULL) {
    return -1;
  }

  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];

  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