//                   tip  dip   pip  mcp
finger_h          = 0.1*[
    [ 10,       15,    18,      28    ], [ 10,       15,    18,      28    ], [ 10,       15,    18,      28    ], [ 10,       15,    18,      28    ], [ 10,       15,    18,      28    ]];
finger_o          = [
    [ 10,       15,    18,      28    ], [ 10,       15,    18,      28    ], [ 10,       15,    18,      28    ], [ 10,       15,    18,      28    ], [ 10,       15,    18,      28    ]];
finger_translate  = [
    [     20,       40,       40      ], [     23,       20,       40      ], [     20,       40,       35      ], [     20,       40,       35      ], [     20,       40,       35      ]];
finger_max_angles = [[    [-5,85],  [-5,90],  [-5,70]  ],[    [-5,85],  [-5,90],  [-5,70]  ],[    [-5,85],  [-5,90],  [-5,70]  ],[    [-5,85],  [-5,90],  [-5,70]  ],[    [-5,85],  [-5,90],  [-5,70]  ]];
finger_offset_trans_from_index=[[-20,0,-40],[0,0,0],[-23,3,3],[-43,2,1],[-64,0,0]];
finger_offset_rot_from_index=[[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]];

finger_curr_angles = [[0,10,0],[0,10,0],[0,10,0],[0,10,0],[0,10,0]];

// uncoment to do sync movement
//finger_curr_angles = [ calc_angles(0,abs($t-0.5)*2),calc_angles(0,abs($t-0.5)*2),calc_angles(0,abs($t-0.5)*2),calc_angles(0,abs($t-0.5)*2),calc_angles(0,abs($t-0.5)*2)];
// sanity check



module finger(idx,finger_curr_angles,show_blob_only) {

    if(len(finger_h[idx]) != len(finger_translate[idx]) +1)
        echo("finger_h does not match to finger_translate");
    if(len(finger_h[idx]) != len(finger_max_angles[idx]) +1)
        echo("finger_h does not match to finger_max_angles");
    if(len(finger_max_angles[idx][0]) != 2)
        echo("finger_max_angles does not have 2 entries");

    translate(finger_offset_trans_from_index[idx])
    rotate(finger_offset_rot_from_index[idx])
    {
        for(r_idx = [0:len(finger_translate[idx])-1])
        {
            rotate([r_idx<=2 ? finger_curr_angles[2] : 0,0,0])
            translate([0,0,r_idx<2 ? finger_translate[idx][2] : 0])
            rotate([r_idx<=1 ? finger_curr_angles[1] : 0,0,0])
            translate([0,0,r_idx<1 ? finger_translate[idx][1] : 0])
            rotate([r_idx<=0 ? finger_curr_angles[0] : 0,0,0])
            //translate([0,0,r_idx<0 ? finger_translate[idx][0] : 0])
            if (show_blob_only == true)  {
                if (r_idx == 0)
                   translate([0,0,finger_translate[idx][0]])
                        sphere(d=2);
            }else {
                cylinder(d2=finger_h[idx][r_idx],d1=finger_h[idx][r_idx+1] , h=finger_translate[idx][r_idx]);
            }
        }
        //translate([-20,-15,-20]) cube([40,30,20]);
        sphere(d=10);
    }
}


translate([140,125,120])
rotate([-25,17+6,05])
rotate([-90,0,0])
rotate([0,0,180])
{
    translate([20-100,0,-50])
    cube([100,5,50]);
    for(idx=[1:4]) {
        finger(idx,finger_curr_angles[idx]);

        for (i=[0:0.01:1])
            finger(idx,calc_angles(idx,i),show_blob_only=true);
    }
}

function calc_angles(idx,t)=[
    finger_max_angles[idx][0][1] + t * (finger_max_angles[idx][0][0] - finger_max_angles[idx][0][1]),
    finger_max_angles[idx][1][1] + t * (finger_max_angles[idx][1][0] - finger_max_angles[idx][1][1]),
    finger_max_angles[idx][2][1] + t * (finger_max_angles[idx][2][0] - finger_max_angles[idx][2][1]),
    ];

function pi()=3.14159265358979323846;

function m_transpose(m) =
let(
    column = len(m[0]),
    row = len(m)
)
[
    for(y = 0; y < column; y = y + 1)
    [
        for(x = 0; x < row; x = x + 1)
        m[x][y]
    ]
];

function transpose(m) =
let(
    column = len(m),
    row = 1
)
[
    for(y = 0; y < column; y = y + 1)
    [
        m[y]
    ]
];

