// Copyright 2019 Rich Altmaier  richalt2@yahoo.com
//
//
//   This program is free software: you can redistribute it and/or modify
//   it under the terms of the GNU General Public License as published by
//   the Free Software Foundation, either version 3 of the License, or
//   (at your option) any later version.
//
//   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.
//
//   You should have received a copy of the GNU General Public License
//   along with this program.  If not, see <http://www.gnu.org/licenses/>.
//


//In a single .png file, use a row of colored boxes starting from lower left corner,
// to define a series of lines to search for.  LAST BOX must be white, to signal
// end of the trace series.  Trace color cannot be white or black! 
//Find the starting point of a given color line.  
//Trace a given color line, producing a list of points, in image pixel coordinates,
//  from begin to end of the line.
//This model supports line crossings, if the crossing is near perpendicular! 





//nSplines to sweep out image
//nSplines openscad library from Parkinbot on thingiverse,
//  https://www.thingiverse.com/thing:1208001/files 
//Install nSplines in a peer directory and point to these two files:
//use <../../../curves/nSplines-2019/files/splines.scad>
//use <../../../curves/nSplines-2019/files/Naca_sweep.scad>
//NOTE: Parkinbot says on Feb 20, 2019 that convex polygons are fine with:
//      sweep(dat, planar_caps = true);

//Borrow a few functions from nSplines library, so we don't have to include it.
//// Expand 2D vector into 3D
function vec3(v,z=0) = v[0][0]==undef?[v[0],v[1],z]:[for(a=v) vec3D(a, z)]; 
function vec2(v) = v[0][0]==undef?[v[0], v[1]]:[for(a=v) vec2(a)]; 
function vec3D(v,z=0) = vec3(v, z); 
// Translation - 1D, 2D, 3D point vector //////////////////////////
//   recursive! operates over vectors, lists of vectors, lists of lists ... 
function T_(x=0, y=0, z=0, v) = let(il=x[0]!=undef?len(x)==3:false)
  let(v = il?y:v, x = il?x:[x, y, z]) v[0][0]!=undef?[for (i=v) T_(x,i)]:v+x; 
function T(x=0, y=0, z=0, v) = T_(x,y,z,v); // synonym
function Rz_(z, A) = let(lenA0is2=A[0]!=undef?len(A)==2:false) 
    A[0][0]!=undef? 
    [for(i=A) Rz_(z, i)]: 
    lenA0is2? 
    A*[[cos(z), sin(z)], [-sin(z), cos(z)]]: 
    A*[[cos(z), sin(z), 0], [-sin(z), cos(z), 0], [0, 0, 1]]; 
function Rz(z, A) = Rz_(z, A); // synonym
//END of Borrow a few functions from nSplines library
    
    
function LT_color_match(c1, Checkc2, colorAndIntensityMatch=true) = 
    //true if c2 is quite a close match to c1
    //input values are array of RGB 0..255 integers, [r, g, b]
    //color is the same when the relative values of rgb are the same, so
    //  [10,10,10] is the same color as [100,100,100], but a different intensity.
    
    let (
      brightest =  c1[0] > c1[1] ? 0 : 1,
      brightestOf3 = c1[brightest] > c1[2] ? brightest : 2,
      IntenNorm = colorAndIntensityMatch ? 1.0 : 100.0 / c1[brightestOf3], //value to adj brightest component to standard intensity of 100
      
      checkbrightest = Checkc2[0] > Checkc2[1] ? 0 : 1,
      checkbrightestOf3 = Checkc2[checkbrightest] > Checkc2[2] ? checkbrightest : 2,
      IntenCheckNorm = colorAndIntensityMatch ? 1.0 : 100.0 / Checkc2[checkbrightestOf3],
 
      rDiff = abs( c1[0] * IntenNorm -  Checkc2[0] * IntenCheckNorm),
      gDiff = abs( c1[1] * IntenNorm -  Checkc2[1] * IntenCheckNorm),
      bDiff = abs( c1[2] * IntenNorm -  Checkc2[2] * IntenCheckNorm),
      closeMatch = (rDiff + gDiff + bDiff) < 16, 
      rMatch = abs( c1[0] * IntenNorm -  Checkc2[0] * IntenCheckNorm) < 3,
      gMatch = abs( c1[1] * IntenNorm -  Checkc2[1] * IntenCheckNorm) < 3,
      bMatch = abs( c1[2] * IntenNorm -  Checkc2[2] * IntenCheckNorm) < 3
      //,      x = echo("LT color brightest, norm", brightestOf3, IntenNorm, ", check brightest, norm", checkbrightestOf3, IntenCheckNorm, "rgb match", rMatch, gMatch, bMatch)
     )
      closeMatch
      //rMatch && gMatch && bMatch 
      ;
      

