#! /usr/bin/env python
# import rospy
# from std_msgs.msg import String
# from geometry_msgs.msg import Twist
# from geometry_msgs.msg import Pose
# from nav_msgs.msg import Odometry
# from sensor_msgs.msg import LaserScan
# import tf
import numpy as np
import math as m
import matplotlib.pyplot as plt

#Ucitavanje podataka iz fajla u koji su podaci bil isnimljeni na casu
with open('range_data.txt', 'r') as myfile:
    data = myfile.read()
data = data+', '
ranges = np.array([])
x = data.find(',')
while x != -1:
    num = float(data[0:x])
    if num == m.inf:
        num = 3.5
    ranges = np.append(ranges, num)
    data = data.replace(data[0:x+2], '', 1)
    x = data.find(',')


def Polar2Cartesian(r, alpha):
    return np.cos(alpha)*r, np.sin(alpha)*r


def Cartesian2Polar(x, y):
    r = np.sqrt(x**2 + y**2)
    phi = np.arctan2(y, x)
    return r, phi


def GetC(x):
    return np.mean(x)


def GetAlpha(x, y):
    M = np.shape(x)[0]
    sum1 = 0
    sum2 = 0
    xc = GetC(x)
    yc = GetC(y)
    for i in range(M):
        xt = xc-x[i]
        yt = yc-y[i]
        sum1 = sum1 + yt*xt
        sum2 = sum2 + m.pow(yt, 2)-m.pow(xt, 2)
    return 0.5*m.atan2(-2*sum1, sum2)


def GetR(x, y, alpha):
    xc = GetC(x)
    yc = GetC(y)
    return xc*m.cos(alpha)+yc*m.sin(alpha)


def GetMostDistant(x, y, r, alpha):
    d = m.sin(alpha)*y+x*m.cos(alpha)-r
    return np.max(d), np.argmax(d)


def Merge(out):
    merged = []
    rec = False
    while len(out) > 0:
        tek = out[0]
        ind = np.array([], dtype=int)
        k_sim = np.array(tek.get('k'))
        n_sim = np.array(tek.get('n'))
        X = tek.get('x')
        Y = tek.get('y')
        for i in out:
            if (i != tek):
                k_dif = abs(tek.get('k')-i.get('k'))
                n_dif = abs(tek.get('n')-i.get('n'))
                p = (tek.get('x')[-1] == i.get('x')[0]) and (tek.get('y')[-1] == i.get('y')[0])
                q = (tek.get('x')[0] == i.get('x')[-1]) and (tek.get('y')[0] == i.get('y')[-1])
                if (k_dif < 1) and (n_dif < 2) and (p or q):
                    ind = np.append(ind, out.index(i))
                    k_sim = np.append(k_sim, i.get('k'))
                    n_sim = np.append(n_sim, i.get('n'))
                    if p:
                        X = np.append(X, i.get('x'))
                        Y = np.append(Y, i.get('y'))
                    if q:
                        X = np.append(i.get('x'), X)
                        Y = np.append(i.get('y'), Y)
                    rec = True
        k_mean = np.mean(k_sim)
        n_mean = np.mean(n_sim)
        for i in sorted(ind, reverse=True):
            del out[i]
        out.remove(out[0])
        merged.append({'r': tek.get('r'), 'alpha': tek.get('alpha'), 'k': k_mean, 'n': n_mean, 'x': X, 'y': Y})
    if rec:
        return Merge(merged)
    else:
        return merged


def SplitAndMerge(x, y):
    s0 = {'x': x, 'y': y}
    L = [s0]
    split = []
    N = len(L)
    i = 0
    while i < N:
        X = L[i].get('x')
        Y = L[i].get('y')
        A = GetAlpha(X, Y)
        R = GetR(X, Y, A)
        if R < 0:
            A = A + m.pi
            if A > m.pi:
                A = A-2*m.pi
            R = -R
        M = np.shape(X)[0]
        d, ind = GetMostDistant(X, Y, R, A)
        if d > 0.05:
            if (ind+1 != M) and (ind != 0):
                s1 = {'x': X[0:ind+1], 'y': Y[0:ind+1]}
                s2 = {'x': X[ind:], 'y': Y[ind:]}
                L.remove(L[i])
                L.append(s1)
                L.append(s2)
                N = len(L)
                i = 0
            elif (ind == 0):
                s1 = {'x': X[0:2], 'y': Y[0:2]}
                s2 = {'x': X[1:], 'y': Y[1:]}
                L.remove(L[i])
                L.append(s1)
                L.append(s2)
                N = len(L)
                i = 0
            else:
                s1 = {'x': X[0:-1], 'y': Y[0:-1]}
                s2 = {'x': X[-2:], 'y': Y[-2:]}
                L.remove(L[i])
                L.append(s1)
                L.append(s2)
                N = len(L)
                i = 0
        else:
            split.append({'r': R, 'alpha': A, 'k': -1/m.tan(A), 'n': R/m.sin(A), 'x': X, 'y': Y})
            L.remove(L[i])
            N = len(L)
            i = 0
    merged = Merge(split)
    return merged

#U slucaju pustanja skipte u okviru ROS paketa, potrebno je zakomentarisati ovaj deo a odkomentarisati naredni
angles = np.linspace(0, 2*m.pi, 360)
x, y = Polar2Cartesian(ranges, angles)
x_axis = np.linspace(-4, 5, 1000)
out = SplitAndMerge(x, y)
plt.figure()
plt.plot(x, y, 'ro')
for i in out:
    plt.plot(x_axis, i.get('k')*x_axis+i.get('n'))
plt.xlim([-4, 5])
plt.ylim([-5, 5])

#def callback(data):
#    r = data.ranges
#    alpha = np.linspace(data.angle_min,data.angle_max,360)
#    for i in range(np.shape(r)[0]):
#        if r[i]==m.inf:
#            r[i]=3.5
#    x,y = Polar2Cartesian(r,alpha)
#    plt.figure()
#    plt.plot(x, y,'ro')
#    for i in out:
#        plt.plot(x_axis, i.get('k')*x_axis+i.get('n'), label = str(out.index(i)))
#    plt.xlim([-4,5])

#if __name__=='__main__':
#    try:
#        rospy.init_node('Domaci3')
#        rospy.spin()
#    except rospy.ROSIntrruptException:
#        pass