diff --git a/build/VS2010/HEVC_encoder.vcxproj b/build/VS2010/HEVC_encoder.vcxproj
index 6622f69f..d0e86ec8 100644
--- a/build/VS2010/HEVC_encoder.vcxproj
+++ b/build/VS2010/HEVC_encoder.vcxproj
@@ -83,6 +83,7 @@
     <ClCompile Include="..\..\src\config.c" />
     <ClCompile Include="..\..\src\encmain.c" />
     <ClCompile Include="..\..\src\encoder.c" />
+    <ClCompile Include="..\..\src\nal.c" />
     <ClCompile Include="..\..\src\picture.c" />
   </ItemGroup>
   <ItemGroup>
@@ -91,6 +92,7 @@
     <ClInclude Include="..\..\src\config.h" />
     <ClInclude Include="..\..\src\encoder.h" />
     <ClInclude Include="..\..\src\global.h" />
+    <ClInclude Include="..\..\src\nal.h" />
     <ClInclude Include="..\..\src\picture.h" />
   </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
diff --git a/build/VS2010/HEVC_encoder.vcxproj.filters b/build/VS2010/HEVC_encoder.vcxproj.filters
index dce2d423..dae96b0e 100644
--- a/build/VS2010/HEVC_encoder.vcxproj.filters
+++ b/build/VS2010/HEVC_encoder.vcxproj.filters
@@ -33,6 +33,9 @@
     <ClCompile Include="..\..\src\picture.c">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="..\..\src\nal.c">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="..\..\src\global.h">
@@ -53,5 +56,8 @@
     <ClInclude Include="..\..\src\picture.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="..\..\src\nal.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>
\ No newline at end of file
diff --git a/src/bitstream.c b/src/bitstream.c
index 32c697bc..212c234d 100644
--- a/src/bitstream.c
+++ b/src/bitstream.c
@@ -71,9 +71,8 @@ void bitstream_init(bitstream* stream)
 {
     stream->cur_byte=0;
     stream->cur_bit=0;
-    stream->buffer_pos = 0;
-    memset(stream->data, 0, sizeof(uint32_t)*32);
- 
+    stream->output = 0;
+    memset(stream->data, 0, sizeof(uint32_t)*32); 
 }
 
 /*
@@ -181,13 +180,13 @@ void bitstream_flush(bitstream* stream)
     if(stream->cur_byte)
     {
       memcpy(&stream->buffer[stream->buffer_pos],&stream->data[0],stream->cur_byte*4);
-      stream->buffer_pos = stream->cur_byte*4;
+      stream->buffer_pos += stream->cur_byte*4;
     }
    
    if(stream->cur_bit>>3)
    {
      memcpy(&stream->buffer[stream->buffer_pos],&stream->data[stream->cur_byte],stream->cur_bit>>3);
-     stream->buffer_pos = stream->cur_bit>>3;
+     stream->buffer_pos += stream->cur_bit>>3;
    }    
   }
     //Stream flushed, zero out the values
diff --git a/src/bitstream.h b/src/bitstream.h
index f72101cb..91a304bd 100644
--- a/src/bitstream.h
+++ b/src/bitstream.h
@@ -26,6 +26,8 @@ typedef struct
 }bitTable;
 
 extern bitTable *exp_table;
+
+int floorLog2(unsigned int n);
  
 void bitstream_alloc(bitstream* stream, uint32_t alloc); 
 void bitstream_init(bitstream* stream); 
@@ -45,8 +47,8 @@ void init_exp_golomb(uint32_t len);
 #ifdef _DEBUG
 static int WRITE_VALUE = 0;
 #define WRITE_U(stream, data, bits, name) { printf("%8d %40s u(%d) : %d\n",WRITE_VALUE, name,bits,data); bitstream_put(stream,data,bits); WRITE_VALUE++;}
-#define WRITE_UE(stream, data, name) { printf("%8d %40s ue(v) : %d\n",WRITE_VALUE, name,data); bitstream_put_ue(stream,data); WRITE_VALUE++;}
-#define WRITE_SE(stream, data, name) { printf("%8d %40s se(v) : %d\n",WRITE_VALUE, name,data); bitstream_put_se(stream,data); WRITE_VALUE++;}
+#define WRITE_UE(stream, data, name) { printf("%8d %40s ue(v): %d\n",WRITE_VALUE, name,data); bitstream_put_ue(stream,data); WRITE_VALUE++;}
+#define WRITE_SE(stream, data, name) { printf("%8d %40s se(v): %d\n",WRITE_VALUE, name,data); bitstream_put_se(stream,data); WRITE_VALUE++;}
 #else
 #define WRITE_U(stream, data, bits, name) { bitstream_put(stream,data,bits); }
 #define WRITE_UE(stream, data, name) { bitstream_put_ue(stream,data); }
diff --git a/src/encmain.c b/src/encmain.c
index 934e40ab..c2f7f697 100644
--- a/src/encmain.c
+++ b/src/encmain.c
@@ -108,6 +108,7 @@
 
     /* Init bitstream */
     bitstream_init(encoder->stream);
