diff --git a/ExampleVM.hs b/ExampleVM.hs
--- a/ExampleVM.hs
+++ b/ExampleVM.hs
@@ -21,7 +21,7 @@
                       ++ (show expect) ++ ". Got: " ++ (show got)
 
       setLoop t@(envQ, motor) =
-          do atomically $ writeTBQueue envQ (RealEnv 55 55)
+          do atomically $ writeTBQueue envQ (RealEnv 55 0)
              threadDelay 100000 -- 10 ms
              assertMotor "light = 55" motor (37, 37)
 
diff --git a/NXT/Core.hs b/NXT/Core.hs
--- a/NXT/Core.hs
+++ b/NXT/Core.hs
@@ -1,4 +1,3 @@
-{-# LANGUAGE GADTs #-}
 {-# LANGUAGE OverloadedStrings #-}
 {-# LANGUAGE FlexibleInstances #-}
 {-# LANGUAGE FlexibleContexts #-}
@@ -46,44 +45,16 @@
 import Prelude hiding ((<), (>), (<=), (>=), (==), (/=), (&&), (||))
 import qualified Prelude as P
 
-prettyFD :: FunDefinition -> String
-prettyFD (FunDefinition name ty args body) =
-    ty ++ " " ++ name ++ "(" ++ arglist ++ ") {" ++ (concatMap prettyStmt body) ++ "}"
-    where
-      arglist = intercalate ", " $ map (\(DeclVar arg) -> ((getCType arg) ++ " " ++ (prettyV arg))) args
-
-prettyStmt :: Stmt -> String
-prettyStmt (If cond t f) =
-    "if (" ++ (prettyV cond) ++ ") {"
-    ++ (concatMap prettyStmt t) ++ "}"
-    ++ (case f of
-          [] -> ""
-          _ -> "else { " ++ (concatMap prettyStmt f) ++ "}"
-       )
-prettyStmt (While cond loop) =
-    "while (" ++ (prettyV cond) ++ ") {"
-    ++ (concatMap prettyStmt loop) ++ "}"
-prettyStmt (DeclVar var@(V (VarP name))) =
-    (getCType var) ++ " " ++ (prettyV var) ++ ";"
-prettyStmt (AssignVar v@(V (VarP pointer)) val) =
-    (prettyV v) ++ " = " ++ (prettyV val) ++ ";"
-prettyStmt (Eval something) =
-    (prettyV something) ++ ";"
-prettyStmt (FunReturn val) =
-    "return " ++ prettyV val ++ ";"
-
-prettyStmt _ = error "Error: Invalid syntax tree"
-
 instance (Num a, Typeable a) => Num (V a) where
-    x + y = pack $ BinOp BAdd x y
-    x * y = pack $ BinOp BMul x y
-    x - y = pack $ BinOp BSub x y
+    x + y = pack $ BinOp BAdd (unpack x) (unpack y)
+    x * y = pack $ BinOp BMul (unpack x) (unpack y)
+    x - y = pack $ BinOp BSub (unpack x) (unpack y)
     abs x = error "Not implemented"
     signum x = error "Not implemented"
     fromInteger = pack . Lit . fromInteger
 
 instance Fractional (V Float) where
-    x / y = pack $ BinOp BDiv x y
+    x / y = pack $ BinOp BDiv (unpack x) (unpack y)
     fromRational = pack . Rat . fromRational
 
 instance IsString (V String) where
@@ -91,39 +62,39 @@
 
 -- | logic and
 (&&) :: V Bool -> V Bool -> V Bool
-a && b = pack $ BinOp BAnd a b
+a && b = pack $ BinOp BAnd (unpack a) (unpack b)
 
 -- | logic or
 (||) :: V Bool -> V Bool -> V Bool
-a || b = pack $ BinOp BOr a b
+a || b = pack $ BinOp BOr (unpack a) (unpack b)
 
 -- | compare two numbers for >
-(>) :: (Num a) => V a -> V a -> V Bool
-a > b = pack $ BinOp BLt a b
+(>) :: (Num a, Typeable a) => V a -> V a -> V Bool
+a > b = pack $ BinOp BLt (unpack a) (unpack b)
 
 -- | compare two numbers for <
-(<) :: (Num a) => V a -> V a -> V Bool
-a < b = pack $ BinOp BSt a b
+(<) :: (Num a, Typeable a) => V a -> V a -> V Bool
+a < b = pack $ BinOp BSt (unpack a) (unpack b)
 
 -- | compare two numbers for >=
-(>=) :: (Num a) => V a -> V a -> V Bool
-a >= b = pack $ BinOp BLEq a b
+(>=) :: (Num a, Typeable a) => V a -> V a -> V Bool
+a >= b = pack $ BinOp BLEq (unpack a) (unpack b)
 
 -- | compare two numbers for <=
-(<=) :: (Num a) => V a -> V a -> V Bool
-a <= b = pack $ BinOp BSEq a b
+(<=) :: (Num a, Typeable a) => V a -> V a -> V Bool
+a <= b = pack $ BinOp BSEq (unpack a) (unpack b)
 
 -- | the c++ == operation
-(==) :: V a -> V a -> V Bool
-a == b = pack $ BinOp BEq a b
+(==) :: (Typeable a) => V a -> V a -> V Bool
+a == b = pack $ BinOp BEq (unpack a) (unpack b)
 
 -- | the c++ != operation
-(/=) :: V a -> V a -> V Bool
-a /= b = pack $ BinOp BNEq a b
+(/=) :: (Typeable a) => V a -> V a -> V Bool
+a /= b = pack $ BinOp BNEq (unpack a) (unpack b)
 
 -- | string concatination
 (&) :: V String -> V String -> V String
-a & b = pack $ FunCall2 "StrCat" a b
+a & b = pack $ FunCall "StrCat" [(unpack a), (unpack b)]
 
 -- | Define an external function. Don't forget to add a type signature
 defExt :: (Typeable a) => String -> V a
@@ -143,12 +114,12 @@
 
 -- | Cast an int to a float
 castFloat :: V Int -> V Float
-castFloat x = pack $ CastOp "cast2float" x
+castFloat x = pack $ CastOp "cast2float" $ unpack x
 
 
 -- | Cast a float to an int
 castInt :: V Float -> V Int
-castInt x = pack $ CastOp "cast2int" x
+castInt x = pack $ CastOp "cast2int" $ unpack x
 
 -- | Void. Do nothing
 void :: V ()
@@ -157,7 +128,7 @@
 -- | Execute a void statement
 (.!) :: (Typeable a) => V a -> FunM ()
 (.!) stmt =
-    tell [Eval stmt]
+    tell [Eval $ unpack stmt]
 
 -- | non monadic variable assignment
 ($=) :: (Typeable a) => V a -> V a -> FunM ()
@@ -168,7 +139,7 @@
 (#=) :: (Typeable a) => V a -> FunM (V a) -> FunM ()
 v@(V (VarP pointer)) #= val =
     do rVal <- val
-       tell [AssignVar v rVal]
+       tell [AssignVar (unpack v) (unpack rVal)]
 (#=) _ _ = error "LH must be variable!"
 
 newtype FResult a = FResult { unFResult :: a }
@@ -176,7 +147,7 @@
 -- | define a functions return value
 ret :: (Typeable a) => V a -> FunM (FResult (V a))
 ret val =
-    do tell [FunReturn val]
+    do tell [FunReturn $ unpack val]
        return $ FResult val
 
 -- | the c++ while construct
@@ -184,7 +155,7 @@
 while cond loop =
     do orig <- get
        (_, _, loopOut) <- lift $ runRWST loop "loop" (orig + 200)
-       tell [While cond loopOut]
+       tell [While (unpack cond) loopOut]
 
 -- | when
 when :: V Bool -> FunM () -> FunM ()
@@ -197,7 +168,7 @@
     do orig <- get
        (_, _, tOut) <- lift $ runRWST t "ifTrue" (orig + 200)
        (_, _, fOut) <- lift $ runRWST f "ifFalse" (orig + 200)
-       tell [If cond tOut fOut]
+       tell [If (unpack cond) tOut fOut]
 
 getCType :: (Typeable a) => V a -> String
 getCType a
@@ -228,7 +199,7 @@
            var :: V a
            var = V (VarP $ "v" ++ show name)
        put name
-       tell [DeclVar var]
+       tell [dVar var]
        --tell ((getCType var) ++ " " ++ (prettyV var) ++ ";")
        return $ var
 
@@ -242,13 +213,6 @@
        put name
        return $ pack fun
 
-funTpl funN fType fArgs fBody =
-    do tell $ funHeader ++ ";"
-       tell $ funHeader ++ " { " ++ (concatMap prettyStmt fBody) ++ " }"
-    where
-      funHeader = fType ++ " " ++ funN
-                  ++ "(" ++ (intercalate "," $ map (\(t, v) -> t ++ " " ++ v) fArgs) ++ ")"
-
 class (Typeable a) => FunDef a b | b -> a  where
     -- | Define a declared function
     def :: V a -> b -> TopM ()
@@ -264,7 +228,7 @@
         do let var :: V a
                var = V (VarP "p1")
            (x, _, out) <- runRWST (funAction var) (prettyV name) 0
-           tell [FunDefinition (prettyV name) (getCType $ unFResult x) [DeclVar var] out]
+           tell [FunDefinition (prettyV name) (getCType $ unFResult x) [dVar var] out]
            return ()
 
 instance forall a b c.  (Typeable a, Typeable b, Typeable c) => FunDef (V a -> V b -> V c) ((V a -> V b -> FunM (FResult (V c)))) where
@@ -274,7 +238,7 @@
                var2 :: V b
                var2 = V (VarP "p2")
            (x, _, out) <- runRWST (funAction var var2) (prettyV name) 0
-           tell [FunDefinition (prettyV name) (getCType $ unFResult x) [DeclVar var, DeclVar var2] out]
+           tell [FunDefinition (prettyV name) (getCType $ unFResult x) [dVar var, dVar var2] out]
            return ()
 
 instance forall a b c d.  (Typeable a, Typeable b, Typeable c, Typeable d) => FunDef (V a -> V b -> V c -> V d) ((V a -> V b -> V c -> FunM (FResult (V d)))) where
@@ -286,7 +250,7 @@
                var3 :: V c
                var3 = V (VarP "p3")
            (x, _, out) <- runRWST (funAction var var2 var3) (prettyV name) 0
-           tell [FunDefinition (prettyV name) (getCType $ unFResult x) [DeclVar var, DeclVar var2, DeclVar var3] out]
+           tell [FunDefinition (prettyV name) (getCType $ unFResult x) [dVar var, dVar var2, dVar var3] out]
            return ()
 
 instance forall a b c d e.  (Typeable a, Typeable b, Typeable c, Typeable d, Typeable e) => FunDef (V a -> V b -> V c -> V d -> V e) ((V a -> V b -> V c -> V d -> FunM (FResult (V e)))) where
@@ -300,42 +264,46 @@
                var4 :: V d
                var4 = V (VarP "p4")
            (x, _, out) <- runRWST (funAction var var2 var3 var4) (prettyV name) 0
-           tell [FunDefinition (prettyV name) (getCType $ unFResult x) [DeclVar var, DeclVar var2, DeclVar var3, DeclVar var4] out]
+           tell [FunDefinition (prettyV name) (getCType $ unFResult x)
+                                   [ dVar var, dVar var2, dVar var3, dVar var4] out]
            return ()
 
+dVar v =
+    DeclVar (getCType v) (unpack v)
+
 -- | Call a function with no arguments
 callF :: forall a. (Typeable a)
       => (V a) -> (V a)
 callF fp@(V (FunP name)) =
-    pack $ FunCall (prettyV fp)
+    pack $ FunCall (prettyV fp) []
 callF _ = error "Can only call functions!"
 
 -- | Call a function with one argument
 callF1 :: forall a b. (Typeable a, Typeable b)
       => (V (V a -> V b)) -> V a -> (V b)
 callF1 fp@(V (FunP name)) arg =
-    pack $ FunCall1 (prettyV fp) arg
+    pack $ FunCall (prettyV fp) [unpack arg]
 callF1 _ _ = error "Can only call functions!"
 
 -- | Call a function with two arguments
 callF2 :: forall a b c. (Typeable a, Typeable b, Typeable c)
       => (V (V a -> V b -> V c)) -> V a -> V b -> (V c)
 callF2 fp@(V (FunP name)) arg1 arg2 =
-    pack $ FunCall2 (prettyV fp) arg1 arg2
+    pack $ FunCall (prettyV fp) [(unpack arg1), (unpack arg2)]
 callF2 _ _ _ = error "Can only call functions!"
 
 -- | Call a function with tree arguments
 callF3 :: forall a b c d. (Typeable a, Typeable b, Typeable c, Typeable d)
       => (V (V a -> V b -> V c -> V d)) -> V a -> V b -> V c -> V d
 callF3 fp@(V (FunP name)) arg1 arg2 arg3 =
-    pack $ FunCall3 (prettyV fp) arg1 arg2 arg3
+    pack $ FunCall (prettyV fp) [(unpack arg1), (unpack arg2), (unpack arg3)]
 callF3 _ _ _ _ = error "Can only call functions!"
 
 -- | Call a function with four arguments
 callF4 :: forall a b c d e. (Typeable a, Typeable b, Typeable c, Typeable d, Typeable e)
       => (V (V a -> V b -> V c -> V d -> V e)) -> V a -> V b -> V c -> V d -> V e
 callF4 fp@(V (FunP name)) arg1 arg2 arg3 arg4 =
-    pack $ FunCall4 (prettyV fp) arg1 arg2 arg3 arg4
+    pack $ FunCall (prettyV fp) [(unpack arg1), (unpack arg2), (unpack arg3), (unpack arg4)]
 callF4 _ _ _ _ _ = error "Can only call functions!"
 
 vCallF fp = (.!)$ callF fp
@@ -359,18 +327,15 @@
 
 mkStdLib :: TopM ()
 mkStdLib =
-    do let var :: V Float
-           var = V (VarP "f")
+    do let var = (VarP "f")
            body = [FunReturn var]
-
-           var2 :: V Int
-           var2 = V (VarP "i")
+           var2 = (VarP "i")
            body2 = [FunReturn var2]
 
-       tell [FunDefinition "cast2int" "int"  [DeclVar var] body]
-       tell [FunDefinition "cast2float" "float" [DeclVar var2] body2]
+       tell [FunDefinition "cast2int" "int"  [DeclVar "float" var] body]
+       tell [FunDefinition "cast2float" "float" [DeclVar "int" var2] body2]
 
 -- | Define a function as main task
-setMain :: V a -> TopM ()
+setMain :: (Typeable a) => V a -> TopM ()
 setMain fp@(V (FunP name)) =
-    tell [FunDefinition "main" "task" [] [Eval $ callF fp]]
+    tell [FunDefinition "main" "task" [] [Eval $ unpack $ callF fp]]
diff --git a/NXT/Interpretation.hs b/NXT/Interpretation.hs
--- a/NXT/Interpretation.hs
+++ b/NXT/Interpretation.hs
@@ -1,4 +1,4 @@
-{-# LANGUAGE GADTs, DeriveDataTypeable, DoAndIfThenElse #-}
+{-# LANGUAGE DeriveDataTypeable, DoAndIfThenElse #-}
 -- |
 -- Module      : NXT.Interpretation
 -- Copyright   : Alexander Thiemann <mail@agrafix.net>
@@ -187,36 +187,36 @@
 
 runStmt :: FunMap -> Env -> Stmt -> IO ()
 runStmt funMap env (If condR t f) =
-    do (NBool cond) <- runV funMap env condR
+    do (NBool cond) <- runT funMap env condR
        if cond
        then mapM_ (runStmt funMap env) t
        else mapM_ (runStmt funMap env) f
 runStmt funMap env l@(While condR loop) =
-    do (NBool cond) <- runV funMap env condR
+    do (NBool cond) <- runT funMap env condR
        if cond
        then do mapM_ (runStmt funMap env) loop
                runStmt funMap env l
        else return ()
-runStmt funMap env (DeclVar (V (VarP p))) =
+runStmt funMap env (DeclVar _ (VarP p)) =
     do defineVar env p NVoid
        return ()
-runStmt funMap env (AssignVar (V (VarP p)) valR) =
-    do val <- runV funMap env valR
+runStmt funMap env (AssignVar (VarP p) valR) =
+    do val <- runT funMap env valR
        setVar env p val
        return ()
 
 runStmt funMap env (Eval v) =
-    do _ <- runV funMap env v
+    do _ <- runT funMap env v
        return ()
 runStmt funMap env (FunReturn v) =
-    do val <- runV funMap env v
+    do val <- runT funMap env v
        defineVar env "__funReturnVal" val
        return ()
 
 runV :: FunMap -> Env -> V a -> IO NXTVal
 runV funMap env ct = runT funMap env (unpack ct)
 
-runT :: FunMap -> Env -> T a -> IO NXTVal
+runT :: FunMap -> Env -> T -> IO NXTVal
 runT _ _ Void = return NVoid
 runT _ env (VarP p) = getVar env p
 runT _ env (Lit i) = return $ NInt i
@@ -225,19 +225,19 @@
 runT _ env (BoolLit b) = return $ NBool b
 
 runT funMap env (CastOp "cast2int" val) =
-     do evaluated <- runV funMap env val
+     do evaluated <- runT funMap env val
         case evaluated of
           NFloat f -> return $ NInt $ floor f
           _ -> error "Invalid cast."
 runT funMap env (CastOp "cast2float" val) =
-     do evaluated <- runV funMap env val
+     do evaluated <- runT funMap env val
         case evaluated of
           NInt i -> return $ NFloat $ fromIntegral i
           _ -> error "Invalid cast."
 
 runT funMap env (BinOp op xR yR) =
-    do x <- runV funMap env xR
-       y <- runV funMap env yR
+    do x <- runT funMap env xR
+       y <- runT funMap env yR
 
        case (x, op, y) of
          (NInt a, BAdd, NInt b) -> return $ NInt (a + b)
@@ -267,40 +267,19 @@
          (NBool a, BOr, NBool b) -> return $ NBool (a || b)
          x -> error $ "Unknown operation: " ++ show x
 
-runT funMap env (FunCall name) =
-    apply funMap name []
-
-runT funMap env (FunCall1 name a) =
-    do aR <- runV funMap env a
-       apply funMap name [aR]
-
-runT funMap env (FunCall2 name a b) =
-    do aR <- runV funMap env a
-       bR <- runV funMap env b
-       apply funMap name [aR, bR]
-
-runT funMap env (FunCall3 name a b c) =
-    do aR <- runV funMap env a
-       bR <- runV funMap env b
-       cR <- runV funMap env c
-       apply funMap name [aR, bR, cR]
-
-runT funMap env (FunCall4 name a b c d) =
-    do aR <- runV funMap env a
-       bR <- runV funMap env b
-       cR <- runV funMap env c
-       dR <- runV funMap env d
-       apply funMap name [aR, bR, cR, dR]
+runT funMap env (FunCall name args) =
+    do evaled <- mapM (runT funMap env) args
+       apply funMap name evaled
 
 runT _ env t = error $ "Not implemented:" ++ (prettyT t)
 
-reqV (V (VarP p)) = p
-reqV x = error $ "Require a variable, but got: " ++ (prettyV x)
+reqV (VarP p) = p
+reqV x = error $ "Require a variable, but got: " ++ (prettyT x)
 
 getArgs funMap funName =
     case HM.lookup funName (fm_funs funMap) of
       Just (FunDefinition _ _ args _) ->
-          map (\(DeclVar arg) -> reqV arg) args
+          map (\(DeclVar _ arg) -> reqV arg) args
       Nothing ->
           error $ "Function " ++ funName ++ " is not defined."
 
diff --git a/NXTDSL.cabal b/NXTDSL.cabal
--- a/NXTDSL.cabal
+++ b/NXTDSL.cabal
@@ -1,5 +1,5 @@
 Name:                NXTDSL
-Version:             0.1
+Version:             0.2
 Synopsis:            Generate NXC Code from DSL
 Description:         Typesafe code generation for the LEGO-NXT
 License:             BSD3
@@ -14,7 +14,7 @@
 Bug-reports:            https://github.com/agrafix/legoDSL/issues
 
 Library
-   Build-Depends:     base >= 3 && < 5, mtl, unordered-containers, resourcet >= 0.4.4, stm >= 2.4.2
+   Build-Depends:     base >= 3 && < 5, mtl, unordered-containers, resourcet >= 0.4.4, stm >= 2.4.2, text, uu-parsinglib >= 2.7.4
    Exposed-modules:   NXT.Core, NXT.Interpretation, NXT.Common, NXT.Motor, NXT.Sensor
 
 Executable ExampleRun
@@ -23,6 +23,9 @@
 Executable ExampleVM
    Main-Is: ExampleVM.hs
    ghc-options:       -threaded
+
+Executable Parser
+   Main-Is: Parser.hs
 
 Source-repository head
    Type:                 git
diff --git a/Parser.hs b/Parser.hs
new file mode 100644
--- /dev/null
+++ b/Parser.hs
@@ -0,0 +1,14 @@
+import NXT.Parser
+import NXT.Types
+
+import System.Environment
+
+main =
+    do args <- getArgs
+       case args of
+         [filename] -> parseFile filename
+         _ -> putStrLn "USAGE: ./Parser [filename]"
+
+parseFile f =
+    do r <- runFile pProg f
+       mapM_ (\v -> putStrLn (prettyFD v)) r