function LT_box4pxColorMatch(img, XYstart) = 
    //starting at XY pixel coord in img, where XY=0,0 is upper left corner,
    //     img is indexed [y][x],
    //     check next 3 pix to match XYstart
    //return [matchBool, [R, G, B]], select RGB from the set of 3 ahead as first one might be a blend at edge. 
    let ( X = XYstart[0], Y = XYstart[1],
        thisColor = img[Y][X],
        runColor = [ img[Y][X+2][0], img[Y][X+2][1], img[Y][X+2][2]],
           //pick out color RGB in middle of run ahead.  The first one might be a blend at start. 
        c1 = LT_color_match(thisColor, img[Y][X+1]),
        c2 = LT_color_match(thisColor, img[Y][X+2]),
        c3 = LT_color_match(thisColor, img[Y][X+3])
    )
    [c1 && c2 && c3, runColor ]
    ;

function LT_findColorRun(img, XY, xIndex=0) = 
    //search in X+1 direction until a sequence of 4 matching colors is found
    //return [ [rgb sequence color], [XY start of this color], noneFoundBoolean, isBlackWhite]
    XY[0] + xIndex > len(img[0]) - 4 ? [ [], [], true, false] :
    let ( thisBox = [XY[0] + xIndex, XY[1]],
          //nextBox = [ XY[0] + xIndex +3, XY[1]],
          //thisColor = LT_box4pxColorAvg(img, thisBox),
          //nextColor = LT_box4pxColorAvg(img, nextBox),
          //matchingIsARun = LT_color_match(thisColor, nextColor), 
          
          isARun = LT_box4pxColorMatch(img, thisBox), 
          matchingIsARun = isARun[0],
          thisColor = isARun[1],
          isBlackWhite = LT_color_match(thisColor, [255,255,255]) || LT_color_match(thisColor, [0,0,0])
      )
     matchingIsARun ? [ thisColor, thisBox, false, isBlackWhite] :
     LT_findColorRun(img, XY, xIndex + 1)
     ;


function LT_findEndofThisColor(img, XY, ThisColor, xIndex=0) =
    // search until 4 color pixels in a row don't match. 
    //return [ [XY start of different color], noneFoundBoolean ]
    // XY = 0,0 is upper left of img.  img is indexed [y][x]. 
    XY[0] + xIndex > len(img[0]) - 4 ? [ [], true] :
    let ( thisRow = [XY[0] + xIndex, XY[1]],
          endofThisRun = [XY[0]+xIndex+3, XY[1]], //if not matching, return the end pixel
          isARun = LT_box4pxColorMatch(img, thisRow),
          stillInARun = isARun[0]
    )
    stillInARun ? LT_findEndofThisColor(img, XY, ThisColor, xIndex+1) :
    [ endofThisRun, false]
    ;



function LT_findNextColorBox(img, XY) =
    // go to end of current color, then find a new color run
    // XY = 0,0 is upper left of img.  img is indexed [y][x]. 
    //return [ [rgb of next color], XY, noneFoundBoolean, isBlackWhite]
    let ( 
       thisBox = XY,
       thisColor = LT_findColorRun(img, thisBox), 
          //[ [rgb], [XY start of this color], noneFoundBoolean, isBlackWhite]
       inTheRun = thisColor[2] == false,
       foundEnd = inTheRun ? LT_findEndofThisColor(img, thisColor[1], thisColor[0]) :
            [ [], true] ,
            //[ [XY start of different color], noneFoundBoolean ]
       foundNext =  foundEnd[1] == true ? [ [], [], true ] : LT_findColorRun(img, foundEnd[0] ) ,
             // [this color, this box, noneFound, isBlackWhite]
       isBlackWhite = foundNext[2] == true ? false : 
         (LT_color_match( foundNext[0], [255,255,255]) || LT_color_match(foundNext[0], [0,0,0]))   
     )
      [ foundNext[0], foundNext[1], foundNext[2], isBlackWhite] 
     ;
    

function LT_ColorTraceList(img, XY, gotFirst=false, isBlackWhite=false, noneFound=false, rgbList=[]) =
    // XY starting point, usually bottom left corner, 3rd px row up.
    // return list of color boxes for line tracing,
    // [  [rgb1], [rgb2], ...],  could be 0 entries
    assert(len(img) > 2 && len(img[0]) > 2, "Image data must be at least 2x2")
    assert(len(img[0][0]) >=3, "Image data must be 3 RGB datums at each pixel")
    assert(len(XY) >=2 && XY[0] < len(img[0]) && XY[1] < len(img), "XY search start point must be pixel coords within the image")
    isBlackWhite || noneFound ? rgbList : //return what we have so far
    //get next color box
    let (
       Cfound = gotFirst == false ? LT_findColorRun(img, XY) :
                LT_findNextColorBox(img, XY),
            //[this color, in this box, noneFound, isBlackWhite]
       Clist = Cfound[2] == false && Cfound[3] == false ? concat( rgbList, [Cfound[0] ]) : rgbList
           //found one and it is not white, so append to list! 
        //, x = echo("ColorTractList Cfound", Cfound)
      )
    LT_ColorTraceList(img, Cfound[1], true, Cfound[3], Cfound[2], Clist)  ;     