+    encoder->stream->buffer_pos = 0;
     bitstream_alloc(encoder->stream, 1024*1024);
 
     /* Config pointer to encoder struct */
diff --git a/src/encoder.c b/src/encoder.c
index dc14506b..021561de 100644
--- a/src/encoder.c
+++ b/src/encoder.c
@@ -23,6 +23,7 @@
 #include "encoder.h"
 #include "cabac.h"
 #include "picture.h"
+#include "nal.h"
 
 void init_encoder_control(encoder_control* control,bitstream* output) {control->stream = output;};
 void init_encoder_input(encoder_input* input,FILE* inputfile, uint32_t width, uint32_t height)
@@ -43,6 +44,122 @@ void init_encoder_input(encoder_input* input,FILE* inputfile, uint32_t width, ui
 
 void encode_one_frame(encoder_control* encoder)
 {
+  //output parameters before first frame
+  if(encoder->frame == 0)
+  {
+    encode_seq_parameter_set(encoder);
+    bitstream_align(encoder->stream);
+    bitstream_flush(encoder->stream);
+    nal_write(encoder->output, encoder->stream->buffer, encoder->stream->buffer_pos, 1, NAL_SEQ_PARAMETER_SET, 0);
+
+    encode_pic_parameter_set(encoder);
+    bitstream_align(encoder->stream);
+    bitstream_flush(encoder->stream);
+    nal_write(encoder->output, encoder->stream->buffer, encoder->stream->buffer_pos, 1, NAL_PIC_PARAMETER_SET, 0);
+    
+  }
+
+}
+
+void encode_pic_parameter_set(encoder_control* encoder)
+{
+#ifdef _DEBUG
+  printf("=========== Picture Parameter Set ID: 0 ===========\n");
+#endif
+  WRITE_UE(encoder->stream, 0, "pic_parameter_set_id");
+  WRITE_UE(encoder->stream, 0, "seq_parameter_set_id");
+
+  WRITE_U(encoder->stream, 0, 1, "sign_data_hiding_flag");
+  WRITE_U(encoder->stream, 0, 1, "cabac_init_present_flag");
+
+  WRITE_UE(encoder->stream, 0, "num_ref_idx_l0_default_active_minus1");
+  WRITE_UE(encoder->stream, 0, "num_ref_idx_l1_default_active_minus1");
+  WRITE_SE(encoder->stream, 0, "pic_init_qp_minus26");
+
+  WRITE_U(encoder->stream, 0, 1, "constrained_intra_pred_flag");
+  WRITE_U(encoder->stream, 0, 1, "enable_temporal_mvp_flag");
+
+  WRITE_U(encoder->stream, 0, 2, "slice_granularity");
+
+  WRITE_UE(encoder->stream, 0, "max_cu_qp_delta_depth");
+
+  WRITE_SE(encoder->stream, 0, "cb_qp_offset");
+  WRITE_SE(encoder->stream, 0, "cr_qp_offset");
+
+  WRITE_U(encoder->stream, 0, 1, "weighted_pred_flag");
+  WRITE_U(encoder->stream, 0, 2, "weighted_bipred_idc");
+  
+  WRITE_U(encoder->stream, 1, 1, "output_flag_present_flag");
+  
+  WRITE_U(encoder->stream, 0, 1, "deblocking_filter_control_present_flag");
+
+  WRITE_UE(encoder->stream, 0, "log2_parallel_merge_level_minus2");
+
+  WRITE_U(encoder->stream, 0, 1, "pps_extension_flag"); 
+  
+}
+
+void encode_seq_parameter_set(encoder_control* encoder)
+{
+#ifdef _DEBUG
+  printf("=========== Sequence Parameter Set ID: 0 ===========\n");
+#endif
+  WRITE_U(encoder->stream, 0, 8, "profile_idc");
+  WRITE_U(encoder->stream, 0, 8, "reserved_zero_8bits");
+  WRITE_U(encoder->stream, 0, 8, "level_idc");
+  WRITE_UE(encoder->stream, 0, "seq_parameter_set_id");
+  WRITE_UE(encoder->stream, 0, "chroma_format_idc"); /* 0 = 4:0:0, 1 = 4:2:0, 2 = 4:2:2, 3 = 4:4:4 */
+  WRITE_U(encoder->stream, 0, 3, "max_temporal_layers_minus1");
+  WRITE_UE(encoder->stream, encoder->in.width, "pic_width_in_luma_samples");
+  WRITE_UE(encoder->stream, encoder->in.height, "pic_height_in_luma_samples");
+  WRITE_U(encoder->stream, 0, 1, "pic_cropping_flag");
+
+  WRITE_UE(encoder->stream, 0, "bit_depth_luma_minus8");
+  WRITE_UE(encoder->stream, 0, "bit_depth_chroma_minus8");
+
+  WRITE_U(encoder->stream, 0, 1, "pcm_enabled_flag");
+
+  WRITE_U(encoder->stream, 0, 1, "qpprime_y_zero_transquant_bypass_flag");   
+
+  WRITE_UE(encoder->stream, 0, "log2_max_pic_order_cnt_lsb_minus4");
+
+  WRITE_UE(encoder->stream, 0, "max_dec_pic_buffering");
+  WRITE_UE(encoder->stream, 0, "num_reorder_pics");
+  WRITE_UE(encoder->stream, 0, "max_latency_increase");
+
+  WRITE_U(encoder->stream, 0, 1, "restricted_ref_pic_lists_flag");
+
+  WRITE_UE(encoder->stream, 0, "log2_min_coding_block_size_minus3");
+  WRITE_UE(encoder->stream, 3, "log2_diff_max_min_coding_block_size");
+  WRITE_UE(encoder->stream, 0, "log2_min_transform_block_size_minus2");
+  WRITE_UE(encoder->stream, 3, "log2_diff_max_min_transform_block_size");
+
+  WRITE_UE(encoder->stream, 2, "max_transform_hierarchy_depth_inter");
+  WRITE_UE(encoder->stream, 2, "max_transform_hierarchy_depth_intra");
 
 
-}
\ No newline at end of file
+  WRITE_U(encoder->stream, 0, 1, "scaling_list_enable_flag");
+  WRITE_U(encoder->stream, 0, 1, "chroma_pred_from_luma_enabled_flag");
+  WRITE_U(encoder->stream, 0, 1, "transform_skip_enabled_flag");
+	
+
+  WRITE_U(encoder->stream, 0, 1, "deblocking_filter_in_aps_enabled_flag");
+  WRITE_U(encoder->stream, 0, 1, "seq_loop_filter_across_slices_enabled_flag");
+  WRITE_U(encoder->stream, 0, 1, "asymmetric_motion_partitions_enabled_flag");
+  WRITE_U(encoder->stream, 0, 1, "nsrqt_enabled_flag");
+  WRITE_U(encoder->stream, 0, 1, "sample_adaptive_offset_enabled_flag");
+	WRITE_U(encoder->stream, 0, 1, "adaptive_loop_filter_enabled_flag");
+	
+  WRITE_U(encoder->stream, 0, 1, "temporal_id_nesting_flag");
+	
+  WRITE_UE(encoder->stream, 0, "num_short_term_ref_pic_sets");
+	
+  WRITE_U(encoder->stream, 0, 1, "long_term_ref_pics_present_flag");
+	
+  WRITE_U(encoder->stream, 0, 2, "tiles_or_entropy_coding_sync_idc");
+	
+	WRITE_U(encoder->stream, 0, 1, "sps_extension_flag");
+  
+}
+  
+
diff --git a/src/encoder.h b/src/encoder.h
index af6d77b1..66956d01 100644
--- a/src/encoder.h
+++ b/src/encoder.h
@@ -53,4 +53,8 @@ void init_encoder_control(encoder_control* control,bitstream* output);
 void init_encoder_input(encoder_input* input,FILE* inputfile, uint32_t width, uint32_t height);
 void encode_one_frame(encoder_control* encoder);
 
