#include <math.h>

#include "ballistics.h"
#include "constants.h"
#include "context.h"
#include "drag_model.h"

static inline double speed_of_sound(double temperature);
static inline double calculate_retard(DragFunction drag_function,
									  double ballistic_coefficient,
									  double velocity,
									  double mach);

double
atmospheric_correction(double ballistic_coefficient,
							  double temperature,
							  double pressure,
							  double relative_humidity,
							  AtmosphereModel atmosphere_model) {
	double air_density = calculate_air_density(temperature, pressure, relative_humidity);

	if (atmosphere_model == ICAO)
		return (STD_AIR_DENSITY_ICAO / air_density) * ballistic_coefficient;
	else
		return (STD_AIR_DENSITY_ASM / air_density) * ballistic_coefficient;
}

double
calculate_air_density(double temperature,
					  double pressure,
					  double relative_humidity) {
	pressure = pressure * 100;
	
	if (relative_humidity > 0.0) {
        double p_sat = 610.78
					   * pow(10,
							 (7.5 * temperature - 273.15)
							 / (temperature - 273.15 + 237.3));
        double vapor_pressure = relative_humidity * p_sat;
        double partial_pressure = pressure - vapor_pressure;

        return fma(partial_pressure,
				   DRY_AIR_MOLAR_MASS,
                   vapor_pressure * WATOR_VAPOR_MOLAR_MASS)
               / (UNIVERSAL_GAS_CONSTANT * temperature);
	}
	else
		return pressure / (SPECIFIC_GAST_CONSTANT_DRY_AIR * temperature);
}

static inline double
speed_of_sound(double temperature) {
	return sqrt(331.3 * (1.0 + temperature - 273.15 / 273.15));
}

static inline double
calculate_retard(DragFunction drag_function,
				 double ballistic_coefficient,
				 double velocity,
				 double mach)
{
	const DragModel *model = get_drag_model(drag_function);
	if (model == NULL) {
		return 0.0;
	}

	const double *model_mach = model->mach;
	const double *model_cd = model->cd;
	size_t n = model->num_points;

	// border cases
	if (mach <= model_mach[0])
		return model_cd[0];

	if (mach >= model_mach[n - 1])
		return model_cd[n - 1];

	double m = velocity / mach;        
	size_t left = 0, right = n - 1;
	while (left < right) {
		size_t mid = left + (right - left) / 2;
		if (model_mach[mid] >= m) {
			right = mid;
		} else {
			left = mid + 1;
		}
	}
	size_t i = left;

	double m_prev = model_mach[i - 1];
	double m_curr = model_mach[i];
	double cd_prev = model_cd[i - 1];
	double cd_curr = model_cd[i];

    double cd = cd_prev
				+ (cd_curr - cd_prev)
				  * (m - m_prev)
				  / (m_curr - m_prev);
    return BC_CONVERSION_FACTOR
		   * (cd / ballistic_coefficient)
		   * pow(velocity, 2);
}

double
retard(DragFunction drag_function,
	   double ballistic_coefficient,
	   double velocity,
	   double temperature)
{
	return calculate_retard(drag_function,
							ballistic_coefficient,
							velocity,
							speed_of_sound(temperature));
}