# integrates glider motion ODEs from specified initial conditions
library(deSolve)

# Parameters
rho <- 1.225   # Air density (kg/m^3)
S <- 15        # Reference area (m^2)
m <- 300       # Glider mass (kg)
g0 <- 9.81     # Acceleration due to gravity at sea level (m/s^2)
R <- 6371000   # Radius of the Earth (m)

# Lift and Drag Coefficients (functions of angle of attack)
lift_coefficient <- function(alpha) {
  max(0, 1.2 * alpha)
}

drag_coefficient <- function(alpha) {
  0.02 + 0.05 * alpha^2
}

# Lift function
lift <- function(v, Cl) {
  0.5 * rho * v^2 * S * Cl
}

# Drag function
drag <- function(v, Cd) {
  0.5 * rho * v^2 * S * Cd
}

# Gravity as a function of altitude
gravity <- function(y) {
  g0 * (R / (R + y))^2
}

# First-Order ODE System
glider_dynamics <- function(time, state, parameters) {
  with(as.list(c(state, parameters)), {
    u <- state[1]
    w <- state[2]
    theta <- state[3]
    x <- state[4]
    y <- state[5]
    
    v <- sqrt(u^2 + w^2)
    alpha <- theta  # Assuming angle of attack is equal to flight path angle (simplified)
    Cl <- lift_coefficient(alpha)
    Cd <- drag_coefficient(alpha)
    
    L <- lift(v, Cl)
    D <- drag(v, Cd)
    
    du_dt <- (L * sin(theta) - D * cos(theta)) / m
    dw_dt <- (L * cos(theta) - D * sin(theta) - m * gravity(y)) / m
    dtheta_dt <- (L * cos(theta) + D * sin(theta)) / (m * v)
    dx_dt <- u
    dy_dt <- w
    
    list(c(du_dt, dw_dt, dtheta_dt, dx_dt, dy_dt))
  })
}

# Initial Conditions
state_initial <- c(u = 15, w = 0, theta = 0.1, x = 0, y = 1000) # Initial speed 15 m/s, altitude 1000 m

# Time Span
times <- seq(0, 600, by = 0.1)  # Simulate for 10 minutes (600 seconds)

# Solve the ODE
out <- ode(y = state_initial, times = times, func = glider_dynamics, parms = NULL)

# Plot Trajectory
plot(out[, "x"], out[, "y"], type = "l", xlab = "Distance (m)", ylab = "Altitude (m)", main = "Glider Trajectory")
grid()