function LT_inbound(img, XY) =
    //return true when point XY is within the img
    //img is indexed with Y,X=0,0 at upper left.  
    // negative Y direction is upwards! 
    (0 <= XY[0] && XY[0] <= len(img[0]) -1) &&
    (0 <= XY[1] && XY[1] <= len(img) -1 )
    ;
    

function LT_XYover90arc(existingLine, checkXY) = 
    //return true if this XY point is overlapping the final line segment treated as
    //  direction arrow and 90 degrees swept behind it. 
    //Note: Line is in normal openscad XY space.
    
    let( lastP = len(existingLine)-1,
       //axis is XY=0,0 upper left.  To make this translation and check comprehensible,
       //shift data points to normal XY = 0,0 at lower left ! 
        Seg = [ existingLine[lastP-1], existingLine[lastP], checkXY],
        Seg3 = vec3(Seg), //use nSplines to do rotation and translation for us.
        Seg3T = T(- Seg3[1][0], -Seg3[1][1], 0, Seg3), //translate last point to 0,0
        DirV =  Seg3T[1] - Seg3T[0], 
        //Omega = LT_vecToang(DirV), //geometric angle in our image XY space
        Seg3TtoXY = [ [Seg3T[0][0], -Seg3T[0][1]], [Seg3T[1][0], -Seg3T[1][1]], [Seg3T[2][0], -Seg3T[2][1]] ],
        Omega2 = LT_vecToang(Seg3TtoXY[1] - Seg3TtoXY[0]), 
        RotateTo135 = 135 - Omega2,
        Seg3TR = Rz(RotateTo135, Seg3T) 
        //, xx = echo("XY overlap Seg", Seg ,"Seg3T", Seg3T, "DirV", DirV, "Omega2", Omega2, "rotate", RotateTo135,  "Seg3TR", Seg3TR, Seg3TR[2][0] >= 0 && Seg3TR[2][1] <= 0   ) 
    )
     Seg3TR[2][0] >= 0 && Seg3TR[2][1] <= 0 //nSplines rotation puts this box in normal +X and -Y quadrant. 
    ;
    
function LT_XYoverlapping(existingLine, checkXY, index=0) =
    //return true if this XY point is overlapping the tail of existingLine
    //  decide overlap by 
    //  a) check last 4 line segments,
    //  b) for each segment, overlap if XY is within 90degrees of line segment
    //Note: Line and checkXY are in normal openscad space. 
    len(existingLine) < 10 ? false : 
    let ( lastP = len(existingLine)-1,
       checkSeg = [ existingLine[lastP-1-index], existingLine[lastP-index]],
       overlapBool = LT_XYover90arc(checkSeg, checkXY)
    )
    overlapBool ? true :  index > 3 ?  false:
    LT_XYoverlapping(existingLine, checkXY, index+1)
    ;
    
 

if(0) {
  echo ("line 1", LT_XYover90arc(line, [12,4])); //just inside box
  echo (LT_XYover90arc(line, [11,4.1])); //just outside box
   echo (LT_XYover90arc(line, [12.1, 3])); //just outside box
    echo (LT_XYover90arc(line, [12.1,12])); // just outside box
      echo (LT_XYover90arc(line, [12.1,4.1])); // just outside box  
 
 line2 = [ [2, 10], [2, 11], [2, 12], [2, 13] ]; //straight up
 echo ("line2", LT_XYover90arc(line2, [3,12])); //on the edge
 echo (LT_XYover90arc(line2, [3.1,12.])); //just outside


 
 line3 = [ [2, 11], [2, 12], [2, 13], [3, 13] ]; // last seg turns right
 
  echo( "line3 seg check", LT_XYoverlapping(line3, [3,12]));
}     
 
function LT_nearPoints(XY, StopHere, howClose) =
    //return true if XY is within howClose points of StopHere
    sqrt( (XY[0]-StopHere[0])*(XY[0]-StopHere[0]) + (XY[1]-StopHere[1])*(XY[1]-StopHere[1]) ) < howClose ; 


    
function LT_avgNahead(img, XY, unitV, N=3, skipCrossing, index=1, RGBsum=[0,0,0]) =
    //img is indexed with Y,X=0,0 at upper left.  
    // unitV is x,y where negative Y direction is upwards,
    //          len, skip
    //index is the look ahead amount (index*unitV) from point XY
    //ignore index values <= skipCrossing
    //RGBsum accumulates the color sums
    //return  rgb. 
    //    XY  
    //    unitV             -> 1,   2,   3,   4,   ...
    //   skipCrossing of 1 --------->
    //   skipCrossing of 2 --------------> 
    index > N ?  [ round(RGBsum[0]/(N-skipCrossing)), round(RGBsum[1]/(N-skipCrossing)), round(RGBsum[2]/(N-skipCrossing)) ] : 
                      //final value
    let (
        skipThisOne = index <= skipCrossing, 
        XYv = XY + index * unitV,
        maxXdim = len(img[0])-1,
        maxYdim = len(img)-1, 
        XYvlimit = [round(max(0, min(XYv[0], maxXdim))), round(max(0, min(XYv[1], maxYdim))) ], 
           //limited to inside of img
        FirstPointNotInBound = LT_inbound(img, XY + unitV) == false,
        imgPoint = FirstPointNotInBound || skipThisOne ? [0,0,0] : img[XYvlimit[1]][XYvlimit[0]]
           //must be at least one point in bound, else black.
     )
     LT_avgNahead(img, XY, unitV, N, skipCrossing, index+1, RGBsum + imgPoint ) 
   ;
   