+
+void encode_seq_parameter_set(encoder_control* encoder);
+void encode_pic_parameter_set(encoder_control* encoder);
+
 #endif
\ No newline at end of file
diff --git a/src/nal.c b/src/nal.c
new file mode 100644
index 00000000..7ffb4ee9
--- /dev/null
+++ b/src/nal.c
@@ -0,0 +1,68 @@
+/**
+ *  HEVC Encoder
+ *  - Marko Viitanen ( fador at iki.fi ), Tampere University of Technology, Department of Computer Systems.
+ */
+
+/*! \file nal.c
+    \brief NAL
+    \author Marko Viitanen
+    \date 2012-06
+    
+    NAL functions
+*/
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "global.h"
+#include "config.h"
+#include "encoder.h"
+#include "cabac.h"
+#include "picture.h"
+#include "nal.h"
+
+void nal_write(FILE* output, uint8_t* buffer, uint32_t buffer_len, uint8_t nal_ref, uint8_t nal_type, uint8_t temporal_id)
+{
+  uint8_t byte;
+  uint32_t i;
+  uint8_t zerocount=0;
+  uint8_t emulation_prevention_three_byte = 0x03;
+  uint8_t start_code_prefix_one_3bytes = 0x01;
+  uint8_t zero = 0x00;
+  
+  //start_code_prefix_one_3bytes
+  fwrite(&zero, 1, 1, output);
+  fwrite(&zero, 1, 1, output);
+  fwrite(&zero, 1, 1, output);
+  fwrite(&start_code_prefix_one_3bytes, 1, 1, output);
+
+  //forbidden_zero_flag(1) + nal_ref_flag(1) + nal_unit_type(6)
+  byte = nal_ref<<6 | nal_type;
+  fwrite(&byte, 1, 1, output);
+
+  //Temporal_id(3) + reserved_one_5bits(5)
+  byte = temporal_id << 5 | 1;
+  fwrite(&byte, 1, 1, output);
+
+  /* Write out bytes and add emulation_prevention_three_byte when needed */
+  for(i = 0; i < buffer_len; i++)
+  {
+    if(zerocount == 2 && buffer[i] < 4) /* Prevent 0x0000 + 00/01/02/03 */
+    {
+      /* Inserting 0x03 */
+      fwrite(&emulation_prevention_three_byte, 1, 1, output);
+      zerocount = 0;
+    }
+    if(buffer[i] == 0)
+      zerocount++;
+    else
+      zerocount = 0;
+
+    fwrite(&buffer[i], 1, 1, output);
+  }
+  //If last byte was 0, add emulation_prevention_three_byte
+  if(buffer[buffer_len-1] == 0)
+    fwrite(&emulation_prevention_three_byte, 1, 1, output);
+
+}
\ No newline at end of file
diff --git a/src/nal.h b/src/nal.h
new file mode 100644
index 00000000..0ebe9610
--- /dev/null
+++ b/src/nal.h
@@ -0,0 +1,17 @@
+/**
+ *  HEVC Encoder
+ *  - Marko Viitanen ( fador at iki.fi ), Tampere University of Technology, Department of Computer Systems 2012.
+ */
+
+/*! \file nal.h
+    \brief NAL
+    \author Marko Viitanen
+    \date 2012-06
+    
+    NAL function headers
+*/
+
+#define NAL_SEQ_PARAMETER_SET 7
+#define NAL_PIC_PARAMETER_SET 8
+
+void nal_write(FILE* output, uint8_t* buffer, uint32_t buffer_len, uint8_t nal_ref, uint8_t nal_type, uint8_t temporal_id);
\ No newline at end of file