#
# Copyright 2022 Philipp Schönberger - mail@phschoen.de
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# See <http://www.gnu.org/licenses/>.
clearvars
clear
close all


  finger_h          = [ 10,       15,    18,      28    ];
  finger_translate  = [     20,       25,       35      ];
  finger_min_angles = [     -5,       -5,       -5  ];
  finger_max_angles = [     85,       90,       70 ];
  finger_min_translate  = [ 20];
  finger_max_translate  = [ 0];


  switch_offset =20;
  home_row = 1/3;

  home_row_angles =  finger_min_angles + home_row * (finger_max_angles - finger_min_angles);
  home_row_trans =  finger_min_translate + home_row * (finger_max_translate - finger_min_translate);
  home_pos = calc_finger_pos(home_row_angles, home_row_trans, finger_translate);

 X=[];
 Y=[];
 Z=[];
for i = [0:100]/100
  angle =  finger_min_angles + i * (finger_max_angles - finger_min_angles);
  trans =  finger_min_translate + i * (finger_max_translate - finger_min_translate);
  pos = calc_finger_pos(angle,trans, finger_translate);

  X = [X, pos(1)];
  Y = [Y, pos(2)];
  Z = [Z, pos(3)];
end

plot (Y,Z,"p");
i = [0:100]/100;
i_f = [0:3600]/3600;
lin_x = interp1 (i, X, i_f, "spline");
lin_y = interp1 (i, Y, i_f, "spline");
lin_z = interp1 (i, Z, i_f, "spline");

figure(1);
plot (Y, Z, "s", lin_y, lin_z, "r");
hold on;
plot (home_pos(2), home_pos(3), "markersize", 75);

axis equal


[row_p1,row_m1 ] = find_next_switch_pos(home_pos, X,Y,Z,switch_offset);
[row_p2,foo ] = find_next_switch_pos([X(row_p1),Y(row_p1),Z(row_p1)], X,Y,Z,switch_offset);
[row_p3,row_0 ] = find_next_switch_pos([X(row_p2),Y(row_p2),Z(row_p2)], X,Y,Z,switch_offset);
[foo, row_m2 ] = find_next_switch_pos([X(row_m1),Y(row_m1),Z(row_m1)], X,Y,Z,switch_offset);
[foo, row_m3 ] = find_next_switch_pos([X(row_m2),Y(row_m2),Z(row_m2)], X,Y,Z,switch_offset);


plot (Y(row_0), Z(row_0), "markersize", 50);
plot (Y(row_p1), Z(row_p1), "markersize", 50);
plot (Y(row_p2), Z(row_p2), "markersize", 50);
#plot (Y(row_p3), Z(row_p3), "markersize", 50);

plot (Y(row_m1), Z(row_m1), "markersize", 50);
plot (Y(row_m2), Z(row_m2), "markersize", 50);
#plot (Y(row_m3), Z(row_m3), "markersize", 50);

home_angle = calc_angle_switch(row_0,round(abs(row_0-row_m1)/3),X,Y,Z);
p1_angle = calc_angle_switch(row_p1,round(abs(row_p1-row_0)/3),X,Y,Z);
p2_angle = calc_angle_switch(row_p2,round(abs(row_p2-row_p1)/3),X,Y,Z);
p3_angle = calc_angle_switch(row_p3,round(abs(row_p3-row_p2)/3),X,Y,Z);

m1_angle = calc_angle_switch(row_m1,round(abs(row_m1-row_0)/3),X,Y,Z);
m2_angle = calc_angle_switch(row_m2,round(abs(row_m2-row_m1)/3),X,Y,Z);
m3_angle = calc_angle_switch(row_m3,round(abs(row_m3-row_m2)/3),X,Y,Z);

#[ foo, row_p1] = min(abs(switch_offset - D));


angle_switch_x = [p2_angle, p2_angle,home_angle,m1_angle,m2_angle];
pos_switch_x = [X(row_p2),X(row_p1),X(row_0),X(row_m1),X(row_m2)];
pos_switch_y = [Y(row_p2),Y(row_p1),Y(row_0),Y(row_m1),Y(row_m2)];
pos_switch_z = [Z(row_p2),Z(row_p1),Z(row_0),Z(row_m1),Z(row_m2)];
plot (Y(row_m1), Z(row_m1), "markersize", 50);
#plot (Y(row_p1), Z(row_p1), "markersize", 50);
 % Noisy data
## x = linspace (0, 2*pi, 100);
## y = sin (x) + 0.1 * randn (size (x));
## % Breaks
## breaks = [0:5, 2*pi];
## % Constraints: y(0) = 0, y'(0) = 1 and y(3) + y"(3) = 0
## xc = [0 0 3];
## yc = [0 1 0];
## cc = [1 0 1; 0 1 0; 0 0 1];
## con = struct ("xc", xc, "yc", yc, "cc", cc);
## % Fit a cubic spline with 8 pieces and constraints
## pp = splinefit (x, y, 8, "constraints", con);
## clf;
## plot (x, y, "s", x, ppval (pp, x), "r", breaks, ppval (pp, breaks), "+r");
## xlabel ("Independent Variable");
## ylabel ("Dependent Variable");
## title ("Fit a cubic spline with constraints");
## legend ({"data", "fit", "breaks"});
## axis tight
## ylim auto