diff --git a/Notes.md b/Notes.md
index 45055db..8512a65 100644
--- a/Notes.md
+++ b/Notes.md
@@ -5,3 +5,23 @@ The density of FR4 is 1,850 kg/m^3. If we assume a PCB area of 12*50 =
 
 For a 1.0mm PCB: 600 * 1.0 * 1.85 = 1.110g
 For a 1.6mm PCB: 600 * 1.6 * 1.85 = 1.728g
+
+## Contestia ##
+
+There doesn't appear to be much documentation for contestia...
+
+[Oh here's some](http://f1ult.free.fr/DIGIMODES/MULTIPSK/contestia_rttym_en.htm)
+
+Really we're looking at Olivia MFSK - contestia is just a variant.
+
+[Wikipedia](http://en.wikipedia.org/wiki/Olivia_MFSK)
+[ARRL](http://www.arrl.org/olivia)
+
+It's already implemented in dl-fldigi [here](https://github.com/jamescoxon/dl-fldigi/blob/master/src/include/jalocha/pj_mfsk.h)
+
+(Inverse) Fast Hadamard Transform needed
+
+
+## APRS ##
+
+[some description..](http://dooce.de/dokuwiki/doku.php?id=projekte:utrak:aprs)
diff --git a/firmware/inc/math/fwht b/firmware/inc/math/fwht
new file mode 100755
index 0000000..de635cc
Binary files /dev/null and b/firmware/inc/math/fwht differ
diff --git a/firmware/inc/math/fwht.h b/firmware/inc/math/fwht.h
new file mode 100644
index 0000000..416ab43
--- /dev/null
+++ b/firmware/inc/math/fwht.h
@@ -0,0 +1,124 @@
+/*
+ * Fast Hadamard Transform
+ * Copyright (C) 2014  Richard Meadows <richardeoin>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef FHT_H
+#define FHT_H
+
+#ifdef FWHT_TEST
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+#else
+#include "samd20.h"
+#endif
+
+/**
+ * Resources:
+ *
+ *
+ */
+
+/**
+ * We might want to use this with different types in the future.
+ */
+#define T int8_t
+
+/**
+ * Fast Walsh-Hadamard Transform
+ *
+ * `data` an array that is the input vector. Modifed in-place
+ * `len` is the number of elements in `data`
+ *
+ * The result is returned in "hadamard" order
+ */
+void fwht(T *Data, size_t Len)
+{
+  size_t Step, Ptr, Ptr2; T Bit1, Bit2, NewBit1, NewBit2;
+  for(Step=1; Step<Len; Step*=2)
+  { for(Ptr=0; Ptr<Len; Ptr+=2*Step)
+    { for(Ptr2=Ptr; (Ptr2-Ptr)<Step; Ptr2+=1)
+      { Bit1=Data[Ptr2];  Bit2=Data[Ptr2+Step];
+        NewBit1=Bit2; NewBit1+=Bit1;
+        NewBit2=Bit2; NewBit2-=Bit1;
+        Data[Ptr2]=NewBit1; Data[Ptr2+Step]=NewBit2;
+      }
+    }
+  }
+}
+
+/**
+ * Inverse Fast Hadamard Transform
+ *
+ * `data` an array that is the input vector. Modifed in-place
+ * `len` is the number of elements in `data`
+ *
+ * The result is returned in "hadamard" order
+ */
+void ifwht(T *Data, size_t len)
+{
+  size_t step, Ptr, Ptr2; T Bit1, Bit2, NewBit1, NewBit2;
+  for(step=len/2; step; step/=2)
+  { for(Ptr=0; Ptr<len; Ptr+=2*step)
+    { for(Ptr2=Ptr; (Ptr2-Ptr)<step; Ptr2+=1)
+      { Bit1=Data[Ptr2];  Bit2=Data[Ptr2+step];
+        NewBit1=Bit1; NewBit1-=Bit2;
+        NewBit2=Bit1; NewBit2+=Bit2;
+        Data[Ptr2]=NewBit1; Data[Ptr2+step]=NewBit2;
+      }
+    }
+  }
+}
+
+#ifdef FWHT_TEST
+
+#include "string.h"
+
+int main(void)
+{
+  T fwht_test[] = {1,0,1,0,0,1,1,0};
+  const int fwht_test_len = 8;
+
+  fwht(fwht_test, fwht_test_len);
+
+  printf("Fast Walsh-Hadamard Transform: [");
+  for (int i; i < 8; i++) { printf("%d,", fwht_test[i]); }
+  printf("]\n");
+
+
+
+  T ifwht_test[] = {1,0,1,0,0,1,1,0};
+  const int ifwht_test_len = 8;
+
+  ifwht(ifwht_test, ifwht_test_len);
+
+  printf("Inverse Fast Walsh-Hadamard Transform: [");
+  for (int i; i < 8; i++) { printf("%d,", ifwht_test[i]); }
+  printf("]\n");
+
+  return 0;
+}
+#endif
+
+
+#endif /* FHT_H */
diff --git a/firmware/inc/mfsk.h b/firmware/inc/mfsk.h
index 325378e..3d3fee0 100644
--- a/firmware/inc/mfsk.h
+++ b/firmware/inc/mfsk.h
@@ -27,6 +27,6 @@
 
 #include "samd20.h"
 
-void olivia_mfsk_encode_block(char* buffer, int8_t* tones);
+void olivia_mfsk_encode_block(char* buffer, uint8_t* tones);
 
 #endif /* MFSK_H */
diff --git a/firmware/src/mfsk.c b/firmware/src/mfsk.c
index 6308ffe..ba9fd50 100644
--- a/firmware/src/mfsk.c
+++ b/firmware/src/mfsk.c
@@ -25,7 +25,8 @@
 #include <string.h>
 
 #include "samd20.h"
-#include "msfk.h"
+#include "mfsk.h"
+#include "math/fwht.h"
 
 //#define OLIVIA_MFSK_ENCODE_TEST
 #ifdef OLIVIA_MFSK_ENCODE_TEST
@@ -53,27 +54,120 @@ static const uint64_t ScramblingCodeContestia	= 0xEDB88320LL;
  * https://github.com/jamescoxon/dl-fldigi/blob/master/src/include/jalocha/pj_mfsk.h
  */
 
+
+
+
+
+size_t BitsPerSymbol = 5;
+size_t Symbols = 32;
+
+size_t BitsPerCharacter = 7;
+size_t SymbolsPerBlock = 64;
+
+uint8_t bContestia = 0;
+
+int8_t FHT_Buffer[64]; /* SymbolsPerBlock */
+uint8_t OutputBlock[64]; /* SymbolsPerBlock */
+
+static const uint64_t ScramblingCode = 0xE257E6D0291574ECLL;
+
+void EncodeCharacter(uint8_t Char) {
+  size_t TimeBit;
+  uint8_t Mask = (SymbolsPerBlock << 1) - 1;
+
+  if (bContestia) {
+    if (Char >= 'a' && Char <= 'z')
+      Char += 'A' - 'a';
+    if (Char == ' ')
+      Char = 59;
+    else if (Char == '\r')
+      Char = 60;
+    else if (Char == '\n')
+      Char = 0;
+    else if (Char >= 33 && Char <= 90)
+      Char -= 32;
+    else if (Char == 8)
+      Char = 61;
+    else if (Char == 0)
+      Char = 0;
+    else
+      Char = '?' - 32;
+//} else if (bRTTYM) {
+  } else {
+    Char &= Mask;
+  }
+
+  for (TimeBit = 0; TimeBit < SymbolsPerBlock; TimeBit++)
+    FHT_Buffer[TimeBit] = 0;
+  if (Char<SymbolsPerBlock)
+    FHT_Buffer[Char] = 1;
+  else
+    FHT_Buffer[Char-SymbolsPerBlock] = (-1);
+  ifwht(FHT_Buffer, SymbolsPerBlock);
+}
+
+void ScrambleFHT(size_t CodeOffset)
+{ size_t TimeBit;
+  size_t CodeWrap=(SymbolsPerBlock-1);
+  size_t CodeBit=CodeOffset&CodeWrap;
+  for (TimeBit=0; TimeBit<SymbolsPerBlock; TimeBit++)
+  { uint64_t CodeMask=1; CodeMask<<=CodeBit;
+    if (ScramblingCode&CodeMask)
+      FHT_Buffer[TimeBit] = (-FHT_Buffer[TimeBit]);
+    CodeBit+=1; CodeBit&=CodeWrap; }
+}
+
+void EncodeBlock(uint8_t *InputBlock) {
+  size_t FreqBit;
+  size_t TimeBit;
+  size_t nShift;
+
+  nShift = (bContestia) ? 5 : 13; // Contestia/RTTYM or Olivia
+
+  for (TimeBit = 0; TimeBit < SymbolsPerBlock; TimeBit ++)
+    OutputBlock[TimeBit] = 0;
+
+  for (FreqBit = 0; FreqBit < BitsPerSymbol; FreqBit++) {
+    EncodeCharacter(InputBlock[FreqBit]);
+    ScrambleFHT(FreqBit * nShift);
+    size_t Rotate = 0;
+    for (TimeBit = 0; TimeBit < SymbolsPerBlock; TimeBit++) {
+      if (FHT_Buffer[TimeBit] < 0) {
+        size_t Bit = FreqBit+Rotate;
+        if (Bit >= BitsPerSymbol) Bit -= BitsPerSymbol;
+        uint8_t Mask = 1;
+        Mask <<= Bit;
+        OutputBlock[TimeBit] |= Mask;
+      }
+      Rotate += 1;
+      if (Rotate >= BitsPerSymbol)
+        Rotate -= BitsPerSymbol;
+    }
+  }
+}
+
+
+
+
+
+
+
 /**
  * This function encodes a single block of Olivia MFSK
  *
  * It takes a buffer of ASCII-encoded text and returns an array of
  * tones to transmit.
  */
-void olivia_mfsk_encode_block(char* buffer, int8_t* tones)
+void olivia_mfsk_encode_block(char* buffer, uint8_t* tones)
 {
   size_t bits_per_character = 7;
 
   size_t bits_per_symbol = 5; /* That is, there are 2^5=32 tones */
   size_t symbols_per_block = 64;
 
-
-
-
-  /* TODO! */
-
-
-
-
+  /* For the moment do this */
+  EncodeBlock((uint8_t*)buffer);
+  memcpy(tones, OutputBlock, symbols_per_block);
 }
 
 #ifdef OLIVIA_MFSK_ENCODE_TEST