#### Voltage Control Along a Feeder
### System Parameters
num_PV = 20
R_L = rep(.001, num_PV)
X_L = rep(.01, num_PV)
Z_L = complex(real = R_L, imaginary = X_L)
S_max = 1
U_0 = 1
U_lim = c(.95, 1.05)

### Initial Conditions
S_ini = complex(real = rep(S_max, num_PV))
U_ini = sqrt(U_0^2 + 2 * abs(cumsum(rev(cumsum(rev(Re(S_ini)))) * Re(Z_L) + rev(cumsum(rev(Im(S_ini)))) * Im(Z_L))))

### Trading Process
Time = 35
U_history = matrix(NA, ncol = num_PV, nrow = Time + 1)
U_history[1, ] = U_ini
U_now = U_ini
S_now = S_ini
Increment = .0001
Violation_Max = (U_now > U_lim[2]) 
Violation_Min = (U_now < U_lim[1])

tick = 1
while(sum(Violation_Max + Violation_Min) > 0 && tick <= Time){
  Bid_Initiator = sort(runif(num_PV) * (Violation_Max + Violation_Min), index.return = T, decreasing = T)$ix[1:sum(Violation_Max + Violation_Min)]
  No_ID = 0
  
  for(ID in 1:length(Bid_Initiator)){
    if(Violation_Max[Bid_Initiator[ID]]){
      
      # Voltage Control from Agents Control VOltage
      delta_U_self_delta_U = U_now / U_now[Bid_Initiator[ID]]
      
      # P-Q Dependencies for each Agent
      delta_Q_delta_P = (sqrt(S_max^2 - (Re(S_now) - Increment)^2)  - abs(Im(S_now))) * Im(cumsum(Conj(Z_L))) / abs(Im(cumsum(Conj(Z_L))))
      
      # Upstream: Voltage Affects Downstream Voltage
      delta_U_delta_P = (Re(cumsum(Z_L)) * (-Increment) + delta_Q_delta_P * Im(cumsum(Z_L))) / U_now  * (1:num_PV <= Bid_Initiator[ID])
      # Downstream: Power Affects Upstream Power & Voltage
      delta_U_delta_P = delta_U_delta_P  + (Re(cumsum(Z_L)[Bid_Initiator[ID]]) * (-Increment) + delta_Q_delta_P * Im(cumsum(Z_L)[Bid_Initiator[ID]])) / U_now  * (1:num_PV > Bid_Initiator[ID])
      delta_U_delta_P = -delta_U_delta_P / Increment
      
      # Agents Determine Sensitivity of Their Reduced Power to VOltage Change at the Initiator's Bus
      Feasible = (Re(S_now) >= Increment) * (abs(complex(real = Re(S_now) - Increment, imaginary = Im(S_now) + delta_Q_delta_P)) <= S_max)
      delta_U_self_delta_P = delta_U_self_delta_U * delta_U_delta_P * Feasible
      
      if(sum(delta_U_self_delta_P) != 0){
        Confirm_Bid = sort(delta_U_self_delta_P, index.return = T, decreasing = T)$ix[1]
        S_now[Confirm_Bid] = S_now[Confirm_Bid] - complex(real = Increment)
        S_now[Confirm_Bid] = S_now[Confirm_Bid] + complex(imaginary = sqrt(S_max^2 - Re(S_now[Confirm_Bid])^2) - abs(Im(S_now[Confirm_Bid]))) * Im(cumsum(Conj(Z_L))[Confirm_Bid]) / abs(Im(cumsum(Conj(Z_L))[Confirm_Bid]))
        
        U_now = sqrt(U_0^2 + 2 * abs(cumsum(rev(cumsum(rev(Re(S_now)))) * Re(Z_L) + rev(cumsum(rev(Im(S_now)))) * Im(Z_L))))
        U_history[tick + 1, ] = U_now
      }else{
        No_ID = No_ID + 1
      }
    }
  }
  
  if(No_ID != sum(Violation_Max + Violation_Min)){
    Violation_Max = (U_now > U_lim[2])
    Violation_Min = (U_now < U_lim[1])
  }
  else{
    break
  }

  print(paste("Time: ", tick, sep = ""))
  tick = tick + 1
}

plot(U_history[, num_PV], type = "l", ylim = range(U_history, na.rm = T), ylab = "Voltage (p.u.)", xlab = "Trading Rounds")
lines(rep(U_lim[2], nrow(U_history) - 1), lty = 2, col = "red")
for(ID in 1:(num_PV - 1)){
  lines(U_history[, ID], col = rgb(runif(1, 0, 1), runif(1, 0, 1), runif(1, 0, 1)))
}