clear;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial Settings
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
cell_size = 0.4;
d = 12;
h = 3;
w = 1;

contour_above = 2;
contour_below = 2;
contour_right = 1;
contour_left = 1;
contour_width = 1;
contour_height = 1;

voltage = 10;
max_y = h + contour_above + 1;
e0 = 8.854E-12;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Calculate the # of cells needed
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
num_gridx = floor(d / cell_size)+1;
num_gridy = floor(max_y / cell_size)+1;
conductor_width = floor(w / cell_size);
epsilon2_boundary = floor(h / cell_size);
nodes_x = num_gridx - 2;
nodes_y = num_gridy - 2;
total_nodes = (nodes_x * nodes_y) - (conductor_width);

contour_width_right = floor(contour_right / cell_size);
contour_width_left = floor(contour_left / cell_size);
contour_height_above = floor(contour_above / cell_size);
contour_height_below = floor(contour_below / cell_size);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Calculate where the conductor is
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
voltage_x = floor((num_gridx / 2))+1;
voltage_y = epsilon2_boundary;

if (mod(conductor_width,2) == 1)
    voltage_right = voltage_x + floor(conductor_width / 2);
    voltage_left = voltage_x - floor(conductor_width / 2);
end

if (mod(conductor_width,2) == 0)
    voltage_left = voltage_x - floor(conductor_width / 2);
    voltage_right = voltage_x;
end
    

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Set boundary to 0 V
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1:num_gridy
    micro(i,1,1) = 0;
    micro(i,num_gridx,1) = 0;
end

for j = 1:num_gridx-1
    micro(1,j,1) = 0;
    micro(num_gridy,j,1) = 0;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Set Conductor to 10V
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = voltage_left: voltage_right
        micro(voltage_y,i,1) = voltage;
        micro(voltage_y,i,3) = voltage;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 3D Array to set nodes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
node_number = 1;
for k = 2:(num_gridy - 1)
    for j = 2:(num_gridx - 1)
        test_node = micro(k,j,1);
        if (test_node ~= voltage)
            micro(k,j,2) = node_number;
            node_number = node_number + 1;
        end
    end
end

for j = 1:num_gridx
    micro(1,j,2) = -1;
    micro(num_gridy,j,2) = -1;
end

for k = 1:num_gridy
    micro(k,1,2) = -1;
    micro(k,num_gridx,2) = -1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% First Matrix: Equal Arm Procedure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    for g = 2:(num_gridy-1)
        for h = 2:(num_gridx-1)
            a = micro(g,h+1,2);
            b = micro(g,h-1,2);
            c = micro(g+1,h,2);
            d = micro(g-1,h,2);
            center = micro(g,h,2);
            
            voltage_a = micro(g,h+1,1);
            voltage_b = micro(g,h-1,1);
            voltage_c = micro(g+1,h,1);
            voltage_d = micro(g-1,h,1);
            voltage_center = micro(g,h,1);

            if ((a > 0) && (center ~= 0))
                matrix(center,a) = 1;  
            end

            if ((b > 0) && (center ~= 0))
                matrix(center,b) = 1;  
            end

            if ((c > 0) && (center ~= 0)) 
                matrix(center,(center + nodes_x)) = 1;  
            end

            if ((d > 0) && (center ~= 0) && (g ~= (voltage_y+1)))
                matrix(center,(center - nodes_x)) = 1;  
            end
            
            if ((d > 0) && (center ~= 0) && (g == (voltage_y+1) && (h <= voltage_right)))
                matrix(center,(center - (nodes_x-(conductor_width)))) = 1;  
            end
            
            if ((d > 0) && (center ~= 0) && (g == (voltage_y+1) && (h > voltage_right)))
                    matrixa(center,(center - nodes_x)) = 1;  
            end

            if (center > 0)
                matrix(center,center) = -4;  
            end

        end
    end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Second Matrix Creation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1:total_nodes
    matrix2(i,1) = i;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Third Matrix Creation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    for k = 1:total_nodes
        matrix3(k,1) = 0;
    end
    
    
    for m = voltage_left: voltage_right
        voltage_node = micro(voltage_y,m,2);
        
        voltage_one = micro(voltage_y,m+1,1);
        voltage_two = micro(voltage_y,m-1,1);
        voltage_three = micro(voltage_y+1,m,1);
        voltage_four = micro(voltage_y-1,m,1);
        voltage_center = micro(voltage_y,m,1);
        
        one = micro(voltage_y,m+1,2);
        two = micro(voltage_y,m-1,2);
        three = micro(voltage_y+1,m,2);
        four = micro(voltage_y-1,m,2);
        loc_center = micro(voltage_y,m,2);
        
        if ((voltage_one ~= voltage) && (one ~= -1))
            matrix3(one,1) = -voltage;
        end
        
        if ((voltage_two ~= voltage) && (two ~= -1))
            matrix3(two,1) = -voltage;
        end
        
        if ((voltage_three ~= voltage) && (three ~= -1))
            matrix3(three,1) = -voltage;
        end
        
        if ((voltage_four ~= voltage) && (four ~= -1))
            matrix3(four,1) = -voltage;
        end
               
        if (((voltage_center == voltage) && (voltage_one ~= voltage) && (voltage_two ~= voltage) && (voltage_three ~= voltage) && (voltage_four ~= voltage)))
            matrix3(m-1,1) = 4*voltage;    
        end
    end

