load('input_data.mat')

% windSpeed is wind speed data from Albany, NY
% windPower is the wind speed scaled up to 36kW, as this is what Union currently has
% timeBreakpoints is time for the Simulink model look up tables 
% SPD_813 is the solar power curve with max 750kW added to our current 63kW = 813kW
% exampleLoadPower is the load curve that comes with power_microgrid, not utilized in this code, but for reference
% Demand is the load curve from National Grid for Union on February 15th, 2019
% cogenPower is the cogen power data from National Grid on February 15th, 2019

%% Create the Optimization Problem 

energyprob = optimproblem;

solar = optimvar('solar',2,'Type','integer','LowerBound',0); % integer because we only want integer values of panels
battery = optimvar('battery',2,'Type','integer','LowerBound',0);
wind = optimvar('wind',2,'Type','integer','LowerBound',0);

costPanel = [96000, 265000];            % cost for each solar system, $
powerPanel = [30000, 100000];           % power output for each array, Watts
cloudy = 0.8;                           % 80 percent of power output on cloudy day
cloudyPanel = cloudy*[30000, 100000];   % output lessened by 20 percent

costBatteryPack = [400e3, 750e3];       % cost for each battery, $ 
energyBattery = [500e3, 1e6];           % energy for each battery, Wh
powerBattery = [100e3, 400e3];          % power that battery can give at an instant, from fully charged, W

costTurbine = [32899, 85000];           % cost for each turbine, $ 
powerTurbine = [5000, 20000];           % power output for each turbine, Watts
slow = 0.8;                             % 80 percent of power output on slow wind day
slowTurbine = slow*[5000, 20000];       % output lessened by 20 percent

costSolar = costPanel*solar;            % cost times number of arrays
costBattery = costBatteryPack*battery;  % cost times number of batteries
costWind = costTurbine*wind;            % cost times number of turbines
cost = costSolar + costBattery + costWind;    % total cost of new additions
energyprob.Objective = cost;            % make the objective function the cost

%% Baseline Before Additions

peakWind = 36e3;                        % installed kW
peakPV = 63e3;                          % installed kW
peakCogen = 1.8e6;                      % underestime by 0.1MW 
peakDemand = 3e6;                       % overestimate by 0.1MW

%% Constraints: Land Availability  

% to calculate: divide available square feet by 100 square feet to get approximate installable kW

realisticSolar = powerPanel*solar == 750e3;     % only have space for this much 
realisticWind = powerTurbine*wind >= 25e3;      % for now, assume we want a diverse mix so need to include wind

%% Constraints: Can Peak Power of New Microgrid Meet Peak Demand for Under $12 million? 

% on worst case day, by using the scaled down power output for cloudy and slow wind day

peakAdd = (cloudyPanel*solar + powerBattery*battery + slowTurbine*wind) >= (peakDemand - (peakPV + peakCogen + peakWind)); % peak that additions produce need to bring supply to meet demand 
cost = costSolar + costBattery + costWind <= 12e6;       % cost needs to be below $12 million

%% Constrain and solve

energyprob.Constraints.conpl = peakAdd;                     % new supply must meet peak demand 
energyprob.Constraints.conc = cost;                         % cost less than $12 million
energyprob.Constraints.realisticSolar = realisticSolar;     % space for panels 
energyprob.Constraints.realisticWind = realisticWind;       % space for turbines 
[sol,fval] = solve(energyprob);
sol.solar                                                   % display number of solar panels
sol.battery                                                 % display number of batteries
sol.wind                                                    % display number of wind turbines

%% Scale Solar & Wind Power Curves to Include Additions

addSolar = sol.solar(1)*powerPanel(1) + sol.solar(2)*powerPanel(2);        % calculate added solar power
addWind = sol.wind(1)*powerTurbine(1) + sol.wind(2)*powerTurbine(2);       % calculate added wind power

% ((total power being added / current peak of power curve) * power curve) + power curve 
% this creates the same shape curve with peak of added power, then sum with what we already have

newSolarPower = ((addSolar/(max(solarPower))))*solarPower + solarPower;     % on cloudy day
newWindPower = ((addWind)/(max(windPower)))*windPower + windPower;          % on slow wind speed day

cloudySolarPower = (cloudy*((addSolar/(max(solarPower))))*solarPower) + solarPower; % on cloudy day
slowWindPower = (slow*((addWind)/(max(windPower)))*windPower) + windPower;          % on slow wind speed day
 
%% Initialize Cogen, Geothermal, and Battery Power

C = ones(size(newSolarPower));              % initialize vector 1441x1
powerCogen = 2e6;                           % cogen power is 2MW all 24 hours
avgCogen = C*powerCogen;                    % cogen power is 2MW all 24 hours
battery = zeros(size(C));                   % initialize battery
sellBack = zeros(size(C));                  % initialize power sold back to main grid 