function sum(lt) =
    let(end = len(lt) - 1)
    end == 0 ? lt[0] :
    let(
        cum_total = [
            for(i = 0, s = lt[0], is_continue = i < end;
                is_continue;
                i = i + 1, is_continue = i < end, s = is_continue ? s + lt[i] : undef) s]
    )
    cum_total[end - 1] + lt[end];
module line(c="blue",p1=[0,0,0],p2,d=0.05)
{
    color(c)
    hull() {
        translate ( p1*2.54) sphere(d);
        translate ( p2*2.54) sphere(d);
    }
    y = acos( (( p1*2.54)-( p2*2.54))[1] / sqrt( (( p1*2.54)-( p2*2.54))[0] *((p1*2.54)-( p2*2.54))[0] + ((p1*2.54)-( p2*2.54))[1]*((p1*2.54)-( p2*2.54))[1]));
    x = acos( (( p1*2.54)-( p2*2.54))[1] / sqrt( (( p1*2.54)-( p2*2.54))[0] *((p1*2.54)-( p2*2.54))[0] + ((p1*2.54)-( p2*2.54))[1]*((p1*2.54)-( p2*2.54))[1]));
    echo([ p2*2.54, [0,
       atan((sqrt(
                (
                ((p1-p2)*2.54 )[0]*((p1-p2)*2.54 )[0]
                +
                ((p1-p2)*2.54 )[1]*((p1-p2)*2.54 )[1]
                ) )/
                ((p1-p2)*2.54 )[2] )),
       atan( ((p1-p2)*2.54 )[1] /((p1-p2)*2.54 )[0] ) ]]);
}

