Wordpress Theme Switcher plugin redirect modification

Hello, I would like to modify the Theme Switcher Plugin for Wordpress (http://dev.wp-plugins.org/wiki/ThemeSwitcher). When I currently request the page to switch the theme (for example: mydomain.com/?wptheme=My+Theme) I'm redirected to the index.php, but instead I would like to redirect back to the page where I was before (for example: mydomain.com/?p=15) Here is the code which is redirecting the page:

function ts_set_theme_cookie() {     $expire = time <http://www.php.net/time>() + 30000000;     if (!empty <http://www.php.net/empty>($_GET["wptheme"])) {         setcookie <http://www.php.net/setcookie>("wptheme" . COOKIEHASH,                            stripslashes <http://www.php.net/stripslashes>($_GET["wptheme"]),                            $expire,                             COOKIEPATH                            );         $redirect = get_settings('home').'/';         if (function_exists <http://www.php.net/function_exists>('wp_redirect'))            wp_redirect($redirect);         else             header <http://www.php.net/header>("Location: ". $redirect);         exit <http://www.php.net/exit>;     } }

I've tried to change the $redirect variable to $_SERVER['REQUEST_URI'] but it doesn't worked. Then I realized that I should save the current page URL in the cookies, so I've set a cookie and then I tried redirect the page to the URL in that cookie. It worked, but not as intended, there are some situations when it is redirecting in a loop. Any recommendations how to do it properly? Thanks, Erik