%% Battery Parameters 

number = 4;                             % number of 1 MWh lithium ion batteries
                                        % can be left as one, but for this case it will not be enough to meet demand
                                        % so scale this number up by one until it meets demand 
totalBatteryEnergy = sol.battery(1)*energyBattery(1) + number*sol.battery(2)*energyBattery(2);

%% Calculate Battery Power Curve 

totalGen = cloudySolarPower + slowWindPower + avgCogen;
energyDifference = trapz(Demand - totalGen);
geo = energyDifference/1441;        % extra energy needed throughout the day
geoPower = ones(size(C))*(geo);     % power curve
battery = Demand - (cloudySolarPower + slowWindPower + avgCogen + geoPower); % demand - supply, absorb surplus, discharge if not enough

%% Battery Control Scheme

% trapz(battery(1:t)) is trapezoidal integration of battery power to get battery energy
% multiplied by 1/60 to get from Watt-minutes to Watt-hours (Wh) 

for t=1:1:1441                                         
        if (trapz(battery(1:t))*(1/60)) <= -totalBatteryEnergy      % if energy absorbed is more than the rating
        battery(t) = 0;                                             % make battery power go to zero
        sellBack(t) = Demand(t) - (cloudySolarPower(t) + slowWindPower(t) + avgCogen(t) + geoPower(t));  % sell back surplus

                 if sellBack(t) > 0                                 % if sellBack goes positive for some reason
                 sellBack(t) = 0;                                   % make it zero
                 avgCogen(t) = Demand(t) - (cloudySolarPower(t) + slowWindPower(t) + geoPower(t) + battery(t)); % increase cogen instead
                 end
                 
                 if avgCogen(t) >= 2.2e6                            % set upper limit for cogen
                 geoPower(t) = Demand(t) - (cloudySolarPower(t) + slowWindPower(t) + avgCogen(t) + battery(t)); % if cogen goes higher than rating (2.2MW) then ramp up geothermal 
                 end
        end

        if (trapz(battery(1:t))*(1/60)) > totalBatteryEnergy        % if battery discharges more than rating 
        battery(t) = 0;                                             % battery caps at that energy, no energy is changed until it needs to either charge or discharge
        avgCogen(t) = Demand(t) - (cloudySolarPower(t) + slowWindPower(t) + geoPower(t) + battery(t));    % increase cogen to meet demand
        end
        
        if avgCogen(t) >= 2.2e6                                     % unless cogen reaches upper limit
        geoPower(t) = Demand(t) - (cloudySolarPower(t) + slowWindPower(t) + avgCogen(t) + battery(t)); % increase geothermal
        end
end

totalSupply = cloudySolarPower + slowWindPower + avgCogen + geoPower + battery + sellBack; % add solar, wind, cogen, geothermal, battery
newavgCogen = geoPower + slowWindPower + avgCogen;                        % for simulink model
 
%% Calculate Costs

geoCost = (2500/1e3)*geo;                                           % $2,500 per kW for geothermal
solarCost = sol.solar(1)*costPanel(1) + sol.solar(2)*costPanel(2);
windCost = sol.wind(1)*costTurbine(1) + sol.wind(2)*costTurbine(2);
batteryCost = sol.battery(1)*costBatteryPack(1) + sol.battery(2)*number*costBatteryPack(2); 
totalCost = geoCost + solarCost + windCost + batteryCost;

%% Check if Supply > Demand

if totalSupply >= Demand 
    fprintf('\nMicrogrid meets demand!\n')
    fprintf('\nOptimal Energy Mix:\n Added Solar %f kW,\n Added Wind %f kW,\n Cogen %f MW,\n Geothermal %f kW,\n Battery %f MWh,\n Total Cost: $%f\n',addSolar/10^3,addWind/10^3,powerCogen/10^6,geo/10^3,totalBatteryEnergy/1e6,totalCost) 
else
    fprintf('\nNeed more power')
end
 
figure(1)
plot(totalSupply,'bx','LineWidth',5)
hold on
plot(Demand,'r','LineWidth',1.7)
hold on
plot(battery,'g','LineWidth',2)
hold on
plot(cloudySolarPower,'m','LineWidth',1.5)
hold on
plot(avgCogen,'black','LineWidth',1.7)
hold on
plot(geoPower,'c','LineWidth',1.7)
hold on
plot(slowWindPower,'y-','LineWidth',1.5)
hold on
plot(sellBack,'b-','LineWidth',1.7)
title('Optimal Energy Mix','fontSize',18)
xlabel('time (seconds)','fontSize',14)
ylabel('MW','fontSize',14)
xlim([0 1441])
legend('Total Power Supply','Demand','Battery Power','Solar Power','Cogen Power','Geothermal','Wind Power','Sell Back','fontSize',14)