__author__ = "kii kang"
__version__ = "2020.05.19"

import rhinoscriptsyntax as rs
import ghpythonlib.components as gh
import math


#interval length coefficient
q = 0.0
toolpath = []
#Initiate points and vectors
pathPt = [gh.EvaluateCurve(crvIn,0)[0]]
VTangent = [gh.EvaluateCurve(crvIn,0)[1]]
VNormal = [gh.CrossProduct(VTangent[-1],gh.UnitZ(-1),True)[0]]
offPt = [gh.Move(pathPt[0],DOff*VNormal[0])[0]]
#Start carefully by moving just a half a length
VMove = [0.5*DMove*VTangent[0]]
movePt = [gh.Move(offPt[0],VMove)[0]]
curvature = [0]

#iterate
for i in range(maxIter):    
    LIntersect = gh.LineSDL(movePt[-1],VNormal[-1],1)
    t = gh.CurveXLine(crvIn,LIntersect)[1]
    while(t == None):
        movePt[-1] = gh.Move(movePt[-1],-0.05*DMove*VTangent[-1])[0]
        LIntersect = gh.LineSDL(movePt[-1],VNormal[-1],1)
        t = gh.CurveXLine(crvIn,LIntersect)[1]
    if (len(t) != 1):
        #choose closer intersecting point(*not needed in real robot)
        compareDist = gh.Distance(movePt[-1],gh.EvaluateCurve(crvIn,t)[0])
        if (compareDist[0] < compareDist[1]): t = t[0]
        else: t = t[1]
        #pathPt
        pathPt_next = gh.EvaluateCurve(crvIn,t)[0]
        pathPt.append(pathPt_next)
        if (i > 0):
            dist1 = gh.Distance(offPt[-1],pathPt[-2])
            dist2 = gh.Distance(movePt[-1],pathPt[-1])
            while(abs(dist2-dist1)/DMove > 0.05):
                movePt[-1] = gh.Move(movePt[-1],-0.05*DMove*VTangent[-1])[0]
                LIntersect = gh.LineSDL(movePt[-1],VNormal[-1],1)
                t = gh.CurveXLine(crvIn,LIntersect)[1]
                #choose closer intersecting point
                compareDist = gh.Distance(movePt[-1],gh.EvaluateCurve(crvIn,t)[0])
                if (compareDist[0] < compareDist[1]): t = t[0]
                else: t = t[1]
                pathPt_replace = gh.EvaluateCurve(crvIn,t)[0]
                dist2 = gh.Distance(movePt[-1], pathPt_replace)
                pathPt[-1] = pathPt_replace
    #vectorTangent
    VTangent_next = gh.EvaluateCurve(crvIn,t)[1]
    VTangent.append(VTangent_next)
    #vectorNormal
    VNormal_next = DMove*gh.CrossProduct(VTangent[-1],gh.UnitZ(-1),True)[0]
    VNormal.append(VNormal_next)
    #offPt
    offPt_next = gh.Move(pathPt[-1],VNormal[-1])[0]
    offPt.append(offPt_next)
    #arc length filtering
    if (i>0):
        curvature.append(gh.VectorLength(VTangent[-1]-VTangent[-2]))
    VMove.append(VTangent[-1]*DMove/(q*(curvature[-1])**2+(1-q)))
    #movePt
    movePt_next = gh.Move(offPt[-1],VMove[-1])[0]
    movePt.append(movePt_next)
    #stop condition
    stopline1 = gh.LineSDL(movePt[0],VNormal[0],DOff*10)
    stopline2 = gh.Line(offPt[-1],movePt[-1])
    if (i > 1 and gh.CurveXCurve(stopline1,stopline2)[0] != None):
        break
    print(curvature[-1])
#number of iterations
print('number of iterations: '+ str(i))
for j in range(len(pathPt)-1):
    toolpath.append(gh.Line(pathPt[j],offPt[j]))
    toolpath.append(gh.Line(offPt[j],movePt[j]))
    toolpath.append(gh.Line(movePt[j],pathPt[j+1]))