# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Python [conda env:.conda-3d_env] *
#     language: python
#     name: conda-env-.conda-3d_env-py
# ---

# +
# %matplotlib inline

import numpy as np
import sys
import os
import matplotlib.pyplot as plt
import math
import pickle
import pandas as pd
import scipy.io
import time
import h5py
import bz2
import multiprocessing as mp


from mpl_toolkits.mplot3d import Axes3D
from matplotlib.colors import ListedColormap, LinearSegmentedColormap
from mpl_toolkits.axes_grid1.axes_divider import make_axes_locatable

from numpy import linalg as LA
from scipy.spatial import Delaunay
from sklearn.neighbors import NearestNeighbors, KDTree

from sklearn.metrics import f1_score
from sklearn.metrics import precision_score
from sklearn.metrics import recall_score
from sklearn.metrics import matthews_corrcoef

#sys.path.insert(0, "../")
from info3d import *
from nn_matchers import *

# +
with open('point_collection/new_contiguous_point_collection.pickle','rb') as f: 
    new_contiguous_point_collection = pickle.load(f)
    
with open('descriptors/new_complete_res5_4by5_descriptors.pickle','rb') as f:
    descriptors = pickle.load(f)
    
with open('descriptors/new_complete_RANSAC_res5_4by5_descriptors.pickle','rb') as f:
    ransac_descriptors = pickle.load(f)
# -

# # Step 2.1: Successive case with generalized spaces

# +
descriptors = ransac_descriptors

skip = 10

t0 = time.time()

