Automatically add custom taxonomies to custom post type menu item

I have two custom post types added to default nav menu and I would like that assigned custom taxonomies to those types automatically show as submenu. Currently for testing purposes I have been able to achieve that for one of my post types it gets added to the end of menu. Currently I have two filters, but I suppose it could be done only with wp_list_categories filter where like now I have that it sets current class. So basically I would need to check if current menu item is one of my custom post types and show its custom taxonomy as submenu.
Here is my current code working somewhere in the direction I want: add_filter('wp_list_categories','style_current_cat_single_post');
function style_current_cat_single_post($output) {
if( is_single()) :
global $post;
foreach ( get_the_terms($post->ID, 'my-cystom-taxonomy') as $cat ) {
$cats[] = $cat->term_id;
}
foreach($cats as $value) {
if(preg_match('#item-' . $value . '">#', $output)) {
$output = str_replace('item-' . $value . '">', 'item-' . $value . ' current-menu-item">', $output);
}
}
endif;
return $output;
}

add_filter( 'wp_nav_menu_items', 'your_custom_menu_item', 10, 2 );
function your_custom_menu_item ( $items, $args ) {
$items .= wp_list_categories('taxonomy=my-cystom-taxonomy&title_li=&echo=0&depth=1');
return $items;
}
If someone knows a good plugin for this, please let me know. I haven’t been able to find exactly the one I need – to be able to add custom post types to the menu with automatically populated submenu with assigned custom taxonomy. And of course showing current classes as needed.