function LT_matchNahead(img, XY, unitV, matchC, N=3, skipCrossing, index=1, RGBallMatch=true) =
    //img is indexed with Y,X=0,0 at upper left.  
    // unitV is x,y where negative Y direction is upwards,
    //          len, skip
    //index is the look ahead amount (index*unitV) from point XY
    //ignore index values <= skipCrossing
    //RGBallMatch goes false if any fail to match
    //return true when all match 
    //    XY  
    //    unitV             -> 1,   2,   3,   4,   ...
    //   skipCrossing of 1 --------->
    //   skipCrossing of 2 --------------> 
    index > N ?  RGBallMatch : //this is final match
    
    let (
        skipThisOne = index <= skipCrossing, 
        XYv = XY + index * unitV,
        maxXdim = len(img[0])-1,
        maxYdim = len(img)-1, 
        XYvlimit = [round(max(0, min(XYv[0], maxXdim))), round(max(0, min(XYv[1], maxYdim))) ], 
           //limited to inside of img
        FirstPointNotInBound = LT_inbound(img, XY + unitV) == false,
        imgPoint = FirstPointNotInBound || skipThisOne ? [0,0,0] : img[XYvlimit[1]][XYvlimit[0]],
           //must be at least one point in bound, else black.
        ThisOneMatch = LT_color_match(imgPoint, matchC),
        AllMatchSoFar = RGBallMatch && ThisOneMatch
        //, x = (XY[1] > 470 && XY[1] < 490) ? echo("matchNahead", "matchC", matchC, "index", index,  "XYv", XYv, "imgPoint", imgPoint, "allMatch", RGBallMatch, "thisonematch", ThisOneMatch, "match so far", AllMatchSoFar  )  :0
     )
     LT_matchNahead(img, XY, unitV, matchC, N, skipCrossing, index+1, AllMatchSoFar) ;
    
    

function LT_angToVec(deg) = [cos(deg), -sin(deg)] ;
function LT_vecToang(vec) = // XY == 00 is upper left.  Y positive is downwards! 
      vec[0] == 0  && vec[1] > 0 ? 270 :
      vec[0] == 0  && vec[1] < 0 ? 90 :
      vec[0] > 0 && vec[1] > 0 ? 360 - atan ( vec[1]/vec[0]) :
      vec[0] > 0 && vec[1] < 0 ? atan( -1.0 * vec[1]/vec[0]) :
      vec[0] < 0 && vec[1] >= 0 ? 180 + atan ( -1.0 * vec[1]/vec[0]) :
      vec[0] < 0 && vec[1] < 0 ? 180 - atan ( vec[1]/vec[0]) : 0; 

if (0) { 
echo ("test LT_vecToang 1,0", LT_vecToang([1, 0]));   
echo ("test LT_vecToang 0.7, -0.7", LT_vecToang([0.7, -0.7]));
    echo ("test LT_vecToang 0, -1", LT_vecToang([0, -1]));
      echo ("test LT_vecToang -0.1, -1", LT_vecToang([-0.1, -1]));
      echo ("test LT_vecToang -1, -0.1", LT_vecToang([-1, -0.1]));
    echo ("test LT_vecToang -1, 0", LT_vecToang([-1, 0]));
    echo ("test LT_vecToang -0.7, 0.7", LT_vecToang([-0.7, 0.7]));
    echo ("test LT_vecToang 0, 1", LT_vecToang([0, 1]));
    echo ("test LT_vecToang -0.01, 1", LT_vecToang([-0.01, 1]));
    echo ("test LT_vecToang 0.01, 1", LT_vecToang([0.01, 1]));
    echo ("test LT_vecToang 0.7, 0.7", LT_vecToang([0.7, 0.7]));
}