for radius in np.arange(0.5,2.1,0.5):
    
    t1 = time.time()
    t2 = t1
    
    with open('successive_sample_points_per_release.pickle','rb') as f:
        successive_sample_points_per_release = pickle.load(f)
        
    try:
        with open('testing_results/successive/radius_{}_RANSAC_successive_scores.pickle'.format(radius), 'rb') as f:
            g_successive_scores = pickle.load(f)
            
        with open('testing_results/successive/radius_{}_RANSAC_successive_errors.pickle'.format(radius), 'rb') as f:
            g_successive_errors = pickle.load(f)
            
            
        if len(g_successive_scores) != len(g_successive_errors): continue
            
        print("Done with",5*len(g_successive_scores),"releases for radius",radius)
        
    except:

        g_successive_scores = []
        g_successive_errors = []
        
        print("New successive scores for radius",radius)

    for i in np.arange(1,100,skip)[len(g_successive_scores):]: # releases
        
        release_scores = []
        
        g_release_scores = []
        
        count = 0
        
        for k, successive_sample_points_per_release_per_obj in enumerate(successive_sample_points_per_release[i]):
    
            #print("  ",len(successive_sample_points_per_release_per_obj),"releases")
            growing_point_cloud = []
            
            new_vX = []
            new_vZ = []
            
            ransac_pointCloud = []
                
            for [obj_, object_name, original_vertex], reference_ransac in successive_sample_points_per_release_per_obj[:i]:

                try:
                    if len(ransac_pointCloud) == 0: # if empty, open. This only happens at beginning
                        with open("../ransac_pc/ransac_point_collection_{}.pickle".format(reference_ransac),'rb') as f:
                            ransac_trial_point_collection = pickle.load(f)

                        object_, ransac_pointCloud, tri_ = ransac_trial_point_collection[int(obj_)]
                        
                        ransac_nbrs = NearestNeighbors(n_neighbors=min(20000,len(ransac_pointCloud)), algorithm='kd_tree').fit(ransac_pointCloud[:,:3])

                except:
                    print("Can't get ransac samples for",i,obj_, object_name)
                    continue

                dist_, ind_ = ransac_nbrs.kneighbors([original_vertex[:3]])
                pointCloud =  ransac_pointCloud[ind_[0,np.where(dist_[0]<=radius)[0]]]

                if len(pointCloud) == 0: continue
                    
                #Regular Accumulation
                if len(growing_point_cloud) == 0:
                    growing_point_cloud = pointCloud

                else:
                    growing_point_cloud = np.concatenate(
                        (growing_point_cloud,pointCloud),
                        axis=0
                    )
                    
                count += 1
                    
            if len(growing_point_cloud) == 0: 
                print("  No pc for release",i,"obj",k,"with name",object_name)
                continue
                    
            #Regular Processing
            growing_point_cloud = np.unique(growing_point_cloud,axis=0)
            
            try:
                p_descriptors, p_keypoints, p_d_c = getSpinImageDescriptors(
                    growing_point_cloud,
                    down_resolution = 5,
                    cylindrical_quantization = [4,5]
                )
            except:
                p_descriptors = []
                p_keypoints = []

            # Resetting the diff_Ratio matrix
            diff_scores = np.ones((p_descriptors.shape[0],len(descriptors),2))
            diff_ratios = np.ones((p_descriptors.shape[0],len(descriptors)))
            diff_indexs = np.ones((p_descriptors.shape[0],len(descriptors),2))

            local_keypoint_matches = []

            for i_r, ref_descriptor in enumerate(descriptors):

                r_descriptors = ref_descriptor[1]
                r_keypoints = ref_descriptor[2]

                matching_range = np.arange(r_descriptors.shape[1])

                try:    
                    f_nearestneighbor, diff = getMatches(p_descriptors,r_descriptors,2,range_to_match=matching_range)
                    diff = diff/np.amax(diff) # max-normalization of differences
                    diff_ratio = diff[:,0]/diff[:,1]
                    diff_ratios[:,i_r] = diff_ratio
                    diff_scores[:,i_r] = diff
                    diff_indexs[:,i_r] = f_nearestneighbor

                    # Taking note of the matched keypoints
                    local_keypoint_matches.append([
                        [obj_, object_name, original_vertex],
                        p_keypoints,
                        r_keypoints[f_nearestneighbor[:,0]]
                    ])

                except Exception as ex:
                    print("Error Matching:",ex)

            # Accumulating the diff_ratio matrix for every partial (rotated) object
            g_release_scores.append([
                [obj_, object_name, original_vertex],
                diff_ratios,
                diff_indexs,
                diff_scores,
                local_keypoint_matches
            ])

            if count % 33 == 0:
                #print('Test')
                print("   radius = {}: Done with {} releases ({}) ({} samples). Time to match {:.3f} seconds. ({})".format(
                    radius,
                    i,
                    k,
                    count,
                    time.time()-t2,
                    growing_point_cloud.shape
                )
                     )
                t2 = time.time()

                current_errors = NN_matcher(g_release_scores)
                print("   Error Rate:",np.sum(current_errors[:,1]/len(g_release_scores)))
            
        g_release_errors = NN_matcher(g_release_scores)
        
        try:
            print(radius,i,"Error Rate:",np.sum(g_release_errors[:,1]/len(g_release_errors)))
            #print(radius,i,"G_Error Rate:",np.sum(g_iteration_errors[:,1]/len(g_iteration_scores)))
        except:
            pass
        
        g_successive_scores.append([
            i,
            g_release_scores
        ])
        
        g_successive_errors.append([
            i,
            g_release_errors
        ])
    
        with open('testing_results/successive/radius_{}_RANSAC_successive_scores.pickle'.format(radius), 'wb') as f:
            pickle.dump(g_successive_scores,f)

        with open('testing_results/successive/radius_{}_RANSAC_successive_errors.pickle'.format(radius), 'wb') as f:
            pickle.dump(g_successive_errors,f)
            
        if i % 40  == 1:
            print("  Done with release {}, radius {} ({} actual pcs) in {:.3f} seconds".format(i,radius,count,time.time()-t1))
            t1 = time.time()

        
    print(" Done with generalized submap generation for radius {} in {:.3f} seconds".format(radius,time.time()-t0))
    t0 = time.time()
                
# -

# # Step 2.3: Results

# +
succ_NN_errors = []
succ_NN_intra_errors = []

t0 = time.time()

