#include <chrono>
#include <iostream>

#include "../econSpd.h"

int main() {
  float altitude;
  float weight;
  int tipTankPercent;
  int ci;

  // Get data

  std::cout << "Altitude (ft, no FL)?: ";
  std::cin >> altitude;

  std::cout << "Weight (kg, no t)?: ";
  std::cin >> weight;

  std::cout << "Tip tank quantity (percent): ";
  std::cin >> tipTankPercent;

  std::cout << "CI (0 to 999)?: ";
  std::cin >> ci;

  std::cout << std::endl << std::endl << "BEGIN Step-By-Step" << std::endl << std::endl;

  // Bound data

  auto [lowerFl, upperFl, ratioFl] = boundAltitude(altitude);

  std::cout << "Lower Altitude Bound: " << lowerFl << std::endl;
  std::cout << "Upper Altitude Bound: " << upperFl << std::endl;
  std::cout << "Ratio: " << ratioFl << std::endl << std::endl;

  auto [lowerWgt, upperWgt, ratioWgt] = boundWeight(weight);

  std::cout << "Lower Weight Bound: " << lowerWgt << std::endl;
  std::cout << "Upper Weight Bound: " << upperWgt << std::endl;
  std::cout << "Ratio: " << ratioWgt << std::endl << std::endl;

  float ratioMMO = boundMMO(tipTankPercent);

  // Check validity

  if (altitude < MIN_FL * 100) {
    std::cerr << "Too low, min Altitude: " << MIN_FL * 100 << std::endl;
    exit(1);
  }
  if (altitude > MAX_FL * 100) {
    std::cerr << "Too high, max Altitude: " << MAX_FL * 100 << std::endl;
    exit(1);
  }
  if (weight < MIN_WGT * 1000) {
    std::cerr << "Too light, min weight: " << MIN_WGT * 1000 << std::endl;
    exit(1);
  }
  if (weight > MAX_WGT * 1000) {
    std::cerr << "Too heavy, max weight: " << MAX_WGT * 1000 << std::endl;
    exit(1);
  }
  if (tipTankPercent < 0 || tipTankPercent > 100) {
    std::cerr << "Invalid quantity" << std::endl;
    exit(1);
  }
  if (ci < 0) {
    std::cerr << "CI must be greater or equal to 0" << std::endl;
    exit(1);
  }
  if (ci > 999) {
    std::cerr << "CI must be lesser or equal to 999" << std::endl;
    exit(1);
  }

  // Grab table, convert altitude and weight to index

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

  // Get boundary values

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

  // Check if we can even fly here (higher FL and weights are NULL due to perf limits)

  if (lowerFlLowerWgtCis85 == NULL || lowerFlUpperWgtCis85 == NULL || upperFlLowerWgtCis85 == NULL || upperFlUpperWgtCis85 == NULL ||
      lowerFlLowerWgtCis87 == NULL || lowerFlUpperWgtCis87 == NULL || upperFlLowerWgtCis87 == NULL || upperFlUpperWgtCis87 == NULL) {
    std::cerr << "Outside operational regime. Check weight/altitude" << std::endl;
    exit(1);
  }

  // Get machs

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

  // Lin interp

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

  float ratioedMach = roundTo(interp(ratioedMach85, ratioedMach87, ratioMMO), 3);

  std::cout << "Mach for CI " << ci << ": " << ratioedMach << std::endl << std::endl;
  std::cout << "END Step-By-Step" << std::endl << std::endl << std::endl;

  std::cout << "BEGIN All-In-One" << std::endl << std::endl;

  auto start = std::chrono::high_resolution_clock::now();
  float aioMach = ci2mach(altitude, weight, tipTankPercent, ci);
  auto stop = std::chrono::high_resolution_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(stop - start);
  
  std::cout << "Mach for CI " << ci << ": " << aioMach << std::endl << std::endl;
  std::cout << "END All-In-One, " << duration.count() << std::endl ;
}