#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Analysis of single run of Au sphere field for a visible monochromatic incident wave.

See also
--------
Routines/np_monoch_field
"""

import imageio as mim
import matplotlib.pyplot as plt
import matplotlib.gridspec as gridspec
import numpy as np
import os
import vmp_utilities as vmu
import vmp_analysis as vma
import v_plot as vp
import v_utilities as vu

vp.set_style()

#%% PARAMETERS

"""
series = "DTest532"
folder = "Field/NPMonoch/AuSphere/VacWatTest/DefinitiveTest/Vacuum"

hfield = False

make_plots = True
make_gifs = True

english = False
maxnframes = 300
"""

#%%

def plot_np_monoch_field_cell(params, series, folder,
                              with_nanoparticle=True, english=False):

    #%% SETUP

    # Computation
    pm = vmu.ParallelManager()
    n_processes, n_cores, n_nodes = pm.specs
    parallel = pm.parallel

    # Saving directories
    sa = vmu.SavingAssistant(series, folder)
    home = sa.home
    sysname = sa.sysname
    path = sa.path

    trs = vu.BilingualManager(english=english)

    #%% DATA EXTRACTION

    from_um_factor = params["from_um_factor"]

    cell_width = params["cell_width"]
    pml_width = params["pml_width"]

    r = params["r"]
    material = params["material"]

    wlen = params["wlen"]
    source_center = params["source_center"]

    submerged_index = params["submerged_index"]
    surface_index = params["surface_index"]
    overlap = params["overlap"]

    #%% PLOT

    if pm.assign(0):

        fig, ax = plt.subplots()
        ax.grid(False)

        # PML borders
        pml_out_square = plt.Rectangle((-cell_width/2, -cell_width/2),
                                       cell_width, cell_width,
                                       fill=False, edgecolor="m", linestyle="dashed",
                                       hatch='/',
                                       zorder=-20,
                                       label=trs.choose("PML borders", "Bordes PML"))
        pml_inn_square = plt.Rectangle((-cell_width/2+pml_width,
                                        -cell_width/2+pml_width),
                                       cell_width - 2*pml_width, cell_width - 2*pml_width,
                                       facecolor="white", edgecolor="m",
                                       linestyle="dashed", linewidth=1, zorder=-10)

        # Surrounding medium
        if submerged_index != 1:
            surrounding_square = plt.Rectangle((-cell_width/2, -cell_width/2),
                                               cell_width, cell_width,
                                               color="blue", alpha=.1, zorder=-6,
                                               label=trs.choose(fr"Medium $n$={submerged_index}",
                                                                fr"Medio $n$={submerged_index}"))

        # Surface medium
        if surface_index != submerged_index:
            surface_square = plt.Rectangle((r - overlap, -cell_width/2),
                                           cell_width/2 - r + overlap,
                                           cell_width,
                                           edgecolor="navy", hatch=r"\\",
                                           fill=False, zorder=-3,
                                           label=trs.choose(fr"Surface $n$={surface_index}",
                                                            fr"Superficie $n$={surface_index}"))
        # Nanoparticle
        if with_nanoparticle:
            if material=="Au":
                circle_color = "gold"
            elif material=="Ag":
                circle_color="silver"
            else:
                circle_color="peru"
            circle = plt.Circle((0,0), r, color=circle_color, linewidth=1, alpha=.4,
                                zorder=0, label=trs.choose(f"{material} Nanoparticle",
                                                              f"Nanopartícula de {material}"))

        # Source
        ax.vlines(source_center, -cell_width/2, cell_width/2,
                  color="r", linestyle="dashed", zorder=5,
                  label=trs.choose("Planewave Source", "Fuente de ondas plana"))

        # Sampling line
        ax.hlines(0, -cell_width/2, cell_width/2,
                  color="limegreen", linestyle=":", zorder=7,
                  label=trs.choose("Sampling Line", "Línea de muestreo"))

        # Sampling plane
        if with_nanoparticle:
            ax.vlines(0, -cell_width/2, cell_width/2,
                      color="limegreen", linestyle="dashed", zorder=7,
                      label=trs.choose("Sampling Plane", "Plano de muestreo"))

        if with_nanoparticle: ax.add_patch(circle)
        if submerged_index!=1: ax.add_patch(surrounding_square)
        if surface_index!=submerged_index: ax.add_patch(surface_square)
        ax.add_patch(pml_out_square)
        ax.add_patch(pml_inn_square)

        # General configuration
        box = ax.get_position()
        width = box.x1 - box.x0
        height = box.y1 - box.y0
        box.x0 = box.x0 - .26 * width
        box.x1 = box.x0 + width
        box.y0 = box.y0 + .12 * height
        box.y1 = box.y0 + height
        ax.set_position(box)
        plt.legend(bbox_to_anchor=trs.choose( (1.8, 0.5), (1.87, 0.5) ),
                   loc="center right", frameon=False)

        fig.set_size_inches(7.5, 4.8)
        ax.set_aspect("equal")
        plt.xlim(-cell_width/2, cell_width/2)
        plt.ylim(-cell_width/2, cell_width/2)
        plt.xlabel(trs.choose(r"Position $X$ [MPu]", r"Posición $X$ [uMP]"))
        plt.ylabel(trs.choose(r"Position $Z$ [MPu]", r"Posición $Z$ [uMP]"))

        plt.annotate(fr"$\lambda$ = {wlen * from_um_factor * 1e3:.0f} nm",
                     (220, 15), xycoords='figure points', color="r")

        plt.annotate(trs.choose(f"1 MPu = {from_um_factor * 1e3:.0f} nm",
                                f"1 uMP = {from_um_factor * 1e3:.0f} nm"),
                     (5, 15), xycoords='figure points')

        fig.set_size_inches([6.68, 3.68])

        #%%

        if with_nanoparticle:
            plt.savefig(sa.file("MiniSimBox.png"))
        else:
            plt.savefig(sa.file("MiniSimBoxNorm.png"))

#%%

def plots_np_monoch_field(series, folder, hfield=False,
                          make_plots=True, make_gifs=False,
                          english=False, maxnframes=300):

    #%% SETUP

    # Computation
    pm = vmu.ParallelManager()
    n_processes, n_cores, n_nodes = pm.specs
    parallel = pm.parallel

    # Saving directories
    sa = vmu.SavingAssistant(series, folder)
    home = sa.home
    sysname = sa.sysname
    path = sa.path

    trs = vu.BilingualManager(english=english)

    #%% EXIT IF NOTHING TO BE DONE

    if not make_plots and not make_gifs:
        return

    #%% GET READY TO LOAD DATA

    f = pm.hdf_file(sa.file("Field-Lines.h5"), "r+")
    results_line = f["Ez"]
    t_line = np.array(f["T"])
    x_line = np.array(f["X"])

    g = pm.hdf_file(sa.file("Field-Planes.h5"), "r+")
    results_plane = g["Ez"]
    t_plane = np.array(g["T"])
    y_plane = np.array(g["Y"])
    z_plane = np.array(g["Z"])

    params = dict(f["Ez"].attrs)

    from_um_factor = params["from_um_factor"]

    r = params["r"]
    material = params["material"]

    wlen = params["wlen"]

    cell_width = params["cell_width"]
    pml_width = params["pml_width"]
    source_center = params["source_center"]

    until_time = params["until_time"]
    period_line = params["period_line"]
    period_plane = params["period_plane"]

    try:
        norm_until_time = params["norm_until_time"]
        norm_amplitude = params["norm_amplitude"]
        norm_period = params["norm_period"]
        norm_path = params["norm_path"]
        requires_normalization = False
    except:
        requires_normalization = True

    try:

        try:
            h = pm.hdf_file(sa.file("Field-Lines-Norm.h5"), "r")

        except:
            h = pm.hdf_file(os.path.join(norm_path, "Field-Lines-Norm.h5"), "r")

        print("Loaded available normfield")

    except:

        norm_path = vmu.check_normfield(params)

        try:
            h = pm.hdf_file(os.path.join(norm_path[0], "Field-Lines-Norm.h5"), "r")
            print("Loaded compatible normfield")

        except:
            h = f
            print(f"Using NP data for normalization.",
                  "This is by all means not ideal!!",
                  "If possible, run again these simulations.")

    results_line_norm = h["Ez"]
    t_line_norm = np.asarray(h["T"])
    x_line_norm = np.asarray(h["X"])

    #%% POSITION RECONSTRUCTION

    t_line_index = vma.def_index_function(t_line)
    x_line_index = vma.def_index_function(x_line)

    t_plane_index = vma.def_index_function(t_plane)
    y_plane_index = vma.def_index_function(y_plane)
    z_plane_index = vma.def_index_function(z_plane)

    t_line_norm_index = vma.def_index_function(t_line_norm)
    x_line_norm_index = vma.def_index_function(x_line_norm)

    x_line_cropped = x_line[:x_line_index(cell_width/2 - pml_width)+1]
    x_line_cropped = x_line_cropped[x_line_index(-cell_width/2 + pml_width):]

    y_plane_cropped = y_plane[:y_plane_index(cell_width/2 - pml_width)+1]
    y_plane_cropped = y_plane_cropped[y_plane_index(-cell_width/2 + pml_width):]

    z_plane_cropped = z_plane[:z_plane_index(cell_width/2 - pml_width)+1]
    z_plane_cropped = z_plane_cropped[z_plane_index(-cell_width/2 + pml_width):]

    #%% DATA EXTRACTION

    source_results = vma.get_source_from_line(results_line_norm,
                                              x_line_norm_index,
                                              source_center)

    if not requires_normalization:

        period, amplitude = norm_period, norm_amplitude

    else:

        period = vma.get_period_from_source(source_results, t_line)[-1]
        amplitude = vma.get_amplitude_from_source(source_results)[-1]

        results_plane = np.asarray(results_plane) / amplitude
        results_line = np.asarray(results_line) / amplitude

    sim_source_results = vma.get_source_from_line(results_line,
                                                  x_line_index,
                                                  source_center)

    zprofile_results = vma.get_zprofile_from_plane(results_plane, y_plane_index)

    zprofile_zintegral = vma.z_integrate_field_zprofile(zprofile_results, z_plane,
                                                       z_plane_index,
                                                       cell_width, pml_width)

    zprofile_max = vma.find_zpeaks_zprofile(zprofile_results, z_plane_index,
                                            cell_width, pml_width)[-1]

    field_peaks_all_index = vma.get_all_field_peaks_from_yzplanes(results_plane,
                                                                  y_plane_index,
                                                                  z_plane_index,
                                                                  cell_width,
                                                                  pml_width)[0]

    field_peaks_results = vma.get_mean_field_peak_from_yzplanes(results_plane,
                                                                y_plane_index,
                                                                z_plane_index,
                                                                cell_width,
                                                                pml_width)
    field_peaks_single_index = field_peaks_results[0]
    field_peaks_zprofile = field_peaks_results[2]
    field_peaks_plane = field_peaks_results[3]
    del field_peaks_results

    zprofile_cropped = vma.crop_field_zprofile(zprofile_results,
                                               z_plane_index,
                                               cell_width,
                                               pml_width)
    results_plane_cropped = vma.crop_field_yzplane(results_plane,
                                                   y_plane_index,
                                                   z_plane_index,
                                                   cell_width,
                                                   pml_width)

    #%% SHOW SOURCE AND FOURIER USED FOR NORMALIZATION

    if make_plots and pm.assign(1):

        fig = plt.figure()
        plt.plot(t_line_norm, source_results/amplitude, linewidth=2)
        plt.xlabel(trs.choose("Time [MPu]", "Tiempo [uMP]"))
        plt.ylabel(trs.choose(r"Electric Field $E_z(y=z=0)$",
                              r"Campo eléctrico $E_z(y=z=0)$"))

        fig.set_size_inches([3.69, 3.15])
        plt.tight_layout()

        plt.savefig(sa.file("MiniSource.png"))

    #%% SHOW FIELD PEAKS INTENSIFICATION OSCILLATIONS

    if make_plots and pm.assign(0):

        fig = plt.figure()
        plt.plot(t_plane, zprofile_max, linewidth=2)
        plt.xlabel(trs.choose("Time [MPu]", "Tiempo [uMP]"))
        plt.ylabel(trs.choose("Electric Field Maximum\n"+r"$max[ E_z(z) ]$",
                              "Máximo del campo eléctrico\n"+r"$max[ E_z(z) ]$"))

        fig.set_size_inches([4.12, 3.15])
        plt.tight_layout()

        plt.savefig(sa.file("MiniMaximum.png"))

    #%% PLOT MAXIMUM INTENSIFICATION PROFILE

    if make_plots and pm.assign(1):

        fig = plt.figure()
        plt.plot(z_plane_cropped,
                 field_peaks_zprofile, linewidth=2)
        plt.axhline(linewidth=1, color="k")
        plt.axvline(linewidth=1, color="k")
        plt.axvline(-r, color="k", linestyle="dashed", linewidth=1)
        plt.axvline(r, color="k", linestyle="dashed", linewidth=1)

        plt.xlim(min(z_plane_cropped), max(z_plane_cropped))
        plt.xlabel(trs.choose(r"Position $Z$ [MPu]", r"Posición $Z$ [uMP]"))
        plt.ylabel(trs.choose(r"Electric Field $E_z(y=z=0)$",
                              r"Campo eléctrico $E_z(y=z=0)$"))

        fig.set_size_inches([3.69, 3.15])
        plt.tight_layout()

        plt.savefig(sa.file("MiniIntensification.png"))

    #%% PLOT A PIECE OF THE NORMALIZATION LINES FILE

    if make_plots and pm.assign(1):

        fig = plt.figure()
        plt.plot(x_line_norm,
                 results_line_norm[...,200], linewidth=2)
        plt.axhline(linewidth=1, color="k")
        plt.axvline(linewidth=1, color="k")
        plt.axvline(-cell_width/2 + pml_width, color="k", linestyle="dashed")
        plt.axvline(cell_width/2 - pml_width, color="k", linestyle="dashed")

        plt.xlim(min(x_line_norm), max(x_line_norm))
        plt.xlabel(trs.choose(r"Position $X$ [MPu]", r"Posición $X$ [uMP]"))
        plt.ylabel(trs.choose(r"Electric Field $E_z(y=z=0)$",
                              r"Campo eléctrico $E_z(y=z=0)$"))

        fig.set_size_inches([3.69, 3.15])
        plt.tight_layout()

        plt.savefig(sa.file("MiniNormLines.png"))

    #%% MAKE ALL GIF

    if make_gifs and pm.assign(1):

        # What should be parameters
        nframes = min(maxnframes, results_plane.shape[-1])
        nframes = int( vu.round_to_multiple(nframes, params["time_period_factor"] ) )
        nframes_period = int(nframes/np.max(params["time_period_factor"]))

        cut_points = []
        for k in range(int(params["time_period_factor"])):
            cut_points.append( np.argmin(np.abs(t_line / period - (k + 1))) )
        cut_points = [0, *cut_points]

        frames_index = []
        for k in range(len(cut_points)):
            if k < len(cut_points)-1:
                intermediate_frames = np.linspace(
                    cut_points[k],
                    cut_points[k+1],
                    nframes_period+1)[:-1]
                intermediate_frames = [int(fr) for fr in intermediate_frames]
                frames_index = [*frames_index, *intermediate_frames]

        # Animation base
        fig = plt.figure()
        plot_grid = gridspec.GridSpec(ncols=2, nrows=1, figure=fig)
        line_ax = fig.add_subplot(plot_grid[:,0])
        plane_ax = fig.add_subplot(plot_grid[:,-1])
        fig.set_size_inches([8.07 , 3.47])

        # line_ax.set_aspect('equal')
        line_ax_lims = (np.min(results_line), np.max(results_line))

        plane_ax.set_aspect('equal')
        plane_ax_lims = (np.min(results_plane), np.max(results_plane))
        plane_ax_lims = max([abs(l) for l in plane_ax_lims])
        plane_ax_lims = [-plane_ax_lims, plane_ax_lims]
        move_on_k = 330

        def make_pic(k):
            line_ax.clear()
            plane_ax.clear()

            line_ax.plot(x_line, results_line[...,k], linewidth=2)
            line_ax.axvline(-cell_width/2 + pml_width, color="k", linestyle="dashed")
            line_ax.axvline(cell_width/2 - pml_width, color="k", linestyle="dashed")
            line_ax.axhline(0, color="k", linewidth=1)
            line_ax.axvline(0, color="k", linewidth=1)
            line_ax.axvline(r, color="k", linestyle="dotted", linewidth=1)
            line_ax.axvline(-r, color="k", linestyle="dotted", linewidth=1)

            line_ax.set_xlabel(trs.choose("Position $X$ [MPu]", "Posición $X$ [uMP]"))
            line_ax.set_ylabel(trs.choose(r"Electric Field $E_z(y=z=0)$",
                                          r"Campo eléctrico $E_z(y=z=0)$"))
            line_ax.set_xlim(min(x_line), max(x_line))

            ims = plane_ax.imshow(results_plane[...,k].T,
                                  cmap='RdBu', #interpolation='spline36',
                                  vmin=plane_ax_lims[0], vmax=plane_ax_lims[1],
                                  extent=[min(y_plane), max(y_plane),
                                          min(z_plane), max(z_plane)])
            plane_ax.axhline(-cell_width/2 + pml_width, color="k", linestyle="dashed", linewidth=1)
            plane_ax.axhline(cell_width/2 - pml_width, color="k", linestyle="dashed", linewidth=1)
            plane_ax.axvline(-cell_width/2 + pml_width, color="k", linestyle="dashed", linewidth=1)
            plane_ax.axvline(cell_width/2 - pml_width, color="k", linestyle="dashed", linewidth=1)
            plane_ax.axhline(0, color="k", linewidth=1)
            plane_ax.axvline(0, color="k", linewidth=1)
            plane_ax.axhline(r, color="k", linestyle="dotted", linewidth=1)
            plane_ax.axhline(-r, color="k", linestyle="dotted", linewidth=1)
            plane_ax.axvline(r, color="k", linestyle="dotted", linewidth=1)
            plane_ax.axvline(-r, color="k", linestyle="dotted", linewidth=1)

            plane_ax.grid(False)
            plane_ax.set_xlabel(trs.choose("Position $Y$ [MPu]", "Posición $Y$ [uMP]"))
            plane_ax.set_ylabel(trs.choose("Position $Z$ [MPu]", "Posición $Z$ [uMP]"))

            if k == move_on_k:
                for ax in [line_ax, plane_ax]:
                    box = ax.get_position()
                    width = box.x1 - box.x0
                    height = box.y1 - box.y0
                    box.x0 = box.x0 - .1 * width
                    box.x1 = box.x0 + 0.9 * width
                    box.y0 = box.y0 + .1 * height
                    box.y1 = box.y0 + 0.9 * height
                    ax.set_position(box)

            line_ax.text(-.15, 1.03, trs.choose(f'Time: {t_line[k]/period:.1f} MPu',
                                                f'Tiempo: {t_line[k]/period:.1f} uMP'),
                         transform=line_ax.transAxes)
            plt.annotate(trs.choose(f"1 MPu = {from_um_factor * 1e3:.0f} nm",
                                    f"1 uMP = {from_um_factor * 1e3:.0f} nm"),
                         (425, 225), xycoords='figure points')
            line_ax.set_ylim(*line_ax_lims)

            cax = plane_ax.inset_axes([1.04, 0, 0.07, 1], #[1.04, 0.2, 0.05, 0.6],
                                      transform=plane_ax.transAxes)
            cbar = fig.colorbar(ims, ax=plane_ax, cax=cax)
            cbar.set_label(trs.choose("Electric Field $E_z$",
                                      "Campo eléctrico $E_z$"))

            plt.show()
            return line_ax, plane_ax

        def make_gif(gif_filename):
            pics = []
            for ik, k in enumerate(frames_index):
                line_ax, plane_ax = make_pic(k)
                plt.savefig('temp_pic.png')
                pics.append(mim.imread('temp_pic.png'))
                print(str(ik+1)+'/'+str(nframes))
            mim.mimsave(gif_filename+'.gif', pics, fps=5)
            os.remove('temp_pic.png')
            print('Saved gif')

            # 330

        #%%

        make_gif(sa.file("All"))
        plt.close(fig)
        # del fig, ax, lims, nframes_step, nframes, call_series, label_function

    #%%

    f.close()
    g.close()
    try:
        h.close()
    except:
        pass


import re
import matplotlib.pyplot as plt
import matplotlib.widgets as widg

book = open('journal-1900.txt', encoding='utf-8')
book_string = book.read()
book.close()

# some wired punctuation characters that you cannot type, you have to copy them
book_string_nopunc = re.sub('[.!?“”,‘’:;]', '', book_string)

# two types of dashes (oh god)
book_string_nopunc = re.sub('[—-]', ' ', book_string_nopunc)

# seems to work
# print(book_string_nopunc)

# now we need to remove all uppercase characters
book_string_nofrills = book_string_nopunc.lower()
# print(book_string_nofrills)

# now we need a list of words from a paragraph
words = book_string_nofrills.split()
words.sort()
# print(words)

words_and_count = {}

for word in words:
    if word in words_and_count:
        words_and_count[word] += 1
    else:
        words_and_count[word] = 1

# graph words with more than threshold
words_and_count_hicount = {}

thresh = 25
for word in words_and_count:
    if words_and_count[word] > 25:
        words_and_count_hicount[word] = words_and_count[word]

print(words_and_count_hicount)

# getting words_x and count_y from words_and_count_hicount
words_x = [word for word in words_and_count_hicount]
count_y = [words_and_count_hicount[word] for word in words_and_count_hicount]

# bar graph time!

# number of words
words_number = len(words_x)
indexes = list(range(words_number))

fig, ax = plt.subplots()

plt.bar(indexes, count_y)
plt.xticks(indexes, words_x)

# make a bar graph that shows 20 words at a time
# and has a slider which allows horizontal navigation

# [left, bottom, width, height]
slider_ax = plt.axes([0.15, 0.9, 0.5, 0.1])
slider = widg.Slider(slider_ax, 'sector', 0, 1, valinit=0)

ax.set_xlim(-0.5, 20.5)

def sector(val):
    left_edge = (words_number - 21) * slider.val - 0.5
    ax.set_xlim(left_edge, left_edge + 21)

slider.on_changed(sector)

plt.show()


import streamlit as st
import tensorflow as tf
import os

def main():
    st.title("Baggage Detection App")

    # Load the trained model
    #model_version = max([int(i) for i in os.listdir("models") + [0]])
    model_path = f"models/1"
    model = tf.keras.models.load_model(model_path)

    # Upload and classify images
    uploaded_image = st.file_uploader("Upload an image", type=["png", "jpg"])
    if uploaded_image is not None:
        # Perform image classification using the uploaded image and the loaded model
        image = tf.io.decode_image(uploaded_image.read(), channels=3)
        image = tf.image.resize(image, [640, 640])
        image = image / 255.0  # Normalize the image
        image = tf.expand_dims(image, axis=0)  # Add batch dimension

        # Make predictions
        predictions = model.predict(image)
        labels = ['Gun', 'Knife', 'Pliers', 'Scissors', 'Wrench']  # Replace with your actual labels

        # Display the results
        st.subheader("Prediction Results:")
        for i, label in enumerate(labels):
            probability = predictions[0][i]
            st.write(f"{label}: {probability}")

        # Display the uploaded image
        st.image(image.numpy().squeeze(), channels="RGB", use_column_width=True)

if __name__ == "__main__":
    main()


import numpy as np
from ..base import less_equal
from bisect import bisect_left,bisect_right
from bintrees import RBTree
from sklearn.neighbors import KDTree
from sklearn.neighbors.dist_metrics import EuclideanDistance

OPTICS_UNDEFINED = np.inf
EPS = np.finfo(float).eps

check_uppoint = lambda r_current,r_next,xi:False if r_current is OPTICS_UNDEFINED else less_equal(r_current,r_next*(1-xi),tol=1e-10)
check_downpoint = lambda r_current,r_next,xi:False if r_current is OPTICS_UNDEFINED else less_equal(r_next,r_current*(1-xi),tol=1e-10)

#check if index_start is the start of a up area or down area
def is_start_of_area(r,xi,index_start,minpts,minlen,up=True):
	if r[index_start] == OPTICS_UNDEFINED:
		return False,None
	check_point = check_uppoint if up else check_downpoint

	r_current,start_point,last_point,count_exception = r[index_start],None,None,0
	for point in range(index_start,len(r)-1):
		r_next = r[point+1]
		if check_point(r_current,r_next,xi):
			count_exception,last_point = 0,point
			if start_point is None:
				start_point = point

		elif start_point is None:
			return False,None

		else:
			count_exception += 1

			if ( r_next<r_current if up else r_next>r_current ) or count_exception == minpts:
				return (True,last_point) if last_point-start_point+1>=minlen else (False,None)

		r_current = r_next

	return (True,last_point) if last_point-start_point+1>=minlen else (False,None)

#delete down areas and update local mib
def update_mib_and_filter(down_area_starts,down_area_ends,down_area_mibs,xi,mib):
	n_delete = 0
	for index in range(len(down_area_mibs)):
		index_ = index-n_delete
		down_area_mibs[index_] = max(mib,down_area_mibs[index_])
		if down_area_mibs[index_]*(1-xi)<mib:
			down_area_starts.pop(index_)
			down_area_ends.pop(index_)
			down_area_mibs.pop(index_)
			n_delete += 1

#construct ξ-area by combining down area and up area
def combine_area(r,down_area_start,down_area_end,up_area_start,up_area_end,down_area_mib,xi,minpts):
	r_sd,r_eu_plus_one = r[down_area_start],r[up_area_end+1]

	#satisify (sc2*) and (3a)
	if r[up_area_end]*(1-xi)>=down_area_mib and up_area_end-down_area_start+1>=minpts:
		s,e = down_area_start,up_area_end

		if r_sd*(1-xi) >= r_eu_plus_one:
			r_end = len(r)-1
			s = r_end - bisect_right(r[::-1],r_eu_plus_one,r_end-down_area_end,r_end-down_area_start)
		elif r_eu_plus_one*(1-xi) >= r_sd:
			e = bisect_right(r,r_sd,up_area_start,up_area_end)
	else:
		s,e = None,None

	return s,e

#extract clusters from r plot
#implement Algorithm ExtractClusters in <<OPTICS: Ordering Points To Identify the Clustering Structure>> Figure 19
#minlen是down area或up area的最小长度
def get_clusters_auto(r,xi,minpts,minlen=1):
	mib,index = 0,0
	cluster_starts,cluster_ends = [],[]
	down_area_starts,down_area_ends,down_area_mibs = [],[],[]
	while index<len(r)-1:
		mib = max(mib,r[index])
		is_start_of_down_area,end_index = is_start_of_area(r,xi,index,minpts,minlen,up=False)

		if is_start_of_down_area:
			update_mib_and_filter(down_area_starts,down_area_ends,down_area_mibs,xi,mib)
			down_area_starts.append(index)
			down_area_ends.append(end_index)
			down_area_mibs.append(0.)

			index,mib = end_index + 1,0
		else:
			is_start_of_up_area,end_index = is_start_of_area(r,xi,index,minpts,minlen,up=True)

			if is_start_of_up_area:
				update_mib_and_filter(down_area_starts,down_area_ends,down_area_mibs,xi,mib)
				up_area_start,up_area_end = index,end_index
				index,mib = end_index +1,0

				for down_area_start,down_area_end,down_area_mib in zip(down_area_starts,down_area_ends,down_area_mibs):
					s,e = combine_area(r,down_area_start,down_area_end,up_area_start,up_area_end,down_area_mib,xi,minpts)
					if not s is None:
						cluster_starts.append(s)
						cluster_ends.append(e)

			else:
				index += 1

	return cluster_starts,cluster_ends

#a stupid way to get minpts different neighbors and k-distance
def get_minpts_neighbors(x,neighbor_indices,neighbor_dists,minpts,kdtree):
	search_k = max(2*neighbor_indices.shape[0],minpts)
	while True:
		ele,indices = np.unique(neighbor_dists,return_index=True,sorted=True)
		#less than minpts different neighbors
		if ele.shape[0]<minpts:
			try:
				#search neighbors with double rad
				neighbor_dists,neighbor_indices = kdtree.query(x,search_k,return_distance=True,sort_results=True)
				neighbor_dists,neighbor_indices = neighbor_dists[0],neighbor_indices[0]

			#can't find minpts different neighbors
			except ValueError:
				raise ValueError("error:can't find minpts different neighbors")

			search_k *= 2
			continue
		break

	end_index = neighbor_indices.shape[0] if ele.shape[0]==minpts else indices[minpts]
	k_distance,neighbor_indices = neighbor_dists[end_index-1],neighbor_indices[:end_index].astype(np.uint32)
	return neighbor_indices,k_distance

#获取各个数据对象的local outlier factor
def get_of(X,minpts,metric,neighbor_indices=None,k_distance=None):
	N = X.shape[0]
	kdtree = KDTree(X,metric=metric)
	lrd = np.empty(N,np.float64)

	if neighbor_indices is None:
		cd,neighbor_indices = np.empty(N,np.float64),np.empty(N,np.object)
		for index in range(N):
			object_neighbor_dists,object_neighbor_indices = kdtree.query(X[index,np.newaxis],minpts,return_distance=True,sort_results=True)
			object_neighbor_dists,object_neighbor_indices = object_neighbor_dists[0],object_neighbor_indices[0]

			neighbor_indices[index],cd[index] = get_minpts_neighbors(X,object_neighbor_indices,object_neighbor_dists,minpts,kdtree)
	else:
		cd = k_distance

	for index in range(N):
		neighbors = neighbor_indices[index]
		neighbor_cds,neighbor_dists = cd[neighbors],metric.pairwise(X[index,np.newaxis],X[neighbors])[0]
		rd = np.where(neighbor_cds>neighbor_dists,neighbor_cds,neighbor_dists)
		lrd[index] = neighbors.shape[0]/np.sum(rd)

	of = cd
	for index in range(N):
		neighbor_lrds = lrd[neighbor_indices[index]]
		of[index] = np.sum(neighbor_lrds)/lrd[index]/neighbor_lrds.shape[0]

	return of

#implement Method OrderSeeds::update in <<OPTICS: Ordering Points To Identify the Clustering Structure>> Figure 7
def orderseed_update(X,orderseeds,cd,rd,processed,object_index,neighbor_indices,neighbor_dists):
	cd_object = cd[object_index]
	for neighbor_index,neighbor_dist in zip(neighbor_indices,neighbor_dists):
		#update orderseeds and reach distance of neighbors
		if not processed[neighbor_index]:
			rd_neighbor = max(cd_object,neighbor_dist)
			if rd_neighbor < rd[neighbor_index]:
				if rd[neighbor_index]!=OPTICS_UNDEFINED:
					indices = orderseeds.get(rd[neighbor_index])
					indices.pop(bisect_left(indices,neighbor_index))
					if len(indices)==0:
						orderseeds.remove(rd[neighbor_index])

				#a simple way to deal problem of duplicate value
				indices = orderseeds.get(rd_neighbor)
				if indices is None:
					indices = []
					orderseeds.insert(rd_neighbor,indices)
				indices.insert(bisect_left(indices,neighbor_index),neighbor_index)
				rd[neighbor_index] = rd_neighbor

#implement Procedure ExpandClusterOrder in <<OPTICS: Ordering Points To Identify the Clustering Structure>> Figure 6
def expand_cluster_order(X,index,minpts,rad,cd,rd,order,n_order,processed,kdtree,minpts_neighbor_indices,k_distance,return_of):
	processed[index] = True
	order[n_order] = index
	n_order += 1

	neighbor_indices,neighbor_dists = kdtree.query_radius(X[index,np.newaxis],rad,return_distance=True,sort_results=True)
	neighbor_indices,neighbor_dists = neighbor_indices[0],neighbor_dists[0]
	if return_of:
		minpts_neighbor_indices[index],k_distance[index] = get_minpts_neighbors(X[index,np.newaxis],neighbor_indices,neighbor_dists,minpts,kdtree)

	if neighbor_indices.shape[0]>=minpts:
		cd[index] = neighbor_dists[minpts-1]

		orderseeds = RBTree()		#use Red Black Tree to implement priority queue
		orderseed_update(X,orderseeds,cd,rd,processed,index,neighbor_indices,neighbor_dists)

		while not orderseeds.is_empty():
			indices = orderseeds.nsmallest(1)[0][1]
			current_object_index = indices.pop()
			if len(indices)==0:
				orderseeds.pop_min()

			processed[current_object_index] = True
			order[n_order] = current_object_index
			n_order += 1

			neighbor_indices,neighbor_dists = kdtree.query_radius(X[current_object_index,np.newaxis],rad,return_distance=True,sort_results=True)
			neighbor_indices,neighbor_dists = neighbor_indices[0],neighbor_dists[0]

			if return_of:
				minpts_neighbor_indices[current_object_index],k_distance[current_object_index] = get_minpts_neighbors(X[current_object_index,np.newaxis],neighbor_indices,neighbor_dists,minpts,kdtree)

			if neighbor_indices.shape[0]>=minpts:
				cd[current_object_index] = neighbor_dists[minpts-1]
				orderseed_update(X,orderseeds,cd,rd,processed,current_object_index,neighbor_indices,neighbor_dists)

		orderseeds.clear()
	return n_order

#implement Algorithm OPTICS in <<OPTICS: Ordering Points To Identify the Clustering Structure>> Figure 5
def optics(X,minpts,rad,metric=None,return_of=False):
	"""
		OPTICS算法
		参数：
			①X：2D array。存储数据对象
			②minpts、rad：OPTICS算法参数，rad-neighborhood的大小大于等于minpts的数据对象将成为core-point
			③metric：用于计算两个数据对象距离的对象，应使用sklearn.neighbors.dist_metrics中的距离对象。若metric为None，则使用EuclideanDistance
			④return_of：是否返回各个数据对象的outlier factor，默认为False

		OPTICS返回cluster-order、core-distance、reachability-distance(以及可选的outlier factor)，通过将这些返回值(除outlier factor外)传入到函数extract_cluster或get_clusters_auto中，可以对数据对象进行聚类，后者进行分层聚类
	"""
	metric = EuclideanDistance() if metric is None else metric
	N = X.shape[0]

	order,n_order = np.empty(N,np.uint32),0
	cd,rd = np.empty(N,np.float64),np.empty(N,np.float64)
	cd[:],rd[:] = OPTICS_UNDEFINED,OPTICS_UNDEFINED
	processed = np.zeros(N,np.bool8)
	kdtree = KDTree(X,metric=metric)

	minpts_neighbor_indices,k_distance = (np.empty(N,np.object),np.empty(N,np.float64)) if return_of else (None,None)

	_indices_ = np.random.choice(range(N),replace=False,size=N)
	for index in _indices_:
		if not processed[index]:
			n_order = expand_cluster_order(X,index,minpts,rad,cd,rd,order,n_order,processed,kdtree,minpts_neighbor_indices,k_distance,return_of)

	if return_of:
		of = get_of(X,minpts,metric,minpts_neighbor_indices,k_distance)

	return (order,cd,rd,of) if return_of else (order,cd,rd)

#implement Algorithm ExtractDBSCAN-Clustering in <<OPTICS: Ordering Points To Identify the Clustering Structure>> Figure 8
def extract_clusters(minpts,rad,order,cd,rd):
	cluster_ids = np.empty(len(order),np.int16)
	current_cluster_id = -1
	for object_index in order:
		if rd[object_index] > rad:
			if less_equal(cd[object_index],rad):
				current_cluster_id += 1
				cluster_ids[object_index] = current_cluster_id
			else:
				cluster_ids[object_index] = -1
		else:
			cluster_ids[object_index] = current_cluster_id
	return cluster_ids


from flask import Flask, render_template, json, request
from flaskext.mysql import MySQL
from mymoodanalyzer import setup, checkin
from requests_oauthlib import OAuth1

app = Flask(__name__)

labels = ['POS', 'NEG', 'OTHER']
colors = ["#32CD32", "#DC143C", "#D3D3D3"]

@app.route('/')
def main():
	return render_template('index.html')

@app.route('/result', methods=['POST'])
def handle_data():
	url = 'https://api.twitter.com/1.1/account/verify_credentials.json'
	auth = OAuth1(your_key, your_api_secret, your_access_token, your_access_token_secret)
	setup(url, auth)
	name = request.form['handle']
	output = checkin(name,auth)
	values = [output["pos_per"], output["neg_per"], 100-output["pos_per"]-output["neg_per"]]
	return render_template('result.html', output=output, set=zip(values, labels, colors))

if __name__ == "__main__":
    app.run()


'''
A network consists of nodes labeled 0 to N. You are given a list of edges (a, b, t),
describing the time t it takes for a message to be sent from node a to node b.
Whenever a node receives a message, it immediately passes the message on to a neighboring node, if possible.

Assuming all nodes are connected, determine how long it will take for every node to receive a message that begins at node 0.

For example, given N = 5, and the following edges:

edges = [
    (0, 1, 5),
    (0, 2, 3),
    (0, 5, 4),
    (1, 3, 8),
    (2, 3, 1),
    (3, 5, 10),
    (3, 4, 5)
]
You should return 9, because propagating the message from 0 -> 2 -> 3 -> 4 will take that much time.
'''

from collections import namedtuple
from typing import List

Edge = namedtuple('Edge', ['a', 'b', 't'])

def reachAllNodes(edges: List[Edge[int, int, int]]) -> int:
    def reachNode(edges: List[Edge], cur_node: int, cur_time: int, node_times: list):
        for e in edges:
            # Check path time for every b where the current node equals a on the edge.
            if e.a == cur_node:
                time_to_b = cur_time + e.t
                if time_to_b < node_times[e.b]:
                    # Quicker path has been found. Set the new time to the node index and recur from there.
                    node_times[e.b] = time_to_b
                    reachNode(edges, e.b, time_to_b, node_times)
        return

    all_nodes = set(map(lambda e: e.a, edges)).union(set(map(lambda e: e.b, edges)))
    # Optional assertions.
    if not all(map(lambda n: 0 <= n < len(all_nodes), all_nodes)):
        raise ValueError("Incomplete Network.")
    if not all(map(lambda e: e.t >= 0, edges)):
        raise ValueError("Node message time cannot be less than zero.")
    # Setup - All node times set to infinity except node 0 which equals 0.
    node_times = [0] + [float('inf')] * (len(all_nodes) - 1)
    # Recursion to get minimum node times starting from node 0.
    reachNode(edges, 0, 0, node_times)

    max_time = max(node_times)
    # Node has not been reached if its index in the list is still infinity.
    if max_time == float('inf'):
        raise ValueError("Unreachable Nodes.")
    return max_time

edges = [
    Edge(0, 1, 5),
    Edge(0, 2, 3),
    Edge(0, 5, 4),
    Edge(1, 3, 8),
    Edge(2, 3, 1),
    Edge(3, 5, 10),
    Edge(3, 4, 5)
]

print(reachAllNodes(edges))


"""
    Purpose: Use SIFT to extract features from a template image. This template image represents the object we are searching for. We can then
    use those features to check for the object in a larger, more uncontrolled scene.

    For further detail: David G. Lowe's paper on Distinctive Image Features from Scale-Invariant Keypoints

    Note: Query image --> template and training image --> search space
