### Simple P-Control Test
## Constants
f_base = 60     # Nomimal frequency under normal condition
f_0 = 1         # Normalized frequency
# I_G = 100     # Rotational Inertia of the generator
P_mech_0 = 1000 # Normalized initial mechanical power exerted on the generator
P_el_0 = 1001   # Normalized initial electrical power given by the generator
# K_p = 500     # Stiffness of the primary frequency control
# k_R = 1       # Reaction Coefficient of Primary Frequency Control
D_el = 0        # Response Coefficient of the Load due to Change of Frequency

## Function for time series of frequency
# I_G = Rotational Inertia of the generator
# K_p = Stiffness of the primary frequency control (P_total / delta_f_max)
TS_freq <- function(I_G, K_p, k_R){
  ## Solving for the Constants
  A = matrix(c(0 , 1, -k_R * K_p / I_G / f_0, k_R * (-D_el / I_G / f_0 - 1)), nrow = 2, byrow = T)
  Eigen = eigen(A)
  
  B = matrix(c(Eigen$vectors[1,], Eigen$values * Eigen$vectors[1,]), nrow = 2, byrow = T)
  value_0 = c(-(P_mech_0 - P_el_0) / K_p, (P_mech_0 - P_el_0) / I_G / f_0)
  const = solve(B, value_0)
  
  ## Dynamic of the System
  f = const[1] * exp(Eigen$values[1] * Time) * Eigen$vectors[1, 1] + const[2] * exp(Eigen$values[2] * Time) * Eigen$vectors[1, 2]
  f_0_new = f_0 + (P_mech_0 - P_el_0) / K_p
  
  plot(Time, (f_0_new + f) * f_base, type = "l")
  
  return((f_0_new + f) * f_base)
}

## Test: K_P Proportional to I_G (VRE don't do primary control)
dt = .001
Time = (0:10000) * dt

freq_0 = TS_freq(125, 62.5, 1)
freq_1 = TS_freq(250, 125, 1)
freq_2 = TS_freq(500, 250, 1)
freq_3 = TS_freq(1000, 500, 1)

plot(Time, freq_0, type = "l", ylab = "Frequency (1/s)", xlab = "Time (s)")
lines(Time, freq_1, lty = 2)
lines(Time, freq_2, col = "red")
lines(Time, freq_3, col = "red", lty = 2)

## Test: K_P Independent of I_G (VRE don't do primary control)
dt = .001
Time = (0:10000) * dt

freq_0 = TS_freq(125, 500, 1)
freq_1 = TS_freq(250, 500, 1)
freq_2 = TS_freq(500, 500, 1)
freq_3 = TS_freq(1000, 500, 1)

plot(Time, freq_0, type = "l", ylab = "Frequency (1/s)", xlab = "Time (s)")
lines(Time, freq_1, lty = 2)
lines(Time, freq_2, col = "red")
lines(Time, freq_3, col = "red", lty = 2)

## Test: Different K_P and Same I_G
dt = .001
Time = (0:10000) * dt

freq_0 = TS_freq(250, 125, 1)
freq_1 = TS_freq(250, 250, 1)
freq_2 = TS_freq(250, 500, 1)
freq_3 = TS_freq(250, 1000, 1)

plot(Time, freq_0, type = "l", ylab = "Frequency (1/s)", xlab = "Time (s)")
lines(Time, freq_1, lty = 2)
lines(Time, freq_2, col = "red")
lines(Time, freq_3, col = "red", lty = 2)

## Test: Same k_R * I_G
dt = .001
Time = (0:10000) * dt

freq_0 = TS_freq(125, 500, 1)
freq_1 = TS_freq(250, 500, 2)
freq_2 = TS_freq(500, 500, 4)
freq_3 = TS_freq(1000, 500, 8)

plot(Time, freq_0, type = "l", ylab = "Frequency (1/s)", xlab = "Time (s)")
lines(Time, freq_1, lty = 2)
lines(Time, freq_2, col = "red")
lines(Time, freq_3, col = "red", lty = 2)