Category with Product in Menu Navigation of Magento frontend

Shops with a few products can view their product according to category in navigation. It will simplify a for customer to find their product find easily.
There is no such feature in magento default layered navigation. So, I created one, It take me 7 hr to write few line of code :-( you can add this code in cms page, /app/design/frontend/base/default/template/page/2column-right.phtml or any where in your theme. have fun !

$collection = Mage::getModel('catalog/category')->getCollection();
foreach ($collection AS $_cat)
if(($_cat->getId()!=1) && ($_cat->getId()!=2))
$cat = Mage::getModel('catalog/category')->load($_cat->getId());
echo '<li>'.$cat->getName().' </li>';
$cur_category = Mage::getModel('catalog/category')->load($_cat->getId());
$_productCollection =Mage::getResourceModel('catalog/product_collection')
->setOrder('position', 'ASC');
echo '<ul>';
foreach ($_productCollection AS $_product)
$cur_product = Mage::getModel('catalog/product')->load($_product->getId());
echo '<li><a href="'.$cur_product->getUrl_key().'.html">'.$cur_product->getName().'</a></li>';
echo '</ul></dd>';
} //end menu