"""
#Libraries required (details added for Windows users)
import cv2 #pip install opencv-python
import numpy as np #pip install numpy
import matplotlib.pyplot as plt #pip install matplotlib
import os
import glob

#Detector basics
MIN_MATCH_COUNT=10 #Used to control how many matches are required before an object is "detected"
detector = cv2.SIFT_create() #SIFT object constructor

#Setting up Flann-based feature/description matcher to be used later (a bit faster than brute force for large datasets)
#to find k best matches for each descriptor from our query set
FLANN_INDEX_KDTREE=0
flannParam = dict(algorithm=FLANN_INDEX_KDTREE, tree=5) #builds dictionary (https://python-reference.readthedocs.io/en/latest/docs/functions/dict.html)
search_params = dict(checks=50)
flann = cv2.FlannBasedMatcher(indexParams=flannParam, searchParams=search_params) #will train flann on descriptor collection (of dicts)

templateImg = cv2.imread(filename="images/template_images/stop_sign_1.png", flags=0) #read the image that is used to train the classifier (aka a template to match on other images)
#Detection section (template image aka query image)
templateKP, templateDesc = detector.detectAndCompute(image=templateImg, mask=None) #used to detect keypoints (templateKP) and get the descriptors (templateDesc) in a single step
templateWithKP = cv2.drawKeypoints(image=templateImg, keypoints=templateKP, outImage=None, color=(255, 0, 0), flags=4) #draws keypoints previously computed
plt.imshow(X=templateWithKP) #X is an array-like or PIL image
plt.show() #shows plot with keypoints (for template image) only ONCE

#Image(s) that need to be searched
folder = "images/test_images" #folder with images that need checking

#iterate through test images and check all of them for stop signs
for filename in os.listdir(folder):
    unknownImg = cv2.imread(os.path.join(folder, filename)) #gets an image in the folder (not necessarily sorted)

    #error checking (wrong directory could cause problems)
    if unknownImg is not None:
        unknownImg = cv2.resize(src=unknownImg, dsize=(700, 330)) #resize the image (helps quite a bit with consistency)

        #Use this to see the directory image every time (unedited):
        # cv2.imshow(winname="Directory image", mat=unknownImg)
        # cv2.waitKey(delay=0)

        #Detection section (unknown image(s) aka training image(s))

        #used to detect keypoints (unknownKP) and get the descriptors (unknownDesc) in a single step
        unknownKP, unknownDesc = detector.detectAndCompute(image=unknownImg, mask=None)
        matches = flann.knnMatch(queryDescriptors=templateDesc, trainDescriptors=unknownDesc, k=2) #finds best 2 matches for each unknownDesc

        #Actual object/feature matching section
        reasonableMatch=[] #array that will be used to hold the best matches
        for m, n in matches:
            #if the Euclidean distance is reasonably close (aka L2 metric or L2 norm) for the template image and the image we are checking
            if(m.distance < (0.75*n.distance)):
                reasonableMatch.append(m) #add it

        #If there are more reasonable matches than the minimum that was previously set (aka a stop sign has been found)
        if(len(reasonableMatch) > MIN_MATCH_COUNT):
            temppt=[]
            unkpt=[]

            for m in reasonableMatch:
                temppt.append(templateKP[m.queryIdx].pt) #template points (source points)
                unkpt.append(unknownKP[m.trainIdx].pt) #points from image we are checking (destination points)
            temppt, unkpt = np.float32((temppt, unkpt)) #convert all to float32 data type

            #Must be 3D and don't know first dimension (not consistent)
            temppt = temppt.reshape(-1, 1, 2)
            unkpt = unkpt.reshape(-1, 1, 2)

            #Object/feature detection visualization calculations (using above calculations)
            #Finds a perspective transformation between two planes (aka maps the points in one point to the corresponding point in another image)
            M, mask = cv2.findHomography(srcPoints=temppt, dstPoints=unkpt, method=cv2.RANSAC, ransacReprojThreshold=3.0)
            matchesMask = mask.ravel().tolist()

            h, w = templateImg.shape #returns the dimensions of the template image (height = h, width = w)
            points = np.float32([[[0, 0], [0, h-1], [w-1, h-1], [w-1, 0]]]).reshape(-1, 1, 2) #convert all to float32 data type
            destination = cv2.perspectiveTransform(src=points, m=M) #used to align image detection

            #Bounding box options (all can coincide to compare, but ideally just choose one)
            print("Stop sign has been found in " + filename)

            #Rotated rectangle option
            # rot_rect = cv2.minAreaRect(destination)
            # box = cv2.boxPoints(rot_rect)
            # box = np.int0(box)
            # cv2.drawContours(unknownImg, [box], 0, (0,0,255), 2)
            # cv2.imshow(winname="Stop sign detection", mat=unknownImg)
            # cv2.waitKey(delay=0)

            #Straight rectangle option
            x,y,w,h = cv2.boundingRect(array=destination)
            cv2.rectangle(img=unknownImg, pt1=(x,y), pt2=(x+w, y+h), color=(0,255,0), thickness=2)
            cv2.imshow(winname="Stop sign found", mat=unknownImg)
            cv2.waitKey(delay=0)

            #Matched image visualization
            draw_params = dict(matchColor=(0,255,0), singlePointColor=None, matchesMask=matchesMask, flags=2) #specifying custom parameters (flag = 2 means only matched keypoints shown)
            matchedImg = cv2.drawMatches(templateImg, templateKP, unknownImg, unknownKP, reasonableMatch, None, **draw_params) #draw keypoints and connectors
            plt.imshow(X=matchedImg, cmap='gray')
            plt.show()

        #if not enough matches (aka a stop sign has not been found)
        else:
            print("No stop signs have been found in " + filename)
            matchesMask=None
            cv2.imshow(winname="No match found", mat=unknownImg)
            cv2.waitKey(delay=0)

cv2.destroyAllWindows() #will close any windows left open if the program is forcibly stopped


import cv2

def ask_for_tracker():
	print('TRACKING APIs')
	print('\n 0. Boosting')
	print('\n 1. MIL')
	print('\n 2. KCF')
	print('\n 3. TLD')
	print('\n 4. MEDIAFLOW')
	choice = input('Select Tracker')

	if choice == '0':
		tracker = cv2.TrackerBoosting_create()

	if choice == '1':
		tracker = cv2.TrackerMIL_create()

	if choice == '2':
		tracker = cv2.TrackerKCF_create()

	if choice == '3':
		tracker = cv2.TrackerTLD_create()

	if choice == '4':
		tracker = cv2.TrackerMedianFlow_create()

	return tracker

tracker = ask_for_tracker()
tracker_name = str(tracker).split()[0][1:]

cap = cv2.VideoCapture(0)
ret, frame = cap.read()
roi = cv2.selectROI(frame, False)
ret = tracker.init(frame, roi)

while True:
	ret, frame = cap.read()
	success, roi = tracker.update(frame)
	(x, y, w, h) = tuple(map(int, roi))

	if success:
		p1 = (x, y)
		p2 = (x+w, y+h)
		cv2.rectangle(frame, p1, p2, (0, 255, 0), 3)

	else:
		cv2.putText(frame, 'Failure to Detect Tracking!!', (100, 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 3)

	cv2.putText(frame, tracker_name, (20, 400), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 3)
	cv2.imshow(tracker_name, frame)

	k = cv2.waitKey(1) & 0xFF
	if k == 27:
		break

cap.release()
cv2.destroyAllWindows()


#encoding: UTF-8
#!/usr/bin/env python2
import cv2 as cv
import os
import numpy as np
import time
from pymycobot.mycobot import MyCobot
from follow_obj import followObj
from opencv_yolo import yolo
from VideoCapture import FastVideoCapture
from GrabParams2 import grabParams
import math
import rospy
from geometry_msgs.msg import Twist
import basic
import argparse
import rotate

#0：'apple', 1：'clock', 2：'banana'
top_pick_class = 0

choose_all_class = False

obj_size_filter = 80

follow_forward_target_obj_size = 108

coords_forward_ready = [80.6, -61.0, 300.9, 86.7, 42.72, 90.5]

#仓库上层货架抓取，机械臂的控制位置，可以通过注释其他代码进行调试
coords_top_ready_myself = [61.5, 80.8, 280.0, 86.53, 40.7, -178.72]
coords_top_grap_myself = [56.5, 168.0, 320.0, 85.0, 40.7, -180.0] #[56.5, 170.0, 315.0, 86.53, 40.7, -178.72]
coords_top_grap_ok_myself = [61.5, 60.8, 330.8, 86.53, 45, -178.72]

#仓库下层货架抓取，机械臂的控制位置，可以通过注释其他代码进行调试
coords_bottom_ready_myself = [63.5, 80.8, 212.0, 89.53, 40.7, -178.72] #61.5, 80.8, 290.0, 86.53, 40.7, -178.72
coords_bottom_grap_myself = [56.5, 168.0, 220.0, 84.53, 40.7, -180.0] #56.5, 170.0, 310.0, 86.53, 40.7, -178.72
coords_bottom_grap_ok_myself = [61.5, 60.8, 260.8, 86.53, 45, -178.72]

#xsize,ysize >=128
#上层货架抓取，机械臂的控制位置，可以通过注释其他代码进行调试
coords_top_ready = [61.5, 80.8, 295.0, 86.53, 40.7, -178.72]
coords_top_grap = [56.5, 151.0, 346.0, 83.53, 40.7, -180.0] #[56.5, 170.0, 315.0, 86.53, 40.7, -178.72]
coords_top_grap_ok = [61.5, 60.8, 350.8, 86.53, 45, -178.72]

#下层货架抓取，机械臂的控制位置，可以通过注释其他代码进行调试
coords_bottom_ready = [61.5, 80.8, 220.0, 86.53, 40.7, -178.72] #61.5, 80.8, 290.0, 86.53, 40.7, -178.72
coords_bottom_grap = [56.5, 151.0, 246.0, 83.53, 40.7, -180.0] #56.5, 170.0, 310.0, 86.53, 40.7, -178.72
coords_bottom_grap_ok = [61.5, 60.8, 260.8, 86.53, 45, -178.72]
arm_speed = 35

find_obj_count_top_max = 3
find_obj_count_bottom_max = 5
find_obj_distance_max_top = 0.45
find_obj_distance_max_bottom = 0.50

y_bias = grabParams.y_bias1
x_bias = grabParams.x_bias1
height_bias = grabParams.height_bias1
coords = grabParams.coords_ready1

class gsdemo(object):
    #初始化函数
    def __init__(self):
        super(gsdemo, self).__init__()

        self.initialize_robotarm()

        self.initialize_rospy()

        self.initialize_vision()

        self.initialize_followObj()

        self.initialize_find_obj_info()

        # #加载模型
        # self.net = cv.dnn.readNetFromONNX("comp.onnx")

        self.color = 0
        # parameters to calculate camera clipping parameters
        self.x1 = self.x2 = self.y1 = self.y2 = 0
         # set color HSV
        self.HSV = {
            "yellow": [np.array([20, 43, 46]), np.array([26, 255, 255])],
            "red": [np.array([0, 43, 46]), np.array([10, 255, 255])],
            "green": [np.array([50, 43, 46]), np.array([65, 255, 255])],
            "blue": [np.array([100, 43, 46]), np.array([124, 255, 255])],
            "purple": [np.array([125, 43, 46]), np.array([155, 255, 255])],
        }
        # use to calculate coord between cube and mycobot
        self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0
        self.ratio1 = grabParams.ratio1

    def initialize_robotarm(self):
        self.mc = MyCobot(grabParams.usb_dev, grabParams.baudrate)
        self.mc.power_on()

    def initialize_rospy(self):
        rospy.init_node('gsdemo', anonymous=True)
        self.pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.rate = rospy.Rate(20) # 20hz

    def initialize_vision(self):
        self.cap = FastVideoCapture(grabParams.cap_num)
        self.c_x, self.c_y = grabParams.IMG_SIZE/2, grabParams.IMG_SIZE/2

        self.yolo = yolo()

        height = grabParams.IMG_SIZE
        focal_length = width = grabParams.IMG_SIZE
        self.center = [width / 2, height / 2]

    def initialize_followObj(self):
        self.followObj = followObj(self.pub, self.rate)

    def initialize_find_obj_info(self):
        self.find_obj_count_top = 0
        self.find_obj_count_bottom = 0
        self.find_obj_distance_top = 0
        self.find_obj_distance_bottom = 0

    def increase_find_obj_count_top(self):
        self.find_obj_count_top += 1

    def increase_find_obj_count_bottom(self):
        self.find_obj_count_bottom += 1

    def increase_find_obj_distance_top(self):
        self.find_obj_distance_top += 0.0501

    def increase_find_obj_distance_bottom(self):
        self.find_obj_distance_bottom += 0.0501

    def is_find_obj_finish_top(self):
        is_finish = False
        if self.find_obj_distance_top > find_obj_distance_max_top:
            is_finish = True
        return is_finish

    def is_find_obj_finish_bottom(self):
        is_finish = False
        if self.find_obj_distance_bottom > find_obj_distance_max_bottom:
            is_finish = True
        return is_finish

    #计算物体的坐标和尺寸
    def get_position_size(self, box):
        left, top, right, bottom = box
        # print("box",box)
        x = left + right
        y = bottom + top
        x = x*0.5
        y = y*0.5
        x_size_p = abs(left - right)
        y_size_p = abs(top - bottom)
        return (-(x - self.c_x)), (-(y - self.c_y)), x_size_p, y_size_p

    #视频显示
    def show_image(self, img):
        cv.imshow("figure", img)
        cv.waitKey(50)

    #物体识别
    def obj_detect(self, img):
        self.is_find = False
        img_ori = img
        img_ori = self.transform_frame(img)
        img = self.transform_frame_128(img)

        #加载模型
        net = cv.dnn.readNetFromONNX("comp.onnx")

        t1 = time.time()
        #输入数据处理
        blob = cv.dnn.blobFromImage(img, 1 / 255.0, (128, 128), [0, 0, 0], swapRB=True, crop=False)
        net.setInput(blob)

        #推理
        outputs = net.forward(net.getUnconnectedOutLayersNames())[0]

        #获得识别结果
        boxes, classes, scores = self.yolo.yolov5_post_process_simple(outputs)
        t2 = time.time()

        best_result = (0,0,0)

        #物体边框处理
        if boxes is not None:
            boxes = boxes*5
            for box, score, cl in zip(boxes, scores, classes):
                self.image_info = self.get_position_size(box)
                wph = self.image_info[2]/self.image_info[3]
                obj_size = (self.image_info[2]+self.image_info[3])*0.5
                # print(cl, wph, self.image_info)

                if (wph > 0.8 and wph <1.2) and (obj_size >= obj_size_filter):
                    if (top_pick_class == cl) or (choose_all_class):
                        temp_result = (box, score, cl)
                        best_score = best_result[1]
                        # print("best_score: ", best_score)
                        if best_score < 0.01:
                            best_result = temp_result
                            self.is_find = True
                        best_box =  best_result[0]
                        # print("best_box:", best_box, "box: ", box)
                        temp_box_left, temp_box_top, temp_box_right, temp_box_bottom = box
                        best_box_left, best_box_top, best_box_right, best_box_bottom = best_box
                        if temp_box_left < best_box_left:
                            best_result = temp_result
                            self.is_find = True

        #画物体边框
        if self.is_find:
            box, score, cl = best_result
            self.yolo.draw_single(img_ori, box, score, cl)
            self.target_image_info = self.get_position_size(box)
            print(self.target_image_info)

        #视频显示
        self.show_image(img_ori)

    #图像处理，适配物体识别
    def transform_frame(self, frame):
        frame, ratio, (dw, dh) = self.yolo.letterbox(frame, (grabParams.IMG_SIZE, grabParams.IMG_SIZE))

        return frame

    #图像处理，适配物体识别
    def transform_frame_128(self, frame):
        frame, ratio, (dw, dh) = self.yolo.letterbox(frame, (128, 128))

        return frame

    ###颜色识别
    def move_arm(self, x, y):
        #done = False

        coords_target = [coords[0]+int(x), coords[1]+int(y), height_bias, coords[3], coords[4], coords[5]]
        basic.move_to_target_coords(coords_target, grabParams.GRAB_MOVE_SPEED)

        basic.grap(True)

        angles = [0, 0, 0, 0, 0, 0]
        self.mc.send_angles(angles,30)
        time.sleep(1.5)
        #debug = False
        #done = True
        #print("Done")

    def pose_to_color(self):
        angles = [0, 0, 0, 0, 0, 0]
        self.mc.send_angles(angles,30)
        basic.grap(False)
        time.sleep(1)
        basic.move_to_target_coords(grabParams.coords_ready1,grabParams.GRAB_MOVE_SPEED)

    def get_position_color(self, x, y):
        wx = wy = 0
        if grabParams.grab_direct == "front":
            wx = (self.c_y - y) * self.ratio1
            wy = (self.c_x - x) * self.ratio1
        elif grabParams.grab_direct == "right":
            wx = (self.c_x - x) * self.ratio1
            wy = (y - self.c_y) * self.ratio1
        return wx, wy

    def color_detect(self, img):
        # set the arrangement of color'HSV
        x = y = 0
        for mycolor, item in self.HSV.items():
            redLower = np.array(item[0])
            redUpper = np.array(item[1])
            # transfrom the img to model of gray
            hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)
            # wipe off all color expect color in range
            mask = cv.inRange(hsv, item[0], item[1])
            # a etching operation on a picture to remove edge roughness
            erosion = cv.erode(mask, np.ones((1, 1), np.uint8), iterations=2)
            # the image for expansion operation, its role is to deepen the color depth in the picture
            dilation = cv.dilate(erosion, np.ones(
                (1, 1), np.uint8), iterations=2)
            # adds pixels to the image
            target = cv.bitwise_and(img, img, mask=dilation)
            # the filtered image is transformed into a binary image and placed in binary
            ret, binary = cv.threshold(dilation, 127, 255, cv.THRESH_BINARY)
            # get the contour coordinates of the image, where contours is the coordinate value, here only the contour is detected
            contours, hierarchy = cv.findContours(
                dilation, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)

            if len(contours) > 0:
                # do something about misidentification
                boxes = [
                    box
                    for box in [cv.boundingRect(c) for c in contours]
                    if 110 < min(box[2], box[3]) and  max(box[2], box[3]) < 170
                    # if min(img.shape[0], img.shape[1]) / 10
                    # < min(box[2], box[3])
                    # < min(img.shape[0], img.shape[1]) / 1
                ]
                print(boxes)
                if boxes:
                    for box in boxes:
                        # print(box)
                        x, y, w, h = box
                        # if abs(w-h)>15:
                        #     return None
                    # find the largest object that fits the requirements
                    c = max(contours, key=cv.contourArea)
                    # get the lower left and upper right points of the positioning object
                    x, y, w, h = cv.boundingRect(c)
                    print(x, y, w, h)
                    # locate the target by drawing rectangle
                    cv.rectangle(img, (x, y), (x+w, y+h), (153, 153, 0), 2)
                    # calculate the rectangle center
                    x, y = (x*2+w)/2, (y*2+h)/2
                    # calculate the real coordinates of mycobot relative to the target
                    if mycolor == "yellow":
                        self.color = 1
                    elif mycolor == "red":
                        self.color = 0
                    else:
                        self.color = 1

        if abs(x) + abs(y) > 0:
            return x, y
        else:
            return None

    def pick_and_place_color(self):
        self.pose_to_color()
        done = False
        while not done:
            img = self.cap.read()
            img = self.transform_frame(img)
            detect_result = self.color_detect(img)
            self.show_image(img)
            if detect_result is None:
                continue
            else:
                x, y = detect_result
                real_x, real_y = self.get_position_color(x, y)
                coords_now = basic.get_coords()
                if len(coords_now) == 6:
                    coords = coords_now
                self.move_arm(real_x + x_bias, real_y + y_bias)
                self.place2right()
                time.sleep(2)
                angles2 = [-83.23, -140.53, 140.97, 58.71, -127.61, 5.71]
                self.mc.send_angles(angles2,25)
                time.sleep(2.5)
                done = True
                print("Done")

    ####

    #小车后退
    def moveback(self):
        print("backward 0.05m...")
        count = 40
        move_cmd = Twist()
        while count > 0:
            move_cmd.linear.x = -0.025
            self.pub.publish(move_cmd)
            self.rate.sleep()
            count -= 1

    def moveforward(self):
        print("forward 0.05m...")
        count = 40
        move_cmd = Twist()
        while count > 0:
            move_cmd.linear.x = 0.025
            self.pub.publish(move_cmd)
            self.rate.sleep()
            count -= 1

    def move30cm(self,n,speed):
        print("move 0.3m...")
        count = n
        move_cmd = Twist()
        while count > 0:
            move_cmd.linear.x = speed
            self.pub.publish(move_cmd)
            self.rate.sleep()
            count -= 1

    #小车右转
    def rotate_to_right(self):
        print("rotate_to_right...")
        count = 20
        move_cmd = Twist()
        while count > 0:
            move_cmd.angular.z = -0.2
            self.pub.publish(move_cmd)
            self.rate.sleep()
            count -= 1

    #小车左转
    def rotate_to_left(self):
        print("rotate_to_left...")
        count = 20
        move_cmd = Twist()
        while count > 0:
            move_cmd.angular.z = 0.2
            self.pub.publish(move_cmd)
            self.rate.sleep()
            count -= 1

    #夹爪闭合
    def pick(self):
        basic.grap(True)

    #旋转图像
    def rotate_image(self, image, rotate_angle):
        rotated_image = self.yolo.rotate_image(image,rotate_angle)
        return rotated_image

    #跟随物体
    def follow_obj(self):
        is_follow_obj_done = False

        while not is_follow_obj_done:
            k=cv.waitKey(1)
            if k==ord('q'):
                print('done')
                break
            img = self.cap.read()
            img = self.rotate_image(img, -90)
            self.obj_detect(img)
            if self.is_find:
                # print("send_cmd_vel")
                is_follow_obj_done = self.followObj.follow(self.target_image_info, self.cap.getWidth())
                print(is_follow_obj_done)
            else:
                move_cmd = Twist()
                self.pub.publish(Twist())

    #跟随物体
    def follow_obj_forward(self):
        is_follow_obj_done_forward = False

        while not is_follow_obj_done_forward:
            k=cv.waitKey(1)
            if k==ord('q'):
                print('done')
                break
            img = self.cap.read()
            img = self.rotate_image(img, -90)
            self.obj_detect(img)
            if self.is_find:
                is_follow_obj_done_forward = self.followObj.follow_forward(self.target_image_info, self.cap.getWidth(), follow_forward_target_obj_size)
            else:
                move_cmd = Twist()
                self.pub.publish(Twist())

    #寻找指定的物体
    def find_target_image(self):
        while not self.is_find:
            img = self.cap.read()
            img = self.rotate_image(img, -90)
            self.obj_detect(img)
            k=cv.waitKey(1)
            if k==ord('q'):
                print('done')
                break

    #机械臂放置积木到右侧
    def place2right(self):
        coords_place_right = [-150.0, 18.2, 182.6, -177.46, 7.03, 20.66]
        self.mc.send_coords(coords_place_right,arm_speed,0)
        time.sleep(4)  ##4

        # open
        basic.grap(False)

        angles = [0, 0, 0, 0, 0, 0]
        self.mc.send_angles(angles,arm_speed)

    def ready_arm_pose_forward(self):
        global choose_all_class, obj_size_filter
        choose_all_class = False
        obj_size_filter = 30
        self.mc.send_coords(coords_forward_ready,arm_speed,0)
        basic.grap(False)
        time.sleep(0.5)

    def ready_arm_pose(self,choose,coords):
        global choose_all_class, obj_size_filter
        choose_all_class = choose
        obj_size_filter = 80
        self.mc.send_coords(coords,arm_speed,0)
        basic.grap(False)
        time.sleep(0.5)

    # #机械臂到达准备抓取的位置
    # def ready_arm_pose_top(self):
    #     global choose_all_class, obj_size_filter
    #     choose_all_class = False
    #     obj_size_filter = 80
    #     self.mc.send_coords(coords_top_ready,arm_speed,0)
    #     basic.grap(False)
    #     time.sleep(3)

    # #机械臂到达准备抓取的位置
    # def ready_arm_pose_bottom(self):
    #     global choose_all_class, obj_size_filter
    #     choose_all_class = True
    #     obj_size_filter = 80
    #     self.mc.send_coords(coords_bottom_ready,arm_speed,0)
    #     basic.grap(False)
    #     time.sleep(3)

    #机械臂进行抓取
    def pick_(self,coords1,coords2):

        self.mc.send_coords(coords1,arm_speed,0)
        time.sleep(2)  ##old: 3
        basic.grap(True)#闭合夹爪

        self.mc.send_coords(coords2,arm_speed,0)
        time.sleep(2)

    # #机械臂进行抓取
    # def pick_top(self):

    #     self.mc.send_coords(coords_top_grap,arm_speed,0)
    #     time.sleep(2)  ##lsw old: 3
    #     basic.grap(True)#闭合夹爪

    #     self.mc.send_coords(coords_top_grap_ok,arm_speed,0)
    #     time.sleep(2)

    # #机械臂进行抓取
    # def pick_bottom(self):

    #     self.mc.send_coords(coords_bottom_grap,arm_speed,0)
    #     time.sleep(2)  ##lsw old: 3
    #     basic.grap(True)#闭合夹爪

    #     self.mc.send_coords(coords_bottom_grap_ok,arm_speed,0)
    #     time.sleep(2)

    def follow_obj_and_pick_and_place(self,coords1,coords2):
        # #跟随物体
        self.follow_obj()

        # #机械臂进行抓取
        self.pick_(coords1,coords2)

        # #机械臂放置积木到右侧
        self.place2right()

    # def follow_obj_and_pick_and_place_bottom_myself(self):
    #     # #跟随物体
    #     self.follow_obj()

    #     # #机械臂进行抓取
    #     self.pick_(coords_bottom_grap_myself,coords_bottom_grap_ok_myself)

    #     # #机械臂放置积木到右侧
    #     self.place2right()
    #     # #中间防碰
    #     time.sleep(2)

    # def follow_obj_and_pick_and_place_top(self):
    #     # #跟随物体
    #     self.follow_obj()

    #     # #机械臂进行抓取
    #     self.pick_(coords_top_grap,coords_top_grap_ok)

    #     # #机械臂放置积木到右侧
    #     self.place2right()

    # def follow_obj_and_pick_and_place_bottom(self):
    #     # #跟随物体
    #     self.follow_obj()

    #     # #机械臂进行抓取
    #     self.pick_(coords_bottom_grap,coords_bottom_grap_ok)

    #     # #机械臂放置积木到右侧
    #     self.place2right()
    #     # #中间防碰
    #     time.sleep(1.5)
    #     #bottom_ready_middle = [-101.8, -27.1, 223.4, 83.72, 43.39, -40.52]
    #     #self.mc.send_coords(bottom_ready_middle,arm_speed,0)

    def find_next_obj(self):
        img = self.cap.read()
        img = self.rotate_image(img, -90)
        self.obj_detect(img)
        if self.is_find:
            self.find_next_obj_count += 1
        else:
            self.find_next_obj_count = 0

    def pick_and_place_objs_top_myself(self):
        self.ready_arm_pose(False,coords_top_ready_myself)
        self.find_next_obj_count = 0
        count = 0
        while not self.is_find_obj_finish_top():
            self.find_next_obj()
            count += 1
            if self.find_next_obj_count >= 5:
                # self.rotate_to_direct2()
                self.increase_find_obj_count_top()
                print("find_obj_count_top: ", self.find_obj_count_top)
                time.sleep(1)
                self.follow_obj_and_pick_and_place(coords_top_grap_myself,coords_top_grap_ok_myself)
                self.ready_arm_pose(False,coords_top_ready_myself)
                time.sleep(2)
                count = 6
            if count >= 6:
                self.moveforward()
                # time.sleep(1)
                count = 0
                self.find_next_obj_count = 0
                self.increase_find_obj_distance_top()
                print("find_obj_distance_top: ", self.find_obj_distance_top)

    def pick_and_place_objs_bottom_myslf(self):
        self.ready_arm_pose(True,coords_bottom_ready_myself)
        self.find_next_obj_count = 0
        count = 0
        while not self.is_find_obj_finish_bottom():
            self.find_next_obj()
            count += 1
            if self.find_next_obj_count >= 5:
                # self.rotate_to_direct2()
                self.increase_find_obj_count_bottom()
                print("find_obj_count_bottom: ", self.find_obj_count_bottom)
                time.sleep(1)
                self.follow_obj_and_pick_and_place(coords_bottom_grap_myself,coords_bottom_grap_ok_myself)
                time.sleep(2)
                self.ready_arm_pose(True,coords_bottom_ready_myself)
                time.sleep(2)
                count = 6
            if count >= 6:
                self.moveback()
                # time.sleep(1)
                count = 0
                self.find_next_obj_count = 0
                self.increase_find_obj_distance_bottom()
                print("find_obj_distance_bottom: ", self.find_obj_distance_bottom)

    def pick_and_place_objs_top(self):
        self.ready_arm_pose(False,coords_top_ready)
        self.find_next_obj_count = 0
        count = 0
        while not self.is_find_obj_finish_top():
            self.find_next_obj()
            count += 1
            if self.find_next_obj_count >= 5:
                # self.rotate_to_direct2()
                self.increase_find_obj_count_top()
                print("find_obj_count_top: ", self.find_obj_count_top)
                time.sleep(1)
                self.follow_obj_and_pick_and_place(coords_top_grap,coords_top_grap_ok)
                self.ready_arm_pose(False,coords_top_ready)
                time.sleep(2)
                count = 6
            if count >= 6:
                self.moveforward()
                # time.sleep(1)
                count = 0
                self.find_next_obj_count = 0
                self.increase_find_obj_distance_top()
                print("find_obj_distance_top: ", self.find_obj_distance_top)

    def pick_and_place_objs_bottom(self):
        self.ready_arm_pose(True,coords_bottom_ready)
        self.find_next_obj_count = 0
        count = 0
        while not self.is_find_obj_finish_bottom():
            self.find_next_obj()
            count += 1
            if self.find_next_obj_count >= 5:
                # self.rotate_to_direct2()
                self.increase_find_obj_count_bottom()
                print("find_obj_count_bottom: ", self.find_obj_count_bottom)
                time.sleep(1)
                self.follow_obj_and_pick_and_place(coords_bottom_grap,coords_bottom_grap_ok)
                time.sleep(1.5)
                self.ready_arm_pose(True,coords_bottom_ready)
                time.sleep(2)
                count = 6
            if count >= 6:
                self.moveback()
                # time.sleep(1)
                count = 0
                self.find_next_obj_count = 0
                self.increase_find_obj_distance_bottom()
                print("find_obj_distance_bottom: ", self.find_obj_distance_bottom)

    def arm_back(self):
        angles1 = [0, 0, 0, 0, 0, 0]
        self.mc.send_angles(angles1,30)
        time.sleep(2)

        angles2 = [-83.23, -140.53, 140.97, 58.71, -127.61, 5.71]
        self.mc.send_angles(angles2,25)
        time.sleep(3)

        self.mc.set_color(0,255,0)#green, arm is free

    def rotate_to_direct(self,filename):
        rotate.target_yaw = rotate.read_direct(filename)
        rotate.done = False
        rotate.rotate_to()

    # def rotate_to_direct2(self):
    #     rotate.target_yaw = rotate.read_direct('direct_2.txt')
    #     rotate.done = False
    #     rotate.rotate_to()
    # def rotate_to_direct1(self):
    #     os.system("python rotate_to_direct1.pyc")

    # def rotate_to_direct2(self):
    #     os.system("python rotate_to_direct2.pyc")

    def goto_shelf_common(self):
        os.system("python goto_shelf_common.pyc")

    def goto_shelf_myself(self):
        os.system("python goto_shelf_myself.pyc")

    def goto_start(self):
        os.system("python goto_start_area.pyc")

    def goto_desk(self):
        os.system("python follow_aruco_2.py")

    # def color_grab(self):
    #     os.system("python color_grab_2.py")

    def goto_table(self):
        os.system("python goto_table.py")

    def goto_objs(self):

        self.ready_arm_pose_forward()

        self.rotate_to_direct('direct_1.txt')

        self.follow_obj_forward()

        self.follow_obj_forward()

        self.follow_obj_forward()

        self.rotate_to_direct('direct_2.txt')

    def pick_common(self):
        self.goto_shelf_common()

        self.mc.set_color(0,0,255)#blue, arm is busy

        self.goto_objs()

        self.pick_and_place_objs_top()

        self.pick_and_place_objs_bottom()  #bottom,4.3

        self.move30cm(50,0.1)

        self.arm_back()

    def pick_color(self):
        self.goto_table()
        self.goto_desk()
        self.pick_and_place_color()
        self.move30cm(55,-0.1)

    def pick_myself(self):
        self.goto_shelf_myself()

        self.mc.set_color(0,0,255)#blue, arm is busy

        self.goto_objs()

        self.initialize_find_obj_info()

        self.pick_and_place_objs_top_myself()

        self.pick_and_place_objs_bottom_myslf()  #bottom,4.3

        self.move30cm(50,0.1)

        self.arm_back()

    #主函数
    def run(self):
        self.pick_common()
        self.pick_color()
        self.pick_myself()
        self.goto_start()

if __name__ == "__main__":
    detect = gsdemo()
    detect.run()


import os
import shutil
import sys
import time

import cv2
import numpy as np

def get_images(input_path):
    files = []
    exts = ['jpg', 'png', 'jpeg', 'JPG']

    for parent, dirnames, filenames in os.walk(input_path):

        for filename in filenames:
            for ext in exts:
                if filename.endswith(ext):
                    files.append(os.path.join(parent, filename))
                    break
    print('Find {} images'.format(len(files)))
    return files

def resize_image(img):
    img_size = img.shape
    img_size_min = np.min(img_size[0:2])
    img_size_max = np.max(img_size[0:2])

    img_scale = float(2400) / float(img_size_min)
    print("scale factor = "+str(img_scale))
    if np.round(img_scale * img_size_max) > 3600:
        img_scale = float(2400) / float(img_size_max)
    new_h = int(img_size[0] * img_scale)
    new_w = int(img_size[1] * img_scale)

    new_h = new_h if new_h // 16 == 0 else (new_h // 16 + 1) * 16
    new_w = new_w if new_w // 16 == 0 else (new_w // 16 + 1) * 16
    re_im = cv2.resize(img, (new_w, new_h), interpolation=cv2.cv2.INTER_CUBIC)
    return re_im, (new_h / img_size[0], new_w / img_size[1])

def remove_border(input_path, output_path):
    """remove P&ID drawing broder
    """
    if os.path.exists(output_path):
        shutil.rmtree(output_path)
    os.makedirs(output_path)
    img_fn_list = get_images(input_path)
    epsilon = 0.0001
    for img_fn in img_fn_list:
        print('===============')
        print(img_fn)
        start = time.time()
        try:
            img_gray = cv2.imread(img_fn,cv2.IMREAD_GRAYSCALE)
        except:
            print("Error reading image {}!".format(img_fn))
            continue
        h, w = img_gray.shape[:2]
        img_blank =  np.ones(shape=[h, w], dtype=np.uint8)*255
        img_binary = cv2.threshold(img_gray, 128, 255, cv2.THRESH_BINARY)[1]
        _, contours, _ = cv2.findContours(img_binary,cv2.RETR_TREE,cv2.CHAIN_APPROX_NONE)
        area = []
        for cnt in contours:
           approx = cv2.approxPolyDP(cnt,epsilon*cv2.arcLength(cnt,True),True)
           area .append(cv2.contourArea(cnt))
        # sort by contour area
        top_cnt_area = np.argsort(-1*np.array(area))
        # drawing has not been pre-processed
        # select the thrid largest contour which fit the drawing broader
        ind = top_cnt_area[2]
        approx = cv2.approxPolyDP(contours[ind],epsilon*cv2.arcLength(contours[ind],True),True)
        cv2.drawContours(img_blank, [approx], 0, (0), thickness = -1, lineType=8)
        # combine image with masks
        img_gray = cv2.bitwise_or(img_blank, img_gray)
        cv2.imwrite(os.path.join(output_path, os.path.basename(img_fn)), img_gray)

def thinning (input_path, output_path):
    """
     Morphological operation that is used to remove selected foreground pixels from binary images
    """
    if os.path.exists(output_path):
        shutil.rmtree(output_path)
    os.makedirs(output_path)
    img_fn_list = get_images(input_path)
    epsilon = 0.0001
    for img_fn in img_fn_list:
        print('===============')
        print(img_fn)
        start = time.time()
        try:
            img_gray = cv2.imread(img_fn,cv2.IMREAD_GRAYSCALE)
        except:
            print("Error reading image {}!".format(img_fn))
            continue
        # swap the color from black white to white black
        img= cv2.subtract(255, img_gray)

        img1 = img.copy()
        # Structuring Element
        kernel = cv2.getStructuringElement(cv2.MORPH_CROSS,(3,3))
        # Create an empty output image to hold values
        thin = np.zeros(img.shape,dtype='uint8')

        # Loop until erosion leads to an empty set
        while (cv2.countNonZero(img1)!=0):
            # Erosion
            erode = cv2.erode(img1,kernel)
            # Opening on eroded image
            opening = cv2.morphologyEx(erode,cv2.MORPH_OPEN,kernel)
            # Subtract these two
            subset = erode - opening
            # Union of all previous sets
            thin = cv2.bitwise_or(subset,thin)
            # Set the eroded image for next iteration
            img1 = erode.copy()
        cv2.imwrite(os.path.join(output_path, os.path.basename(img_fn)), thin)

def skeleton(input_path, output_path):
    """
     Skeletonization is a process for reducing foreground regions in a binary
     image to a skeletal remnant that largely preserves the extent and connectivity
     of the original region while throwing away most of the original foreground pixels.
    """
    if os.path.exists(output_path):
        shutil.rmtree(output_path)
    os.makedirs(output_path)
    img_fn_list = get_images(input_path)
    epsilon = 0.0001
    for img_fn in img_fn_list:
        print('===============')
        print(img_fn)
        start = time.time()
        try:
            img_gray = cv2.imread(img_fn,cv2.IMREAD_GRAYSCALE)
        except:
            print("Error reading image {}!".format(img_fn))
            continue
        # swap the color from black white to white black
        img= cv2.subtract(255, img_gray)

        # Step 1: Create an empty skeleton
        size = np.size(img)
        skel = np.zeros(img.shape, np.uint8)

        # Get a Cross Shaped Kernel
        element = cv2.getStructuringElement(cv2.MORPH_CROSS, (3,3))
        # Repeat steps 2-4
        while True:
            #Step 2: Open the image
            open = cv2.morphologyEx(img, cv2.MORPH_OPEN, element)
            #Step 3: Substract open from the original image
            temp = cv2.subtract(img, open)
            #Step 4: Erode the original image and refine the skeleton
            eroded = cv2.erode(img, element)
            skel = cv2.bitwise_or(skel,temp)
            img = eroded.copy()
            # Step 5: If there are no white pixels left ie.. the image has been completely eroded, quit the loop
            if cv2.countNonZero(img)==0:
                break
        cv2.imwrite(os.path.join(output_path, os.path.basename(img_fn)), skel)


"""Routes for httpstan.

