close all
clear all
global theta;
global theta_prime;
global mag gamma;
global updated;


nu = 1e-6;  %kinematic viscosity (m^2/s)
rho = 1025;  %water density (kg/m^3)
updated = 1;

%Frame characteristics
Fr_w = 1;
Fr_t = 3/100;
Fr_o = [0 0];

Fr_OrigPts(1,:) = [Fr_w/2 -Fr_w/2 -Fr_w/2 Fr_w/2 Fr_w/2];
Fr_OrigPts(2,:) = [Fr_t/2 Fr_t/2 -Fr_t/2 -Fr_t/2 Fr_t/2];

%Foil characteristics
F_c = 12/39.4;  %Chord length (m)
F_d = .15*F_c;  %Shaft distance from leading edge
F_t = 12/100;  %Thickness
F_l = 2;  %Foil length (m)
F_area = F_c*F_l;

F_theta_l = 0;
F_theta_r = 0;

F_Origin_left =  [-Fr_w/2+0.25*F_c 0]';
F_Origin_right = [+Fr_w/2-0.25*F_c 0]';

x = 0:.1:1;
x =  (1-cos(x*pi/2));
y = NACA_4Digit_Ordinates(F_t,x);
F_OrigPts_right(1,:) = F_c*[1-x 1-x(end:-1:1)]-(F_c-F_d);
F_OrigPts_right(2,:) = F_c*[y -y(end:-1:1)];

F_OrigPts_left = rot(F_OrigPts_right,180,[0 0]);





fh1 = figure('Position',[ 300  100   1200   1000]);

hold on;
%z = rot(Fr_OrigPts,0,[0 0]); h_Fr = patch(z(1,:),z(2,:),[.7 .7 .7])
%z = rot(F_OrigPts_left,0,F_Origin_left); h_F_left = patch(z(1,:),z(2,:),'y');
%z = rot(F_OrigPts_right,0,F_Origin_right); h_F_right = patch(z(1,:),z(2,:),'y');
h_Fr = patch([1],[1],[.7 .7 .7])
h_F_left = patch([1],[1],'y');
h_F_right = patch([1],[1],'y');
h_line = line([0 0],[0 0]);
set(h_line,'Color','r');

h_line_left_force = line([0 0],[0 0]);
h_line_right_force = line([0 0],[0 0]);
h_line_total_force = line([0 0],[0 0]);
set(h_line_total_force,'Color','k');

xlim([-1 1]);
ylim([-1 1]);
%axis equal
mydata = 12;
set(fh1,'WindowButtonDownFcn',{@Click_Callback,h_line})
set(fh1,'Units','normalized')
%h_axis = get(fh1,'CurrentAxes');

theta = 0;
ThetaText = 'theta = ';
th1 = text(-1.03,-1.12,[ThetaText num2str(theta,3)],'FontSize',14);
sh1 = uicontrol(fh1,'Style','slider',...
                'Max',180,'Min',-181,'Value',theta,...
                'SliderStep',[0.01 0.05],'Units','normalized',...
                'Position',[.05 0.02 .2 .02],'Callback',{@ThetaSlider_Callback,th1,ThetaText});   

theta_prime = 0;
ThetaPrimeText = 'theta\_prime = ';
th2 = text(-0.42,-1.12,[ThetaPrimeText num2str(theta_prime,3)],'FontSize',14);
sh2 = uicontrol(fh1,'Style','slider',...
                'Max',90,'Min',-91,'Value',theta_prime,...
                'SliderStep',[0.01 0.05],'Units','normalized',...
                'Position',[.3 0.02 .2 .02],'Callback',{@ThetaPrimeSlider_Callback,th2,ThetaPrimeText});               

MomentText = 'Net Moment = '            
h_text1 = text(-.9,-.7,[MomentText num2str(0)],'FontSize',16)            
            