// scale since patent is in inch
//scale([27.4,25.4,25.4])
{
// finger thumb
line("magenta" ,[2.666560,3.808380,1.894690],[2.660100,3.769870,2.090850]);
line("magenta", [1.980350,3.553940,1.815560],[1.973320,3.510470,2.010680]);
line("magenta", [2.261190,2.888580,1.692630],[2.245370,2.853450,1.888880]);
line("magenta", [2.534610,2.192950,1.774750],[2.534420,2.232090,1.578620]);
line("magenta", [3.121180,2.762650,1.907540],[3.118430,2.801320,1.711340]);
line("magenta", [3.798070,3.053630,1.922090],[3.787220,3.098700,1.727540]);
for(i = [
            [[6.75665, 9.57547, 5.31076], [0, -11.2583, 80.4774]] ,
            [[5.01223, 8.91659, 5.10713], [0, -12.7175, 80.8136]] ,
            [[5.70324, 7.24776, 4.79776], [0, -11.107, 65.7567]]  ,
            [[6.43743, 5.66951, 4.00969], [0, 11.2859, -89.7219]] ,
            [[7.92081, 7.11535, 4.3468], [0, 11.1772, -85.9323]]  ,
            [[9.61954, 7.8707, 4.38795], [0, 13.4026, -76.4644]]  ,
            ])
color("magenta")
translate(i[0])
rotate(i[1])
cylinder(d=0.05, h =10);


for(i = [
         [[9.36308, 16.6995, 5.18671]   , [0, 41.7436,-40.9591],  ], // red 1
         [[9.15197, 15.116, 4.46027]    , [0, 35.82,  -27.665],   ], // red 2
         [[9.14731, 13.1702, 4.05678]   , [0, 31.6187, -8.27799], ], // red 3
         [[9.34928, 11.1944, 3.99245]   , [0, 30.8097, 13.0736],  ], // red 4
            ])
color("red")
translate(i[0])
rotate(i[1])
cylinder(d=0.05, h =10);


for(i = [
         [ [11.1253, 17.2887, 4.37277], [0, 36.5622, -58.15]    ], // green 0
         [ [10.9842, 15.4535, 3.5566], [0, 26.4538, -42.1509]  ], // green 1
         [ [10.9941, 13.4816, 3.14991], [0, 19.7498, -13.0805]  ], // green 2
         [ [11.1546, 11.4748, 3.17369], [0, 20.1998, 25.738]    ], // green 3
         [ [11.4574, 9.53647, 3.62672], [0, 27.4539, 53.0897]   ], // green 4
            ])
color("green")
translate(i[0])
rotate(i[1])
cylinder(d=0.05, h =9);


for(i = [
[[12.9567, 17.5306, 3.76933], [0, 41.261, -73.2541]  ],
[[12.8667, 15.7504, 2.71251], [0, 25.0898, -61.3791] ],
[[12.9038, 13.7373, 2.22214], [0, 12.2935, -21.7835] ],
[[13.0646, 12.4369, 2.3429],  [0, 12.1358, 32.3507]  ],
[[13.3344, 9.75092, 3.06377], [0, 31.2974, 74.9369]  ],
            ])
color("blue")
translate(i[0])
rotate(i[1])
cylinder(d=0.05, h =7);

for(i = [
[[14.9523, 17.5466, 3.60612], [0, 33.4625, -77.0026]]  ,
[[14.9599, 15.7283, 2.71953], [0, 20.3697, -69.2842]]  ,
[[15.0261, 13.7548, 2.27958], [0, 8.62728, -39.7016]]  ,
[[15.1474, 11.7358, 2.31069], [0, 9.91521, 53.965]]    ,
[[15.3169, 9.78303, 2.81112], [0, 22.0582, 77.4566]]   ,
            ])
color("brown")
translate(i[0])
rotate(i[1])
cylinder(d=0.05, h =8.5);



for(i = [
[[17.0407, 16.9158, 3.60332], [0, 29.4563, -77.3251]] ,
[[17.1098, 13.0971, 2.51373], [0, 6.38541, -25.8394]] ,
[[17.2136, 11.1048, 2.63309], [0, 11.3871, 63.4095]]  ,
[[17.3572, 9.19213, 3.19471], [0, 23.5255, 79.5863]]  ,
            ])
color("gray")
translate(i[0])
rotate(i[1])
cylinder(d=0.05, h =8.9);

/* // finger 1 */
/* line("red",     [5.626412,4.890490,4.921143],[3.603139,5.951178,1.756010]); */
/* line("red",     [5.626412,4.890490,4.921143],[3.601304,5.185127,1.597157]); */
/* line("red",     [5.626412,4.859049,4.921143],[3.680820,4.407240,1.571830]); */
/* line("red",     [5.626412,4.890490,4.921143],[3.686250,6.574620,2.042010]); */
/* // finger 2 */
/* line("green",   [5.476960,5.040864,4.524379],[4.324486,6.084067,1.400235]); */
/* line("green",   [5.476960,5.040864,4.524379],[4.328402,5.307730,1.240122]); */
/* line("green",   [5.476960,5.040864,4.524379],[4.391585,4.517622,1.249485]); */
/* line("green",   [5.476960,5.040864,4.524379],[4.510779,3.754514,1.427841]); */
/* line("green",   [5.476960,5.040864,4.524379],[4.380041,6.806573,1.721562]); */
/* line("blue",    [5.614666,5.194831,3.515830],[5.065645,6.200934,1.067919]); */
/* line("blue",    [5.614666,5.194831,3.515830],[5.080251,5.408403,0.874860]); */
/* line("blue",    [5.614666,5.194831,3.515830],[5.143544,4.896416,0.922400]); */
/* line("blue",    [5.614666,5.194831,3.515830],[5.249757,3.838946,1.206208]); */
/* line("blue",    [5.614666,5.194831,3.515830],[5.101057,6.901806,1.483988]); */
// finger 3
/* line("brown",   [6.306084,5.091255,4.240877],[5.889712,6.192233,1.070680]); */
/* line("brown",   [6.306084,5.091255,4.240877],[5.915799,5.415294,0.897474]); */
/* line("brown",   [6.306084,5.091255,4.240877],[5.963534,4.620381,0.909720]); */
/* line("brown",   [6.306084,5.091255,4.240877],[6.030270,3.851587,1.106739]); */
/* line("brown",   [6.306084,5.091255,4.240877],[5.886720,6.908103,1.419731]); */
/* // finger 4 */
/* line("gray",    [7.085053,4.987385,4.453756],[6.736145,5.156350,0.989659]); */
/* line("gray",    [7.085053,4.987385,4.453756],[6.777004,4.371971,1.036648]); */
/* line("gray",    [7.085053,4.987385,4.453756],[6.708940,6.659750,1.418630]); */
/* line("gray",    [7.085053,4.987385,4.453756],[6.833560,3.618950,1.257760]); */
/* line("cyan",    [7.736480,5.132073,3.684937],[7.569335,6.085933,1.084243]); */
/* line("cyan",    [7.736480,5.132073,3.684937],[7.606606,5.316667,0.918996]); */
/* line("cyan",    [7.736480,5.132073,3.684937],[7.654340,4.532530,0.976580]); */
/* line("cyan",    [7.736480,5.132073,3.684937],[7.545530,6.778350,1.459020]); */

for(i = [
[[19.1656, 17.217, 3.70591], [0, 36.6697, -83.3839]]  ,
[[19.2261, 15.4583, 2.75398], [0, 20.4233, -80.061]]  ,
[[19.3208, 13.5043, 2.33425], [0, 4.66506, -54.8711]] ,
[[19.442, 11.5126, 2.48051], [0, 12.595, 82.1988]]    ,
            ])
color("cyan")
translate(i[0])
rotate(i[1])
cylinder(d=0.05, h =7);
}