function LT_unitVecAheadList2(degAhead) =
    //degAhead is interpreted trigonometrically with 0 deg to the right and 90 deg toward
    //  top of image! 
    //return unit vectors with spec for length of lookahead, e.g.
    //  [ x, y, Nlookahead, skip, degreeangle for debug] 
    //This list is in search priority order, with long look aheads,
    //  then short lookaheads, and each group is first straight
    //  ahead and then lateral to degAhead. 
    //Lookahead length is selected for long being about 2x expected 4px line width,
    //   and short is just narrower than line width. 
    //Skip allows line crossing, disregarding the first skip points, checking remainder.
    // vector is x,y, but x,y=0,0 is upper left corner! Positive Y is downwards
    [ [cos(degAhead), -sin(degAhead),       8, 0, degAhead ],
      [cos(degAhead-45), -sin(degAhead-45), 8, 0, degAhead-45],
      [cos(degAhead+45), -sin(degAhead+45), 8, 0, degAhead+45],
      [cos(degAhead-90), -sin(degAhead-90), 8, 0, degAhead-90],
      [cos(degAhead+90), -sin(degAhead+90), 8, 0, degAhead+90],
      [cos(degAhead), -sin(degAhead),       3, 0, degAhead ],
      [cos(degAhead-45), -sin(degAhead-45), 3, 0, degAhead-45],
      [cos(degAhead+45), -sin(degAhead+45), 3, 0, degAhead+45],
      [cos(degAhead-90), -sin(degAhead-90), 3, 0, degAhead-90],
      [cos(degAhead+90), -sin(degAhead+90), 3, 0, degAhead+90],
      
      [cos(degAhead), -sin(degAhead),       9, 5, degAhead ],
      [cos(degAhead-45), -sin(degAhead-45), 9, 5, degAhead-45],
      [cos(degAhead-22.5), -sin(degAhead-22.5), 9, 5, degAhead-22.5],
      [cos(degAhead+22.5), -sin(degAhead+22.5), 9, 5, degAhead+22.5],
      [cos(degAhead+45), -sin(degAhead+45), 9, 5, degAhead+45],
     
      [cos(degAhead), -sin(degAhead),       11, 7, degAhead ],
      [cos(degAhead-45), -sin(degAhead-45), 11, 7, degAhead-45],
      [cos(degAhead-22.5), -sin(degAhead-22.5), 11, 7, degAhead-22.5],
      [cos(degAhead+22.5), -sin(degAhead+22.5), 11, 7, degAhead+22.5],
      [cos(degAhead+45), -sin(degAhead+45), 11, 7, degAhead+45],
      
      [cos(degAhead+11.25), -sin(degAhead+11.25),  10, 7, degAhead+11.25 ],
      [cos(degAhead-11.25), -sin(degAhead-11.25),  10, 7, degAhead-11.25 ],
      
      [cos(degAhead), -sin(degAhead),             19, 15, degAhead ],
      [cos(degAhead+11.25/2), -sin(degAhead+11.25/2),       19, 15, degAhead+11.25/2 ],
      [cos(degAhead-11.25/2), -sin(degAhead-11.25/2),       19, 15, degAhead-11.25/2 ],
      [cos(degAhead+11.25), -sin(degAhead+11.25),       19, 15, degAhead+11.25 ],
      [cos(degAhead-11.25), -sin(degAhead-11.25),       19, 15, degAhead-11.25 ],
      
      [cos(degAhead), -sin(degAhead),       1, 0, degAhead ],
      [cos(degAhead-45), -sin(degAhead-45), 1, 0, degAhead-45],
      [cos(degAhead+45), -sin(degAhead+45), 1, 0, degAhead+45],
      [cos(degAhead-90), -sin(degAhead-90), 1, 0, degAhead-90],
      [cos(degAhead+90), -sin(degAhead+90), 1, 0, degAhead+90]
    ];

function LT_NpxLineAvgListAhead(img, XY, vecAngleDegAhead) =
    // return [ [  [rgb color avg], unitV, length of match, "skip"], [for each ahead & lateral direction] ]
    //XXX some directions don't exist, being at edge of image XXX 
    let ( unitV = LT_unitVecAheadList2(vecAngleDegAhead)
    )
   [ for (V = [0 : len(unitV)-1])
                   //img, XY, unitV 0 & 1, Nahead, skip#ahead 
      [LT_avgNahead(img, XY, unitV[V], unitV[V][2], unitV[V][3] ), unitV[V], unitV[V][2], unitV[V][3] > 0 ? "is crossing skip" : "" ] 
                    
      //note: if vector hits edge of image, then that rgb will be 000
    ]; 

function LT_ColorTrackBestpath(color, unitVList, index=0) =
    //search down list for first match
    // return [noColorMatchBoolean, unitVofMatch, length of match, index in UnitVList]
    index > len(unitVList)-1 ? [true, [0,0], 0, 0 ] :
    let ( //xx = echo("ColorTrackBestpath index", index,  "unitVitem",unitVList[index] )
     ) 
    LT_color_match(color, unitVList[index][0])  ?
       [false, unitVList[index][1], unitVList[index][2] , index ] :
       LT_ColorTrackBestpath(color, unitVList, index+1)
    ;

function LT_taillist(v, startI) = 
   //return the tail elements of list v, starting at index startI.
   startI >= len(v) ? [] : //none left to return
   [for (i = [startI: len(v)-1]) v[i] ] ; 
   
   
//function LT_ColorTrackTryVectors(img, XY, matchColor, VListAhead, cklineList) =
   