R = eye(2);
gamma = 0;
mag = 0;
theta = 0;
theta_prime = 0; 
theta_0 = 180;
x_last = zeros(1,6);
x_last(3) = theta_0;
x_last(6) = theta_0;
while(1)
  while(updated == 0)  %Wait here until update flag is set.
      pause(0.01);
  end
                     %gamma is the direction the frame is moving (-180->180 deg)
  beta = gamma+180  %beta is the direction the flow is going relative to the fixed coordinate system (0->360 deg)
                     %mag is the flow speed in m/s.
  Re = mag*F_c/nu;
   
                     
  if(theta > -180)
  theta_left = theta;  %Angle of foil relative to the frame
  theta_right = theta;
  Fr_theta = theta_prime;
  disp('Computing Forces based on specified angles and velocity');
  %Left Side
  alpha_left = theta_prime+theta_left-beta;  %Aoa of left foil
  disp(['alpha_left = ' num2str(alpha_left)]);
  [CL CD CM] = LiftDragCoeff(Re,alpha_left);
  F_lift_left = CL*0.5*rho*F_area*mag^2;
  F_drag_left = CD*0.5*rho*F_area*mag^2;
  F_left_force_mag = sqrt(F_lift_left^2+F_drag_left^2);
  F_left_force_angle = beta - atan2(F_lift_left,F_drag_left)*180/pi;
  F_left_force = F_left_force_mag*[cos(F_left_force_angle*pi/180) sin(F_left_force_angle*pi/180)];
  left_origin = rot(F_Origin_left,Fr_theta,[0 0]);
  M_left_origin = cross([left_origin ; 0],[F_left_force 0]);
  M_left_origin = M_left_origin(3);
  
  %Right Side
  alpha_right = 180+theta_prime-theta_right-beta;  %Aoa of right foil
  disp(['alpha_right = ' num2str(alpha_right)]);
  [CL CD CM] = LiftDragCoeff(Re,alpha_right);
  F_lift_right = CL*0.5*rho*F_area*mag^2;
  F_drag_right = CD*0.5*rho*F_area*mag^2;
  F_right_force_mag = sqrt(F_lift_right^2+F_drag_right^2);
  F_right_force_angle = beta - atan2(F_lift_right,F_drag_right)*180/pi;
  F_right_force = F_right_force_mag*[cos(F_right_force_angle*pi/180) sin(F_right_force_angle*pi/180)];
  right_origin = rot(F_Origin_right,Fr_theta,[0 0]);
  M_right_origin = cross([right_origin ; 0],[F_right_force 0]);
  M_right_origin = M_right_origin(3);
  
  else
      disp('Computing Forces and Foil Angles');

      k = .5;
      options = optimset('TolFun',.1)
      x = fsolve(@(x)SumForces_1(x, F_c, F_d, nu, rho, F_area, k, theta_0),x_last,options);
      x_last = x;
      theta_left = x(3);
      F_left_force = [x(1) x(2)];
      theta_right = x(6);
      F_right_force = [x(4) x(5)];
      
      left_origin = rot(F_Origin_left,Fr_theta,[0 0]);
      M_left_origin = cross([left_origin ; 0],[F_left_force 0]);
      M_left_origin = M_left_origin(3)
      right_origin = rot(F_Origin_right,Fr_theta,[0 0]);
      M_right_origin = cross([right_origin ; 0],[F_right_force 0]);
      M_right_origin = M_right_origin(3);
  end
  
  
  %Update Plot
  
  z = rot(Fr_OrigPts,Fr_theta,[0 0]); 
     set(h_Fr,'Xdata',z(1,:)); set(h_Fr,'Ydata',z(2,:));    
  z = rot(F_OrigPts_left,Fr_theta+theta_left,rot(F_Origin_left,Fr_theta,[0 0])); 
     set(h_F_left,'Xdata',z(1,:)); set(h_F_left,'Ydata',z(2,:));   
  z = rot(F_OrigPts_right,Fr_theta-theta_right,rot(F_Origin_right,Fr_theta,[0 0])); 
     set(h_F_right,'Xdata',z(1,:)); set(h_F_right,'Ydata',z(2,:));   
     
  clear z;  %Left foil force arrow
  z(:,1) = rot(F_Origin_left,Fr_theta,[0 0]);
  %z(1,2) = z(1,1) + F_left_force_mag*cos(F_left_force_angle*pi/180)/100;
  %z(2,2) = z(2,1) + F_left_force_mag*sin(F_left_force_angle*pi/180)/100;
  z(1,2) = z(1,1) + F_left_force(1)/100;
  z(2,2) = z(2,1) + F_left_force(2)/100;
  set(h_line_left_force,'Xdata',z(1,:));
  set(h_line_left_force,'Ydata',z(2,:));

  clear z;  %Right foil force arrow
  z(:,1) = rot(F_Origin_right,Fr_theta,[0 0]);
  %z(1,2) = z(1,1) + F_right_force_mag*cos(F_right_force_angle*pi/180)/100;
  %z(2,2) = z(2,1) + F_right_force_mag*sin(F_right_force_angle*pi/180)/100;
  z(1,2) = z(1,1) + F_right_force(1)/100;
  z(2,2) = z(2,1) + F_right_force(2)/100;
  set(h_line_right_force,'Xdata',z(1,:));
  set(h_line_right_force,'Ydata',z(2,:));
  
  z_tot(:,1) = [0 0];
  z_tot(1,2) = z_tot(1,1) + F_left_force(1)/100 + F_right_force(1)/100;
  z_tot(2,2) = z_tot(2,1) + F_left_force(2)/100 + F_right_force(2)/100;
  
  %z_tot(1,2) = z_tot(1,1) + F_left_force_mag*cos(F_left_force_angle*pi/180)/100 + F_right_force_mag*cos(F_right_force_angle*pi/180)/100;
  %z_tot(2,2) = z_tot(2,1) + F_left_force_mag*sin(F_left_force_angle*pi/180)/100 + F_right_force_mag*sin(F_right_force_angle*pi/180)/100;
  
  set(h_line_total_force,'Xdata',z_tot(1,:));
  set(h_line_total_force,'Ydata',z_tot(2,:));
  
  set(h_text1,'String',[MomentText num2str(M_right_origin+M_left_origin)]);
  
  updated = 0;
end