Routes for the HTTP server are defined here.
"""
import aiohttp.web

import httpstan.views as views

def setup_routes(app: aiohttp.web.Application) -> None:
    """Add routes to Application.

    Arguments:
        app (aiohttp.Application): Application to which routes should be added.

    """
    # Note: changes here must be mirrored in `openapi.py`.
    app.router.add_get("/v1/health", views.handle_health)
    app.router.add_post("/v1/models", views.handle_create_model)
    app.router.add_get("/v1/models", views.handle_list_models)
    app.router.add_delete("/v1/models/{model_id}", views.handle_delete_model)
    app.router.add_post("/v1/models/{model_id}/params", views.handle_show_params)
    app.router.add_post("/v1/models/{model_id}/log_prob", views.handle_log_prob)
    app.router.add_post("/v1/models/{model_id}/log_prob_grad", views.handle_log_prob_grad)
    app.router.add_post("/v1/models/{model_id}/write_array", views.handle_write_array)
    app.router.add_post("/v1/models/{model_id}/transform_inits", views.handle_transform_inits)
    app.router.add_post("/v1/models/{model_id}/fits", views.handle_create_fit)
    app.router.add_get("/v1/models/{model_id}/fits/{fit_id}", views.handle_get_fit)
    app.router.add_delete("/v1/models/{model_id}/fits/{fit_id}", views.handle_delete_fit)
    app.router.add_get("/v1/operations/{operation_id}", views.handle_get_operation)


import sys
import re
import collections

# Can be on of Test, Normal or Verbose
mode = 'Normal'

orientation = ('N', 'S', 'E', 'W')
command = ('F', 'L', 'R')

turnLeftSwitcher = {
    'N': 'W',
    'S': 'E',
    'E': 'N',
    'W': 'S'
}

turnRightSwitcher = {
    'N': 'E',
    'S': 'W',
    'E': 'S',
    'W': 'N'
}

class Scent:
    'Definition of a scent'

    def __init__(self, xCoord, yCoord, orientation):
        self.xCoord = xCoord
        self.yCoord = yCoord
        self.orientation = orientation

    def compareScent(self, xCoord, yCoord, orientation):
        return (self.xCoord == xCoord) and (self.yCoord == yCoord) and (self.orientation == orientation)

class Map:
    'Definition of the map'

    __listScent = []

    def __init__(self, xSize, ySize):
        self.xSize = xSize
        self.ySize = ySize

    def addScent(self, Scent):
        self.__listScent.append(Scent)

    def checkScent(self, Scent):
        for i in range(len(self.__listScent)):
            if self.__listScent[i].compareScent(Scent.xCoord, Scent.yCoord, Scent.orientation):
                return True
        return False

    def printScent(self):
        for i in range(len(self.__listScent)):
            print(self.__listScent[i].xCoord, self.__listScent[i].yCoord)

class Robot:
    'Robot that will land in the planet landscape'

    isLost = False

    def __init__(self, origX, origY, orientation):
        self.xPos = origX
        self.yPos = origY
        self.orientation = orientation

    def move(self, command, Map):

        if self.isLost:
            return

        tempScent = Scent(self.xPos, self.yPos, self.orientation)

        if Map.checkScent(tempScent) and command == 'F':
            del tempScent
            return

        if command == 'F':  # Move Formard
            if self.orientation == 'N':
                self.yPos += 1
            elif self.orientation == 'S':
                self.yPos -= 1
            elif self.orientation == 'E':
                self.xPos += 1
            elif self.orientation == 'W':
                self.xPos -= 1
        elif command == 'L':  # Turn Left
            self.orientation = turnLeftSwitcher.get(self.orientation)
        elif command == 'R':  # Turn Right
            self.orientation = turnRightSwitcher.get(self.orientation)

        if self.xPos > Map.xSize or self.yPos > Map.ySize or self.xPos < 0 or self.yPos < 0:
            self.isLost = True
            self.xPos = tempScent.xCoord
            self.yPos = tempScent.yCoord
            Map.addScent(tempScent)

        return True

if mode == 'Verbose':
    inputString = raw_input("Please, input the planet size in this format: 23 32 (Max 50x50)\n")
    valid = bool(re.match("^(([0-9]|[0-4][0-9]|50)\s([0-9]|[0-4][0-9]|50))$", inputString))
    if valid != True:
        print("Incorrect input, the first line format is x y to define the size of the planet landscape. X and Y must to be between 1 and 50.")
        sys.exit(-1)

    listInput = inputString.split(' ')
    xSize = int(listInput[0])
    ySize = int(listInput[1])

    print("Planet size: {}x{}\n".format(xSize, ySize))

    listRobots = collections.OrderedDict()

    moreInput = True
    while moreInput:
        inputString = raw_input("Input the initial position of the robot in this format: 1 1 W\n")
        valid = bool(re.match("^(([0-9]|[0-4][0-9]|50)\s([0-9]|[0-4][0-9]|50)\s(W|E|N|S))$", inputString))
        if valid is not True:
            print("Incorrect input, the format must to be: X Y (one of W, N, S, E).")
            del inputString
            del valid
            del moreInput
            sys.exit(-1)
        listInput = inputString.split(' ')
        if int(listInput[0]) > xSize or int(listInput[1]) > ySize:
            print("Incorrect input, the coordinates for the robot need to be within the planet size.")
            del inputString
            del valid
            del moreInput
            del listInput
            sys.exit(-1)
        tempRobot = Robot(int(listInput[0]), int(listInput[1]), str(listInput[2]))
        inputString = raw_input("Input the list of commands for this robot. Commands valid are F, R or L. Max 100 commands.\n")
        valid = bool(re.match("^(R|F|L){1,100}$", inputString))
        if valid is not True:
            print("Incorrect input, wrong command found or too many commands.")
            del inputString
            del valid
            del moreInput
            del listInput
            del tempRobot
            sys.exit(-1)
        listRobots.update({tempRobot: inputString})
        inputString = raw_input("Do you want to add another robot? (y/n): \n")
        if inputString[0] == 'n' or inputString[0] == 'N':
            moreInput = False

    mars = Map(xSize, ySize)
    for key, value in listRobots.items():
        for i in value:
            key.move(i, mars)
        print("{} {} {} {}".format(key.xPos, key.yPos, key.orientation, 'LOST' if key.isLost else ''))
    del listRobots
    del inputString
    del valid
    del moreInput
    del listInput
    del tempRobot
    del mars
elif mode == 'Normal':
    inputString = raw_input()
    if not inputString:
        sys.exit(-1)
    valid = bool(re.match("^(([0-9]|[0-4][0-9]|50)\s([0-9]|[0-4][0-9]|50))$", inputString))
    if valid != True:
        sys.exit(-1)

    listInput = inputString.split(' ')
    xSize = int(listInput[0])
    ySize = int(listInput[1])

    listRobots = {}

    moreInput = True
    while moreInput:
        inputString = raw_input()
        if not inputString:
            break
        valid = bool(re.match("^(([0-9]|[0-4][0-9]|50)\s([0-9]|[0-4][0-9]|50)\s(W|E|N|S))$", inputString))
        if valid is not True:
            del inputString
            del valid
            del moreInput
            sys.exit(-1)
        listInput = inputString.split(' ')
        if int(listInput[0]) > xSize or int(listInput[1]) > ySize:
            del inputString
            del valid
            del moreInput
            del listInput
            sys.exit(-1)
        tempRobot = Robot(int(listInput[0]), int(listInput[1]), str(listInput[2]))
        inputString = raw_input()
        valid = bool(re.match("^(R|F|L){1,100}$", inputString))
        if valid is not True:
            del inputString
            del valid
            del moreInput
            del listInput
            del tempRobot
            sys.exit(-1)
        listRobots.update({tempRobot: inputString})
        inputString = raw_input()
        if inputString:
            inputString = raw_input()
            if inputString:
                moreInput = False

    mars = Map(xSize, ySize)
    for key, value in listRobots.items():
        for i in value:
            key.move(i, mars)
        print("{} {} {} {}".format(key.xPos, key.yPos, key.orientation, 'LOST' if key.isLost else ''))
    del listRobots
    del inputString
    del valid
    del moreInput
    del listInput
    del tempRobot
    del mars
else:
    print("---Test Mode---")
    # We initiliaze the planet landscape
    xSize = 5
    ySize = 3
    mars = Map(xSize, ySize)
    r1 = Robot(1, 1, 'E')
    r1.move('R', mars)
    r1.move('F', mars)
    r1.move('R', mars)
    r1.move('F', mars)
    r1.move('R', mars)
    r1.move('F', mars)
    r1.move('R', mars)
    r1.move('F', mars)
    print("{} {} {} {}".format(r1.xPos, r1.yPos, r1.orientation, 'LOST' if r1.isLost else ''))
    del r1

    r2 = Robot(3, 2, 'N')
    r2.move('F', mars)
    r2.move('R', mars)
    r2.move('R', mars)
    r2.move('F', mars)
    r2.move('L', mars)
    r2.move('L', mars)
    r2.move('F', mars)
    r2.move('F', mars)
    r2.move('R', mars)
    r2.move('R', mars)
    r2.move('F', mars)
    r2.move('L', mars)
    r2.move('L', mars)
    print("{} {} {} {}".format(r2.xPos, r2.yPos, r2.orientation, 'LOST' if r2.isLost else ''))
    del r2

    r3 = Robot(0, 3, 'W')
    r3.move('L', mars)
    r3.move('L', mars)
    r3.move('F', mars)
    r3.move('F', mars)
    r3.move('F', mars)
    r3.move('L', mars)
    r3.move('F', mars)
    r3.move('L', mars)
    r3.move('F', mars)
    r3.move('L', mars)
    print("{} {} {} {}".format(r3.xPos, r3.yPos, r3.orientation, 'LOST' if r3.isLost else ''))
    del r3

    del mars


# This Source Code Form is subject to the terms of the Mozilla Public
# License, v. 2.0. If a copy of the MPL was not distributed with this
# file, You can obtain one at http://mozilla.org/MPL/2.0/.

import datetime
import ssl
from datetime import date

import pytest

from wampy.peers.clients import Client
from wampy.roles.callee import callee
from wampy.testing.helpers import wait_for_session, wait_for_registrations

class DateService(Client):

    @callee
    def get_todays_date(self):
        return datetime.date.today().isoformat()

class TestIP4(object):

    @pytest.fixture
    def config_path(self):
        return './wampy/testing/configs/crossbar.json'

    def test_ipv4_websocket_connection(self, config_path, router):
        service = DateService(url=router.url)
        with service:
            wait_for_registrations(service, 1)

            client = Client(url=router.url)

            with client:
                result = client.rpc.get_todays_date()

        today = date.today()

        assert result == today.isoformat()

class TestIP6(object):

    @pytest.fixture
    def config_path(self):
        return './wampy/testing/configs/crossbar.ipv6.json'

    @pytest.mark.skip(reason="Travis errors wheh swapping between IPV 4 & 6")
    def test_ipv6_websocket_connection(self, config_path, router):
        service = DateService(url=router.url)
        with service:
            wait_for_registrations(service, 1)

            client = Client(url=router.url)

            with client:
                result = client.rpc.get_todays_date()

        today = date.today()

        assert result == today.isoformat()

class TestSecureWebSocket(object):

    @pytest.fixture
    def config_path(self):
        return './wampy/testing/configs/crossbar.tls.json'

    @pytest.fixture
    def url(self):
        return 'wss://localhost:9443'

    def test_ipv4_secure_websocket_connection_by_router_url(
        self, config_path, router
    ):
        assert router.url == "wss://localhost:9443"

        try:
            ssl.PROTOCOL_TLSv1_2
        except AttributeError:
            pytest.skip('Python Environment does not support TLS')

        with DateService(
            url="wss://localhost:9443",
            cert_path="./wampy/testing/keys/server_cert.pem",
        ) as service:
            wait_for_registrations(service, 1)

            client = Client(
                url="wss://localhost:9443",
                cert_path="./wampy/testing/keys/server_cert.pem",
            )
            with client:
                wait_for_session(client)
                result = client.rpc.get_todays_date()

        today = date.today()

        assert result == today.isoformat()