function LT_ColorTrack(img, XY, matchColor, unitV, possibleTrackVecList, cklineList) =
   //from XY, going in unitV direction, return next color match point by
   //  evaluating possibleTrackVecList elements for first & best one. 
   // [ noneFoundBoolean, nextXY, newUnitV] 
   let (  
       noPath = len(possibleTrackVecList) == 0, //boolean
       bestMatchV = noPath ? [true, [0,0], 1, 0 ]  : 
                   LT_ColorTrackBestpath(matchColor, possibleTrackVecList),
                    //returns [noColorBool, unitV, len, index used]
                    //bestMatchV[3] is index into possibleTrackVecList, to find debug
       crossingInfo = possibleTrackVecList[bestMatchV[3]][3], //debug info
       //if noPath || bestMatchV[0] == true, then no color match direction found
       
       lenBest = bestMatchV[2], 
       proposedFoundAtVListIndex = bestMatchV[3],
       jumpV = (lenBest < 2.0) ? 1.0 :  (lenBest / 2.0), //jump just half the matching
              //color section, to reduce jitter in the line. 
       //jumpV = lenBest,
       proposedNextPt = [ XY[0] + jumpV * bestMatchV[1][0], XY[1] + jumpV * bestMatchV[1][1]],
       checkOverlapSelf = LT_XYoverlapping(cklineList, [proposedNextPt[0], len(img)-1-proposedNextPt[1]]) 
           //do overlap check of this possible vector.  If overlapping self, go on to the next vector. 
           
      //below debug can show a portion of line matches
      //, x= (XY[1] > 880) && (noPath==false && bestMatchV[0]==false) ? echo("LT_ColorTrack at XY", XY, "seeking color", matchColor, "in direction angle", LT_vecToang(unitV), "found color in match vector", bestMatchV[1], "unitVlistIndex",proposedFoundAtVListIndex, "len of match", lenBest, "overlap?", checkOverlapSelf, crossingInfo ) :0
    )  
   //bestMatchV in possibleTrack is accepted, with that unitvector,
   // UNLESS it is a self overlap.  In that case, go on to try next unitV entries.
   // no matches is noneFound. 
    noPath == false && checkOverlapSelf ? 
         //search from next item in the vector list, not accepting overlap solution.
         LT_ColorTrack(img, XY, matchColor, unitV, LT_taillist(possibleTrackVecList, proposedFoundAtVListIndex +1), cklineList) :
         //have solution! 
         [bestMatchV[0], proposedNextPt, [bestMatchV[1][0], bestMatchV[1][1] ]  ]; 
 


function LT_TrackColorLine(img, XY, matchColor, unitV, noMatch=false, lineList=[], StopHere=[0,0]) =
    //starting at XY, direction unitV, look for matchColor.  Keep going until noMatch.
    //return [  [x,y], [x,y] the list of matching coordinates in pixel dimensions, BUT
    //   where X,Y=00 is lower left, suitable for openscad line on top of the image
    //   in XY plane !!
    noMatch ? lineList :  //no matches further, return accumulated list of points
    let ( MAXIMUM_POINTS = 2000,
         LineStartingPoint = StopHere == [0,0] ? XY : StopHere, //keep the start point, to stop search if reached again
         possibleTrackVList = LT_NpxLineAvgListAhead(img, XY, LT_vecToang(unitV)),
          //note: possibleTrackVList elements have length which may jump a crossing, where 4th element is debug note
        aNextOne = LT_ColorTrack(img, XY, matchColor, unitV, possibleTrackVList, lineList),
        tailLoop = LT_XYoverlapping(lineList, [aNextOne[1][0],  len(img)-1 - aNextOne[1][1]]), //note coordinates in lineList are normal openscad XY space
        
        reachedStart= len(lineList) > 10 ?  LT_nearPoints(XY, StopHere, 6) : false,
            //check if current point is near to start point, provided line has advanced 10 points or more
        overlapsSelf = tailLoop || reachedStart
    )
    overlapsSelf || len(lineList) > MAXIMUM_POINTS ? LT_TrackColorLine(img, XY, matchColor, unitV, true, lineList)  :  //debug, limit line length when seeing funny tracking. 

    aNextOne[0] == false ? //got a next color match point
       LT_TrackColorLine(img, aNextOne[1], matchColor, aNextOne[2], false, 
          concat(  lineList, [[ aNextOne[1][0],  len(img)-1 - aNextOne[1][1] ]] 
          ), LineStartingPoint ):
       LT_TrackColorLine(img, XY, matchColor, unitV, true, lineList, LineStartingPoint) //end
    ;


function LT_findLineStart(img, XY, matchColor, unitV, index=0) =
    //search in unitV direction for sequence matchColor, 
    //return [ noneFoundBoolean, XYmatchPoint ] 
    let ( 
         //checkcolorRGB = LT_avgNahead(img, XY, unitV, 3, 0), //N=3, skip=0
         //compare = LT_color_match(matchColor, checkcolorRGB),
         compare = LT_matchNahead(img, XY, unitV, matchColor, 2, 0),
             //when found, first color is XY + unitV ! 
         nextpoint = XY + unitV, 
        notFound = LT_inbound(img, nextpoint) == false
       // , xx= (XY[1] < 150) && echo("LT_findLineStart matchColor", matchColor, "XY, unitV", XY, unitV, "XY + unitV", XY + unitV, "img RGB", img[XY[1]][XY[0]], "is a match?", compare, "out of bound?", notFound, "index", index) 
    )
     compare ? [false, XY + unitV] :
     notFound ? [true, XY] :
     LT_findLineStart(img, XY + unitV, matchColor, unitV, index+1)
   ; 


