import csv
import numpy as np
import plotly.graph_objects as go

# To generate distorsed covariance matrixes
def genS(sds, R, distor=1):
    # sds: standard deviations
    # R: correlation matrix
    # distor: level of distorsion
    sdsdis = sds * distor
    S = R * np.outer(sdsdis, sdsdis)
    np.fill_diagonal(S, sdsdis ** 2)
    return S

def get_datXYZ(gesto, data_path):
    file_path = data_path + f"config{gesto}.csv"
    with open(file_path, 'r') as file:
        csv_reader = csv.reader(file)
        data = list(csv_reader)
    n = len(data)
    coordinates = np.zeros((n, 21, 3))
    for i, row in enumerate(data):
        for j in range(21):
            x, y, z = map(float, row[j*3 : (j+1)*3])
            coordinates[i, j] = [x, y, z]
    return coordinates

def plot_3d_points(points_21_3d, title):    
    # Create a scatter plot
    colors = ['red', 'green', 'yellow', 'orange', 'blue'] 
    colors = ["purple"]
    colors += ["red"] * 4
    colors += ["green"] * 4
    colors += ["yellow"] * 4
    colors += ["orange"] * 4
    colors += ["blue"] * 4
    fig = go.Figure(data=go.Scatter3d(
        x=points_21_3d[:, 0],
        y=points_21_3d[:, 1],
        z=points_21_3d[:, 2],
        mode='markers',
        marker=dict(
            size=5,
            color=colors, 
            opacity=0.8
        )
    ))

    # Define the line segments
    segments = [(0, 1), (1, 2), (2, 3), (3, 4), # Thumb, 4 segments
                (0, 5), (5, 6), (6, 7), (7, 8), # Index, 4 segments
                (0, 9), (9, 10), (10, 11), (11, 12), # Middle, 4 segments
                (0, 13), (13, 14), (14, 15), (15, 16), # Ring, 4 segments
                (0, 17), (17, 18), (18, 19), (19, 20)] # Little, 4 segments

    # Create a Scatter3d trace for each segment and add to the figure
    for segment in segments:
        start_index, end_index = segment
        start_point = points_21_3d[start_index]
        end_point = points_21_3d[end_index]
        
        line = go.Scatter3d(
            x=[start_point[0], end_point[0]],
            y=[start_point[1], end_point[1]],
            z=[start_point[2], end_point[2]],
            mode='lines',
            line=dict(color='black', width=2),
            name ="",
            showlegend=False
        )
        
        fig.add_trace(line)
    
    # Set axes labels and title
    fig.update_layout(
        scene=dict(
            xaxis=dict(title='X'),
            yaxis=dict(title='Y'),
            zaxis=dict(title='Z')
        ),
        dragmode = 'orbit',
        title=title
    )
    
    # Return the figure
    return fig