# -*- coding: utf-8 -*-

from vector import Vector

class Bezier:
    
    def __init__(self,pts):
        if len(pts)==4:
            self.pts = [p.copy() for p in pts]
        if len(pts)==3:
            self.pts = [pts[0].copy(), 1/3*pts[0]+2/3*pts[1], 1/3*pts[2]*2/3*pts[1], pts[2].copy()]
        if len(pts)==2:
            self.pts = [pts[0].copy(), 2/3*pts[0]+1/3*pts[1], 1/3*pts[0]+2/3*pts[1], pts[1].copy()]
        
    # Punkt für Parameter t (in [0,1])        
    def x(self,t):
        return (1.0-t)**3*self.pts[0]+\
               3*(1.0-t)**2*t*self.pts[1]+\
               3*(1.0-t)*t**2*self.pts[2]+\
               t**3*self.pts[3]

    # Geschwindigkeit (dx/dt) in t-Parametrierung
    def v(self,t):
        return -3*(1.0-t)**2*self.pts[0]+\
               3*(1.0-4*t+3*t**2)*self.pts[1]+\
               3*(2*t-3*t**2)*self.pts[2]+\
               3*t**2*self.pts[3]

    # Normalisierte Geschwindigkeit
    def vnorm(self,t):
        return self.v(t).normalize()
    
    
    def adl(self,t, dt=0.000001): # Ableitung der normalisierten Geschwindigkeit nach der Länge
        dl = self.v(t).length*dt # (self.x(t+dt)-self.x(t-dt)).length
        return (self.vnorm(t+dt/2)-self.vnorm(t-dt/2))/dl
        
    # Effektive Beschleunigung im Punkt zum Parameter t, mit
    # gegebenem Geschwindigkeitsbetrag und Gravitationsvektor
    def aeff(self, t, v, g):
        return self.adl(t)*v*v-g
    
    # Orthonnormales Bahn-Koordinatensystem im Punkt zum Parameter t, mit
    # gegebenem Geschwindigkeitsbetrag und Gravitationsvektor.
    # Resultat (v,a, v cross a)
    def koordsyst(self, t, v, g):
        x = self.vnorm(t)
        # Effektive Beschleunigung
        aeff = self.aeff(t,v,g)
        # Projektion von aeff auf x (d.h. normalisierte Geschwindigkeit)
        av = x.dot(aeff)/(x.dot(x))*x
        # Effektive Beschleunigung in Normalebene zur Bahn.
        # (ist mathematisch rechtwinklig!)
        aa = (aeff-av).normalize()
        z = x.cross(aa)
        return (x,aa,z)
        
        
        
        
   
    # Laenge der Bezier-Kurve
    def length(self, tmin=0.0, tmax=1.0, steps=1000):
        l = 0.0
        # Zeitschritt
        dt = (tmax-tmin)/steps
        for i in range(steps):
            # Den i-ten Zeitpunkt von steps Zeitpunkten
            ti = (i+0.5)*dt+tmin
            l += self.v(ti).length*dt
        return l
    
    # Input: Startzeitpunkt t, Streckenlaenge l
    # Output: Endzeitpunkt t oder -Restlaenge
    def forward(self, l, startt, dt=0.0001):
        lastl = l
        lastt = startt
        while (startt<1.0 and l>0):
            dl = self.v(startt).length*dt
            lastl = l
            lastt = startt
            l -= dl
            startt += dt
        if startt<1.0:  # Noch auf der Bezierkurve
            return (startt-lastt)*lastl/(lastl+abs(l))+lastt
        
        if l>0: # Es ist noch Laenge uebrig, also auf naechster Kurve
            return -(lastl - (self.x(1)-self.x(lastt)).length)
        return 1.0 # Sonst genau auf dem Ende der Kurve
    

    # Zeichnet den Spline
    # Achtung: Funktioniert nur, wenn diese Datei ausgeführt wird...
    def draw(self, steps=1000):
        move(self.pts[0][0], self.pts[0][1]);
        for i in range(1,steps+1):
            p = self.x(1.0*i/steps)
            lineTo((p[0],p[1]))
                        

if __name__== "__main__":
    from gpanel import *
    makeGPanel(Size(600,600))
    window(0, 599, 0, 599)
    b = Bezier([Vector([100,500,0]), Vector([200,0,0]), Vector([300,300,0]), Vector([500,300,0])])
    b.draw()
    print(b.length(0,1,10000))
    t = 0.0
    while (t>=0.0):
        p = b.x(t)
        a = b.adl(t)
        pa = p+a*10000
        # move(p.x, p.y)
        # circle(5)
        line(p.x,p.y, pa.x, pa.y)
        t = b.forward(10,t)
        print(t)