function LT_reverseTrace(trace) =
   [  for (i=[len(trace)-1:-1:0])  trace[i] ];
    
//echo( "test reverseTrace", LT_reverseTrace([ [1,1], [2,2], [4,4]]) ); 

function LT_distanceBP(p, q) = 
    sqrt( (p[0] - q[0])* (p[0] - q[0]) +  (p[1] - q[1]) * (p[1] - q[1])); 

module ShowLinePoints(v, hei=1) {
    //use line vector reversing on itself to form a closed poly.
    //assume v is in oscad units
    //have to be careful turning corners so the poly doesn't intersect itself!
    closedpoly = concat(v,
      [ for (index = [len(v)-1:-0.5:2]) 
          let( i = floor(index),
              beside = i == index,
              P0 = v[i], P1 = v[i-1], p2=v[i-2],
               dirV = [P1[0] - P0[0], P1[1] - P0[1] ], 
                 //direction vector from p0 -> p1
               dirVm90 = Rz_(-90, dirV),
               Plateral = P0 + (0.5 * dirV) + (0.1 * dirVm90),
               
               PbesideP1 = P1 + (0.1 * dirVm90),
               PbesideP1lag = P1 + (0.1 * dirVm90) + - 0.1 * dirV,
               PbesideP1lead = P1 + (0.1 * dirVm90) +  0.1 * dirV,
               PbesideTurningCorner = LT_distanceBP(p2, PbesideP1) > LT_distanceBP(p2, P1) ?  PbesideP1lead : PbesideP1lag
              //, xx = echo("P0 to P1", P0, P1, "dirV", dirV, "dirVm90", dirVm90, "Plateral", Plateral)
          )
          beside ? PbesideTurningCorner : Plateral
      ] 
    );
    linear_extrude(height = hei) polygon(closedpoly);
    //echo (closedpoly);  
}

function LT_validColorImageList(imgList, imgI=0, validSoFar = true) =
    imgI >= len(imgList) ?  (len(imgList) == 0 ? false: validSoFar) :
    let ( Img = imgList[imgI],
          dimOK = len(Img) > 2 && len(Img[0]) >2,
          pixOK = len(Img[0][0]) >=3, //an RGB datum
          ImgValidBool = dimOK && pixOK
     )
     ImgValidBool == false ? false : LT_validColorImageList(imgList, imgI+1, true);

function LT_buildTopLineList(imgList, XYstart, refColorList, SunitV, TunitV) =
    //img is indexed with Y,X=0,0 at upper left.  
    //XYstart is point to start looking for a colored line, following vector SunitV.
    //SunitV is vector to search for start of a colored line
    //TunitV is direction to start following the line.
    // SunitV and TunitV are x,y where negative Y direction is upwards! 
    //resulting traces are in pixel dimensions, but not necessarily whole integers,
    //with XY coordinates of XY=0,0 at lower left (normal openscad XY plane). 
    //Further do not assume there is one entry in the trace per column or row of
    //  the image file! 
    assert(LT_validColorImageList(imgList), "must have list of images, each a 2 dimensional array of RGB pixels") 
  [
   for (i = [0: len(refColorList)-1] )
     let ( findLineStart = LT_findLineStart(imgList[i], XYstart, refColorList[i], SunitV),
           xy = echo("find trace start", i, "XYstart", XYstart, "ref color", refColorList[i], "SunitV", SunitV), 
           xx = echo("color box", refColorList[i], "noneFound at XY?", findLineStart),
           
          line = findLineStart[0] == false ? 
            LT_TrackColorLine(imgList[i], findLineStart[1], refColorList[i], TunitV, false,  
              [ [findLineStart[1][0], len(imgList[i])-1 - findLineStart[1][1]] ])
            : [],
          xz = echo("trace for color", refColorList[i], "has length", len(line) )
      )
     
      line
   
   ]
   ;

function LT_FlipTraceonY(trace, maxYvalue) =
    //trace is a list of x,y points taken from an image with XY = 0,0 at upper left.
    // assume Y values in points are in range 0:maxYvalue
    //Return trace flipped on Y such that extrusions go toward bottom of image.
    //  This means BG/background is at smallest Y values
   [ let (Tlen = len(trace)
         )
     for (i = [0: Tlen-1])
     [ trace[i][0], maxYvalue - trace[i][1]]
   ];

T1 = [ ["x0", 10], ["x1", 6], ["x2", 2] ];
T2 = [ ["2x0", 10], ["2x1", 6], ["2x2", 2] ];
//echo("test LT_FlipTraceonY", LT_FlipTraceonY( T1, 10));

function LT_FlipTraceonYList(traceList, maxYvalue) =
   [
     for (i = [0:len(traceList)-1])
      LT_FlipTraceonY(traceList[i], maxYvalue)
   ];