for radius in np.arange(0.5,2.1,0.5):
    
    succ_per_release_errors = []
    succ_per_release_intra_errors = []

    try:
        
        with open('testing_results/successive/radius_{}_RANSAC_successive_scores.pickle'.format(radius), 'rb') as f:
            g_successive_scores = pickle.load(f)
            
        with open('testing_results/successive/radius_{}_RANSAC_successive_errors.pickle'.format(radius), 'rb') as f:
            g_successive_errors = pickle.load(f)
        
        for __, release_errors in g_successive_errors:    
            #print("  RAW",radius,iteration_errors.shape)
            
            succ_per_release_errors.append(np.sum(release_errors[:,1]/len(release_errors)))
            
            intraspace_errors = release_errors[np.where(release_errors[:,1]==0)[0],2]
            
            succ_per_release_intra_errors.append([
                np.nanmean(intraspace_errors),
                np.nanstd(intraspace_errors)  
            ])
       
        succ_NN_errors.append([
            radius,
            np.asarray(succ_per_release_errors)
        ])
        
        succ_NN_intra_errors.append([
            radius,
            np.asarray(succ_per_release_intra_errors)
        ])
        
        #print("Raw",np.asarray(succ_RawNN_per_iteration_errors).shape)

    except Exception as ex:
        print(radius,": successive error results", ex)
        pass
    
with open('testing_results/successive/nn_matcher_errors.pickle', 'wb') as f:
    pickle.dump([succ_NN_errors,succ_NN_intra_errors],f)

# +
fig=plt.figure(figsize=(12, 3.25))

markersize = 6
linewidth = 2

ax1 = fig.add_subplot(121) 

ax1.grid(alpha = 0.7)
ax1.set_ylim(-0.025,1.025)
ax1.set_xlim(0,100)

for tick in ax1.xaxis.get_major_ticks():
    tick.label.set_fontsize(12) 

for tick in ax1.yaxis.get_major_ticks():
    tick.label.set_fontsize(12) 
    # specify integer or one of preset strings, e.g.
    #tick.label.set_fontsize('x-small') 
#ax1.set_xticklabels(np.arange(1,3.1,0.5),fontsize = 12)
#ax1.set_yticklabels([-0.1,0,0.2,0.4,0.6,0.8,1.0],fontsize = 12)
ax1.set_ylabel("INTER-space Privacy, $\Pi_1$", fontsize = 14)
ax1.set_xlabel("Number of Releases", fontsize = 14)

for radius, inter_errors in succ_NN_errors:#[:2]:

    if radius == 1.5: continue

    #print(RawNN_per_iteration_errors.shape)
    ax1.plot(
        np.arange(1,100,skip)[:len(inter_errors):]+1,
        inter_errors[::], 
        '-o', 
        linewidth = linewidth,fillstyle = 'none',
        mew = linewidth,markersize = min(markersize*(1.25/radius),markersize),
        label = "r ="+ str(radius) + ", $\Pi_{1,NN}$"
    )
    

ax1.legend(loc = "best", ncol = 1,fontsize = 11)

ax2 = fig.add_subplot(122) 

ax2.grid(alpha = 0.7)
ax2.set_ylim(-0.25,10.25)
ax2.set_xlim(0,100)

for tick in ax2.xaxis.get_major_ticks():
    tick.label.set_fontsize(12) 

for tick in ax2.yaxis.get_major_ticks():
    tick.label.set_fontsize(12) 
    # specify integer or one of preset strings, e.g.
    #tick.label.set_fontsize('x-small') 
#ax1.set_xticklabels(np.arange(1,3.1,0.5),fontsize = 12)
#ax1.set_yticklabels([-0.1,0,0.2,0.4,0.6,0.8,1.0],fontsize = 12)
ax2.set_ylabel("INTRA-space Privacy, $\Pi_2$ (m)", fontsize = 14)
ax2.set_xlabel("Number of Releases", fontsize = 14)

for radius, intra_errors in succ_NN_intra_errors:#[:2]:
    
    if radius == 1.5: continue
    
    ax2.plot(
        np.arange(1,100,skip)+1,#[:len(intra_errors):2]+1,
        intra_errors[::,0], 
        '-.o',
        linewidth = linewidth, #capsize = 4.0, 
        #marker = markers[0],
        fillstyle = 'none',
        mew = linewidth,markersize = min(markersize*(1.25/radius),markersize),
        label = "r = "+ str(radius) + ", $\Pi_{2,NN}$"
    )
    
#    print(intra_errors[:,0])

ax2.legend(loc = "best", ncol = 1,fontsize = 11)

# -



StarcoderdataJupyterScriptsDedupFiltered