If you have complex algorithms, you probably want to create a Java service and import it in your template. This imply that you have knowledge in Java and Eclipse plug-in development.
You can also have a look at the Capella extension code.
Some services can help you like ancestors() that can be filtered on a type, but it will not be enough to satisfy your requirement.