//echo("test LT_FlipTraceonYList", LT_FlipTraceonYList([T1, T2], 10));

module seek_algorithm(x_pixdim, y_pixdim, unitV, matchedIndex, img) {
    //show the color search algorithm  
    //place at x, y_pixdim, pointing in unitV direction
    // unitV x,y=0,0 is upper left corner! Positive Y is downwards!
    // matchedIndex is which index in unitV list matched, so color it
    //img is to convert pixel datums to osc
    
    x_osc = x_pixdim; 
    y_osc = len(img)-1 - y_pixdim;
    ang = LT_vecToang(unitV);
    lookHere = LT_unitVecAheadList2(ang);
       //returns list of: unit vec x,y, len, skip, direction angle
       //            x,y=0,0 is upper left corner! Positive Y is downwards!
    
    for (i = [0: len(lookHere)-1]) {
        ColorMatch = i == matchedIndex ? "green" : "yellow"; 
        Lx = lookHere[i][0];
        Ly = lookHere[i][1];
        Lomega = LT_vecToang([Lx, Ly]);
        Llen = lookHere[i][2];
        Lskip = lookHere[i][3];
        //echo("seek alg, i, dir vec[", Lx, Ly, "] omega", Lomega, "len", Llen, "skip", Lskip); 
        if (Lskip > 0) {
          translate([x_osc, y_osc, 0 + i])
             rotate([0,0, Lomega])
               union() {
                  rotate([0,90,0]) cylinder(r = 0.1, h = Lskip, $fn=60);
                  translate([Lskip,0,0]) rotate([0,90,0]) color(ColorMatch)cylinder(r = 0.3, h=Llen, $fn=60) ;
               }
            
        } else {
          translate([x_osc, y_osc, 0 + i])
            rotate([0,0, Lomega])
               rotate([0, 90,0]) color(ColorMatch) cylinder(r = 0.3, h=Llen, $fn=60) ; 
        }
        
    }
}





if(0) { // testing 
// image in the form of an openscad array definition is built by python script: png-rcol.py
// In this case:
//     python3 png-rcol.py  Line_test2.png  > Line_test2.txt
// note the Openscad 2 dimension array name is the same as the file name, with "-" changed to "_". 
//  In this case:  Line_test2
    
//Commonly more than one .png is processed to obtain all the line traces needed.
//only the first one is used to obtain the color boxes at lower left corner.
FirstImageFileName = "Line_test2.png"; 
include <./Line_test2.txt>;  Image_with_lines = Line_test2;

    
echo ("Image with lines,  y rows", len(Image_with_lines), " x rgb cols", len(Image_with_lines[0]) );
    
//seek_algorithm(447.978, 322.682, [0.0386359, 0.999253], 25, Image_with_lines);    
//seek_algorithm(285.454, 77.3136, [0.980785, -0.19509], 22, Image_with_lines);
//seek_algorithm(470.262, 257.619, [-0.55557, 0.83147], 5, Image_with_lines);
//seek_algorithm(144.552, 115.971, [0.382683, -0.92388], 10, Image_with_lines);
    

TraceImageList = [Image_with_lines, Image_with_lines]; //specify the image array for every color trace box in the first image! 
    
//obtain the colors from the sequence of color boxes at lower left corner, white or black box terminated. 
RefColorsForLineSearch = LT_ColorTraceList(Image_with_lines, [1, len(Image_with_lines)-1-1] );
echo("Ref Colors For Line Search", RefColorsForLineSearch);
//note: each one of these colors must have an entry in TraceImageList !! 


//search for each color trace. 
//note the XYstart is an XY coordinate for an image where XY = 0,0 is upper left corner.
//note the unitVs are XY with Y=0 at top of image, and Y increasing is going toward bottom of the image (reverse of openscad coordinates of an image lying in XY plane).
ContourTraceList = LT_buildTopLineList(TraceImageList, [0,2],RefColorsForLineSearch,  [0,1], [0,1]);


//tracing the lines can be expensive.  You may wish to capture the data
//    and include it as a .txt file for further processing.
//CAPTURE TRACES in stderr
//  in Windows cmd.exe:
//  set oscad="c:\Program Files\OpenSCAD\openscad.com"
//  cd  directory of .scad
//  %oscad% -o test.png  test.scad  > test.txt 2>&1
//  --> performs Preview 
//echo("Traces", ContourTraceList);




  //display the discovered lines on top of one .png file
  for (i = [0: len(ContourTraceList)-1] ) {
    echo("show trace", i, "of length", len(ContourTraceList[i]));
    if ( len(ContourTraceList[i]) < 10) echo("Failed to trace this line!  Most likely due to trace start not full 4 pix width at edge of image."); 
    color(c=RefColorsForLineSearch[i]/255.0) ShowLinePoints(ContourTraceList[i], 5.0);
  }
  
  translate([0,0,-10]) 
  scale([1.0,1.0, 6.0/255]) surface(file=FirstImageFileName, convexity=3); 
}