#!/usr/bin/env python3

import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
import rospkg



def main():
    experiments = ['octomap_baseline_new',
                    'octomap_res005',
                    'octomap_res02',
                    'voxblox_baseline_new',
                    'voxblox_res005',
                    'voxblox_res02',
                    'skimap_baseline_new',
                    'skimap_res02',
                    'skimap_res005']
   

    data = []

    px = 1/plt.rcParams['figure.dpi']

    # Define rospack
    rospack = rospkg.RosPack()

    for experience in experiments:
        df = pd.read_csv(rospack.get_path('larcc_volumetric') + f'/results/{experience}/averages.csv')
        column_data = df.values.tolist()
        data.append([column_data[0][0], column_data[1][0], column_data[2][0]])
    print(data[0])

    # create data
    x = np.arange(3)
    width = 0.2
    
    # plot data in grouped manner of bar type
    plt.figure(figsize=(1920*px, 1080*px))
    plt.bar(x-width, data[0], width, color='blue')
    plt.bar(x, data[3], width, color='gold')
    plt.bar(x+width, data[6], width, color='green')
    plt.legend(['Octomap', 'Voxblox', 'Skimap'])
    plt.xticks(x, ["Precision", "Recall", "F1 Score"])
    plt.xlabel("Volumetric Detection Algorithms")
    plt.ylabel("Percentage")
    plt.savefig(rospack.get_path('larcc_volumetric') + '/results/figures/default_algs.svg')
    plt.savefig(rospack.get_path('larcc_volumetric') + '/results/figures/default_algs.pdf')

    plt.figure(figsize=(1920*px, 1080*px))
    plt.bar(x-width, data[1], width, color='blue')
    plt.bar(x, data[0], width, color='gold')
    plt.bar(x+width, data[2], width, color='green')
    plt.legend(['0.05', '0.1', '0.2'])
    plt.xticks(x, ["Precision", "Recall", "F1 Score"])
    plt.xlabel("Octomap Voxel Resolution")
    plt.ylabel("Percentage")
    plt.savefig(rospack.get_path('larcc_volumetric') + '/results/figures/octomap_resolution.svg')
    plt.savefig(rospack.get_path('larcc_volumetric') + '/results/figures/octomap_resolution.pdf')

    plt.figure(figsize=(1920*px, 1080*px))
    plt.bar(x-width, data[4], width, color='blue')
    plt.bar(x, data[3], width, color='gold')
    plt.bar(x+width, data[5], width, color='green')
    plt.legend(['0.05', '0.1', '0.2'])
    plt.xticks(x, ["Precision", "Recall", "F1 Score"])
    plt.xlabel("Voxblox Voxel Resolution")
    plt.ylabel("Percentage")
    plt.savefig(rospack.get_path('larcc_volumetric') + '/results/figures/voxblox_resolution.svg')
    plt.savefig(rospack.get_path('larcc_volumetric') + '/results/figures/voxblox_resolution.pdf')

    plt.figure(figsize=(1920*px, 1080*px))
    plt.bar(x-width, data[7], width, color='blue')
    plt.bar(x, data[6], width, color='gold')
    plt.bar(x+width, data[8], width, color='green')
    plt.legend(['0.05', '0.1', '0.2'])
    plt.xticks(x, ["Precision", "Recall", "F1 Score"])
    plt.xlabel("Skimap Voxel Resolution")
    plt.ylabel("Percentage")
    plt.savefig(rospack.get_path('larcc_volumetric') + '/results/figures/skimap_resolution.svg')
    plt.savefig(rospack.get_path('larcc_volumetric') + '/results/figures/skimap_resolution.pdf')

if __name__ == '__main__':
    main()