#!/usr/bin/env python3

import numpy as np

# Calcul la distance du point pM au segment [p1, p2]
def point2seg(pM, p1, p2):
    vec = p2 - p1
    vec /= np.sqrt(vec[0]**2+vec[1]**2)
    vecT = np.array([-vec[1], vec[0]])
    return np.abs(np.sum((pM-p1)*vecT))


class Cluster:
    def __init__(self, points):
        self.points = np.array(points).T  # Transposition de matrice

        # Cluster center
        # TODO(2)
        self.center = np.array([
            (self.points[0].max() + self.points[0].min())/2.0,  # x
            (self.points[1].max() + self.points[1].min())/2.0  # y
        ])

        # Cluster width and length
        # TODO(2)
        self.width = (self.points[0].max() - self.points[0].min())# along the x coordinate
        self.length = (self.points[1].max() - self.points[1].min()) # along the y coordinate

        # Cluster polyline
        self.eps = 0.05
        self.poly = np.array(self.rdp(self.points, self.eps))

    def rdp(self, points, eps):
        # Ramer-Douglas-Peucker algorithm
        if points.shape[1] > 2:
            dmax = 0
            jmax = 0
            d = np.array([point2seg(points[:, i], points[:, 0], points[:, -1]) for i in range(1, points.shape[1] - 1)])

            dmax = d.max()
            jmax = d.argmax() + 1
            
            if dmax > eps:
                Pleft = self.rdp(points[:, :jmax+1], eps)
                Pright = self.rdp(points[:, jmax:], eps)
                
                Pres = Pleft + Pright
                
                return(Pres)
            # Return the list of points Pres
        return [points[:, 0], points[:, -1]]  # : on prends les x et y et sur la deuxième dim : 0 le premier -1 le dernier

def clustering(points):
    k = 3
    D = 0.03
    # Clustering algorithm
    # points.shape[1] = le nombre d'éléments de la ligne 0 de la var points
    G = np.zeros(points.shape[1], dtype=int)  # par défaut ça fait des float, c'est pour ça qu'on met dtype = int.
    Gp = []  # contient une liste de clusters, et pour chaque cluster une liste de points.
    # use "Gp.append([])" to create a new cluster of point
    # use "Gp[G[i]-1].append(points[:, i])" to add points[:, i] to the  cluster of point Gp[G[i]-1]
 
    
# points.shape[1] = len(points[0]). points.shape = (dim1, dim2). 
    for i in range(k, points.shape[1]):  #k+1ème point jusqu'au dernier
        d = [np.linalg.norm(points[:2, i] - points[:2, i-j]) for j in range(1, k+1)]
        
        """ Autre façon de faire
        
        d=[]
        for j in range (1, k+1):
            d.append(np.linalg.norm(points[:2, i]-points[:2, i-j]))
        """
        # [d_min, j_min + 1] = min(d)
        dmin = np.min(d)
        jmin = np.argmin(d) + 1 #argmin commence à 0 et j à 1

        if dmin < D:
            if G[i-jmin] == 0:
                Gp.append([])
                G[i-jmin] = len(Gp)
                Gp[-1].append(points[:2, i-jmin])
            G[i] = G[i-jmin]
            Gp[G[i]-1].append(points[:2, i])  # on lui rajoute le point i.
        
            
        #TODO(2)
        pass

    clusters = [Cluster(np.array(cluster_p)) for cluster_p in Gp]

    return G, clusters

## Le main

# To use math functions
import numpy as np

# To read csv files
import pandas as pd

#To plot data
import matplotlib.pyplot as plt

# Clustering function
from clustering import clustering

plt.figure()

fig = plt.gcf()
ax = plt.gca()

plt.ion()

# read csv
df_lidar = pd.read_csv('lidar.csv')
# change index
df_lidar['time'] = (df_lidar['time'] - df_lidar['time'].tolist()[0])/1000000000
df_lidar.set_index('time',inplace=True)
# strip column names
df_lidar.columns = [name[6:] for name in df_lidar]

for time, row in df_lidar.iloc[:500].iterrows():
    n = 360  # <-- number of points
    ranges = np.array(row[10:10+n])
    intensities = np.array(row[10+n:10+2*n])

    # Scatter of raw data
    # c=  is used to change the color of the points depending on their intensities
    # plt.scatter(range(len(ranges)), ranges, c=intensities)

    # TODO(1) Conversion from polar to cartesian coordinates
    # ranges: distance entre robot et obstacle
    # row.angle_min: angle min
    # row.angle_max: angle max
    # row.angle_increment: pas d'echantillonage
    theta = row.angle_min + np.array(range(n))*row.angle_increment  # Tableau de tous les angles en rad.
    scan = np.array([ranges*np.cos(theta), ranges*np.sin(theta), intensities])  # [np.array : x, np.array: y] 

    scan = scan[:, (ranges > row.range_min) & (ranges < row.range_max)]

    plt.scatter(scan[0], scan[1], c=scan[2])

    # TODO(2) Clustering of the LiDAR scan (voire clustering.py)
    (clusters_id, clusters) = clustering(np.array(scan[:3]))
    plt.scatter(scan[0], scan[1], c=clusters_id)

    # TODO(3) Bounding boxes (voire clustering.py)
    #ax = plt.gca()
    for clust in clusters :
        rect = plt.Rectangle(xy=(clust.center[0] - clust.width/2, clust.center[1] - clust.length/2) ,width=clust.width, height=clust.length, linewidth=1, color='blue', fill=False)
        ax.add_artist(rect)

    # TODO(4) Polylines (voire clustering.py)
    for clust in clusters :
        plt.plot(clust.poly[:,0], clust.poly[:,1], linewidth=4, color='magenta')

    #plt.axis('square')
    plt.draw()
    plt.pause(0.2)
    plt.clf()