diff --git a/main.c b/main.c
index ccfb0fe..ad65852 100644
--- a/main.c
+++ b/main.c
@@ -5,6 +5,7 @@
 #include <stdio.h>
 
 #define LED_PIN PD6
+#define RSA_16
 
 void exit_blink() {
     for (int i = 0; i < 4; i++) {
@@ -40,37 +41,43 @@ int main() {
     while (p == q) p = gen_prime(1 << 15, 1 << 16);
 
     uint64_t n = p * q;
+    uint64_t phi_n = (p - 1) * (q - 1);
 
-    int before = SysTick->CNT;
-    bool check_a = miller_rabin(p, 10);
-    int miller = SysTick->CNT - before;
+    // 'e' is public. E for encrypt.
+    uint64_t e = prand_range(3, phi_n - 1);
+    while (gcd(e, phi_n) != 1) e = prand_range(3, phi_n - 1);
 
-    before = SysTick->CNT;
-    bool check_b = is_prime(p);
-    int isprime = SysTick->CNT - before;
-
-    printf("Is prime: %s %s\n", check_a ? "true" : "false",
-           check_b ? "true" : "false");
-
-    printf("Miller took %d ticks\n", miller);
-    printf("Is_prime took %d ticks\n", isprime);
-
-    // Make these work by patching printf
-    printf("P: %u\n", (uint32_t)p);
-    printf("Q: %u\n", (uint32_t)q);
-    printf("N: %u\n", (uint32_t)n);
-
-    for (int idx = 0; idx < 16; idx++) {
-        funDigitalWrite(LED_PIN, p >> idx & 1);
-        Delay_Ms(200);
+    // 'd' is our private key. D as in decrypt
+    uint64_t d = mod_inverse(e, phi_n);
+    if (d == 0 || d == 1) {
+        printf("Modular inverse not found...");
+        while (1);
     }
-    for (int idx = 0; idx < 16; idx++) {
-        funDigitalWrite(LED_PIN, q >> idx & 1);
-        Delay_Ms(200);
+
+    char msg[] = "Hello";
+    uint64_t coded[sizeof(msg)] = {0};
+    char decoded[sizeof(msg)] = {0};
+
+    // Encode the message
+    for (int i = 0; i < sizeof(msg); i++) {
+        coded[i] = (uint64_t)modexp((uint64_t)msg[i], e, n);
     }
-    for (int idx = 0; idx < 16; idx++) {
-        funDigitalWrite(LED_PIN, n >> idx & 1);
-        Delay_Ms(200);
+
+    // Decode the message
+    for (int i = 0; i < sizeof(msg); i++) {
+        decoded[i] = (char)modexp(coded[i], d, n);
+    }
+
+    {
+        printf("P: %u\n", (uint32_t)p);
+        printf("Q: %u\n", (uint32_t)q);
+        printf("N: %u\n", (uint32_t)n);
+        printf("Phi_N: %u\n", (uint32_t)phi_n);
+        printf("Pubkey (e): %u\n", (uint32_t)e);
+        printf("Privkey (d): %u\n", (uint32_t)d);
+
+        printf("Message: %s\n", msg);
+        printf("Decoded: %s\n", decoded);
     }
 
     // Exit and hang forever