revisite o codigo aqui e parece estar bem legal e simples para você iiniciar.

minha dica, execute ele com um debugger e rode linha a linha, vendo os parametros e resultados de cada operação.