#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""

@author: *Anonymous*
"""
import numpy as np
from matplotlib import pyplot as plt
# from sklearn.linear_model import LinearRegression
from scipy import integrate
from functools import partial
import shutil

def arclenX(x, cd):
    return np.sqrt(1 + np.polyval(cd, x)**2)


def main(savePath='Sampling_based_Planning/rrt_2D/RRT_5'):
    x0, y0 = np.loadtxt('Sampling_based_Planning/rrt_2D/waypoints.csv', delimiter=',', unpack=True)
    print(x0.shape)
    print(y0.shape)
    print(x0[0])
    print(y0[0])
    print(x0)
    print(y0)
    breakpoint()
    
    
    LL = len(y0)
    print(LL)
    breakpoint()
    
    nPoints = 101
    if LL < 102:
        nPoints = LL - 2
    
    y1 = np.zeros(nPoints)
    x1 = np.zeros(nPoints)
    dy = ((np.max(y0)-y0[0]-0.00000001))/(nPoints-1)
    
    if y0[0] > y0[-1]:
        print('Reverse Order!')
        ll = LL - 1
        ll0 = ll
        #x1[0] = x0[ll]
        #y1[0] = y0[ll]
        
        print(x1[0], y1[0])
        count = 0
        
        for k in range(0, nPoints):
            while y0[ll] < k*dy:
                ll = ll - 1
            
                
            if ll0 == ll and k < nPoints-1:
                count += 1
            else: 
                ll0 = ll
                if (k == nPoints-1):
                    count += 1
                
                x1[k] = x0[ll]
                y1[k] = y0[ll]
                for g in range(1, count+1):
                    x1[k-g] = (g/count)*x1[k-count-1] + ((count-g)/count)*x1[k]
                    y1[k-g] = (g/count)*y1[k-count-1] + ((count-g)/count)*y1[k]
                
                count = 0
            #if ll == 0:
            #    KK = k
            #    break
    else:
        print('Ascending Order!')
        ll = 0
        ll0 = ll
        x1[0] = x0[0]
        y1[0] = y0[0]
        
        print(x1[0], y1[0])
        count = 0
        
        for k in range(0, nPoints):
            while y0[ll] < k*dy+y0[0]-0.0001:
                ll = ll + 1
            
                
            if ll0 == ll and k < nPoints-1:
                count += 1
            else: 
                ll0 = ll
                if (k == nPoints-1):
                    count += 1
                
                x1[k] = x0[ll]
                y1[k] = y0[ll]
                for g in range(1, count+1):
                    x1[k-g] = (g/count)*x1[k] + ((count-g)/count)*x1[k-1-count]
                    y1[k-g] = (g/count)*y1[k] + ((count-g)/count)*y1[k-1-count]
                
                count = 0
                if (k == nPoints - 1):
                    break
            #if ll == 0:
            #    KK = k
         
    x = x1
    y = y1
    
    print(x)
    print(y)
    
    print(np.max(y))
    print(len(y))
    deg = 20
    if nPoints < deg:
        deg = nPoints - 1
    
    plt.figure()
    plt.plot(x, y,'k')
    plt.plot(x0, y0, 'r--')
    plt.show()
    breakpoint()
    '''
    YY = np.zeros((len(y), 2*deg+1))
    YY[:,0] = np.ones(len(y))
    for k in range(deg):
        YY[:, 1+2*k] = np.cos((1+k)*y)
        YY[:, 2+2*k] = np.sin((1+k)*y)
    '''    
    
    yy = np.arange(0, 551, 5)/100.
    coeffs = np.polyfit(y, x, deg=deg)
    print(coeffs.shape)
    
    co = np.flip(coeffs)
    cd = np.flip(np.polynomial.polynomial.polyder(co, 1, 1))
    print(coeffs)
    print(cd)
    
    #g = partial(arclenX, cd)
    #g2 = lambda x: g(x)
    
    print(cd[-1])
    print(np.sqrt(1+cd[-1]**2))
    # print(g(0))
    
    breakpoint()
    
    arcLenArray = np.zeros((len(y), 1))
    
    for zz in range(len(y)):
        arcLenArray[zz, 0], _ = integrate.quad(arclenX, 0, float(y[zz]), args=(cd))
    
    
    xx2 = np.polyval(coeffs, y)
    xx3 = np.polyval(coeffs, yy)
    arcLenArray[:,0] = arcLenArray[:,0] - arcLenArray[0,0]
    #model = LinearRegression()
    #model.fit(YY, x.reshape(-1,1))
    
    '''
    yy0 = np.zeros((len(yy), 2*deg+1))
    yy0[:,0] = np.ones(len(yy))
    for k in range(deg):
        yy0[:, 1+2*k] = np.cos((1+k)*yy)
        yy0[:, 2+2*k] = np.sin((1+k)*yy)
    '''
    
    # xx = model.predict(YY)
    # xx0 = model.predict(yy0)
    
    plt.figure()
    plt.plot(x, y,'k')
    plt.plot(xx2, y, 'r--')
    plt.plot(xx3, yy, 'g*')
    plt.xlim([-4, 4])
    plt.ylim([0, 10])
    # plt.plot(xx0, yy, 'g')
    plt.show()
    
    np.savetxt(str(savePath+'.csv'), np.concatenate((np.arange(len(y)).reshape(-1, 1), xx2.reshape(-1, 1), y.reshape(-1, 1), arcLenArray), axis=1),delimiter=',')
    shutil.copy("Sampling_based_Planning/rrt_2D/InitialPath.png",str(savePath+"_initialpath.png"))
    shutil.copy("Sampling_based_Planning/rrt_2D/IteratedPath.png",str(savePath+"_iteratedpath.png"))
    
if __name__ == '__main__':
    main('Sampling_based_Planning/rrt_2D/RRT_6')