final = inv(matrix)*matrix3;
cond(matrix);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Contour Integration for NO Dielectric
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
starting_point_x = voltage_left - contour_width_left;
starting_point_y = voltage_y;

j = starting_point_x;
node_charge_save = 0;
for k = starting_point_y:(starting_point_y+contour_height_above)
    node_charge = 0;

    phi1_node = micro(k,j+1,2);
    phi3_node = micro(k,j-1,2);

    phi1 = final(phi1_node,1);
    phi3 = final(phi3_node,1);
    
    micro(k,j,3) = 15;

    node_charge = e0*cell_size*((phi1-phi3)/(2*cell_size));
    node_charge_save = node_charge_save + node_charge;
end

k = (starting_point_y+contour_height_above);
for j = (starting_point_x+1):(voltage_right + contour_width_right)
    node_charge = 0;
    
    phi1_node = micro(k-1,j,2);
    phi3_node = micro(k+1,j,2);
      
    phi1 = final(phi1_node,1);
    phi3 = final(phi3_node,1);
    
    micro(k,j,3) = 14;
    
    node_charge = e0*cell_size*((phi1-phi3)/(2*cell_size));
    node_charge_save = node_charge_save + node_charge;
end

j = (voltage_right + contour_width_right);
b = starting_point_y - contour_height_below;
for k = (starting_point_y+contour_height_above-1):-1:b
    node_charge = 0;
    phi_center_node = micro(k,j,2);
     
    phi1_node = micro(k,j-1,2);
    phi3_node = micro(k,j+1,2);
    
    phi1 = final(phi1_node,1);
    phi3 = final(phi3_node,1);
    
    micro(k,j,3) = 13;

    node_charge = e0*cell_size*((phi1-phi3)/(2*cell_size));
    node_charge_save = node_charge_save + node_charge;
end

k = (starting_point_y - contour_height_below);
for j = (voltage_right + contour_width_right-1):-1:(starting_point_x+1)
    node_charge = 0;
    phi_center_node = micro(k,j,2);
  
     
    phi1_node = micro(k,j-1,2);
    phi3_node = micro(k,j+1,2);
     
    phi1 = final(phi1_node,1);
    phi3 = final(phi3_node,1);
    
    micro(k,j,3) = 12;

    node_charge = e0*cell_size*((phi1-phi3)/(2*cell_size));
    node_charge_save = node_charge_save + node_charge;
end

j = starting_point_x;
for k = (starting_point_y - contour_height_below):(starting_point_y-1)
    node_charge = 0;
    phi_center_node = micro(k,j,2);
     
    phi1_node = micro(k,j-1,2);
    phi3_node = micro(k,j+1,2);   
    
    phi1 = final(phi1_node,1);
    phi3 = final(phi3_node,1);

    micro(k,j,3) = 11;
    
    node_charge = e0*cell_size*((phi1-phi3)/(2*cell_size));
    node_charge_save = node_charge_save + node_charge;
end

capacitance = node_charge_save / voltage;
capacitance = capacitance * (10E12);
fprintf('Co = %6.1f pF',capacitance);