diff --git a/sim/avr/interrupt.c b/sim/avr/interrupt.c
index 2bcbe0d..f03c29b 100644
--- a/sim/avr/interrupt.c
+++ b/sim/avr/interrupt.c
@@ -22,12 +22,14 @@
 
 #include "interrupt.h"
 #include "io.h"
+#include "wdt.h"
 
-//pseudo-Interrupt vector table  
+//pseudo-Interrupt vector table
 isr_fp compa_vect[6]={0};
 isr_fp compb_vect[6]={0};
 isr_fp ovf_vect[6]={0};
-
+isr_fp wdt_vect = 0;
+isr_fp pc_vect = 0;
 
 void sei() {io.sreg|=SEI;}
 void cli() {io.sreg&=~SEI;}
@@ -45,91 +47,108 @@ enum sim_wgm_mode {
   wgm_RESERVED
 };
 
-enum sim_wgm_mode sim_wgm0[4] = {wgm_NORMAL,wgm_PHASE_PWM,wgm_CTC,wgm_FAST_PWM};
-enum sim_wgm_mode sim_wgmN[8] = {wgm_NORMAL,wgm_PHASE_PWM,wgm_PHASE_PWM,wgm_PH_F_PWM,
-											wgm_CTC, wgm_FAST_PWM, wgm_FAST_PWM, wgm_FAST_PWM};
+//3-bit wgm table for 8-bit timers
+enum sim_wgm_mode sim_wgm_3[] = {wgm_NORMAL,wgm_PHASE_PWM,wgm_CTC,wgm_FAST_PWM,
+                                 wgm_RESERVED,wgm_PHASE_PWM, wgm_RESERVED, wgm_FAST_PWM};
+
+//4-bit wgm modes for 16-bit timers
+enum sim_wgm_mode sim_wgm_4[16] = {wgm_NORMAL,wgm_PHASE_PWM,wgm_PHASE_PWM,wgm_PHASE_PWM,
+                                   wgm_CTC, wgm_FAST_PWM, wgm_FAST_PWM, wgm_FAST_PWM,
+                                   wgm_PH_F_PWM, wgm_PH_F_PWM, wgm_PHASE_PWM, wgm_PHASE_PWM,
+                                   wgm_CTC, wgm_RESERVED, wgm_FAST_PWM, wgm_FAST_PWM};
+
+static const uint16_t timer_bitdepth[SIM_N_TIMERS] = {
+  0xFF,0xFFFF,0xFF,
+  //0xFFFF,0xFFFF,0xFFFF   3 more for mega
+};
 
 void timer_interrupts() {
   int i;
   uint8_t ien = io.sreg&SEI;  //interrupts enabled?
   io.prescaler++;
- 
+
   //all clocks
-  for (i=0;i<2;i++){
+  for (i=0;i<SIM_N_TIMERS;i++){
+    uint8_t cs = io.tccrb[i]&7; //clock select bits
+    int16_t increment = sim_scaling[cs];
+    uint16_t bitmask = timer_bitdepth[i];
 
-	 uint8_t cs = io.tccrb[i]&7; //clock select bits 
-	 int16_t increment = sim_scaling[cs];
-	 //check scaling to see if timer fires
-	 if (increment && (io.prescaler&(increment-1))==0) { 
+    //check scaling to see if timer fires
+    if (increment && (io.prescaler&(increment-1))==0) { 
 
-		//select waveform generation mode 
-		enum sim_wgm_mode mode;
-		if (i==0 || i==2) {  //(T0 and T2 are different from rest)
-		  uint8_t wgm = io.tccra[i]&3; //look at low 2 bits
-		  mode = sim_wgm0[wgm];
-		}
-		else {
-		  uint8_t wgm = ((io.tccrb[i]&8)>>1) | (io.tccra[i]&3); //only using 3 bits for now
-		  mode = sim_wgmN[wgm];
-		}
-		
-		//tick
-		io.tcnt[i]++;
-		//comparators
-		if ((io.timsk[i]&(1<<SIM_OCA)) && io.tcnt[i]==io.ocra[i]) io.tifr[i]|=(1<<SIM_OCA);
-		if ((io.timsk[i]&(1<<SIM_OCB)) && io.tcnt[i]==io.ocrb[i]) io.tifr[i]|=(1<<SIM_OCB);
-		if ((io.timsk[i]&(1<<SIM_OCC)) && io.tcnt[i]==io.ocrc[i]) io.tifr[i]|=(1<<SIM_OCC);
-		  
+    //select waveform generation mode
+    enum sim_wgm_mode mode;
+    if (i==0 || i==2) {  //(T0 and T2 use only 3 wgm bits)
+      uint8_t wgm = ((io.tccrb[i]&0x08)>>1) | (io.tccra[i]&3);
+      mode = sim_wgm_3[wgm];
+    }
+    else {
+      uint8_t wgm = ((io.tccrb[i]&0x18)>>1) | (io.tccra[i]&3);  //4 wgm bits
+      mode = sim_wgm_4[wgm];
+    }
 
-		switch (mode) {
-		  case wgm_NORMAL: //Normal mode
-			 if (i==0) io.tcnt[i]&=0xFF; //timer0 is 8 bit;
-			 if (i==2) io.tcnt[i]&=0xFF; //timer2 is 8 bit;
-			 if (io.tcnt[i]==0) io.tifr[i]|=(1<<SIM_TOV); 
-			 break;
+    //tick
+    if (io.tifr[i]&(1<<SIM_ROLL)) {  //handle CTC mode rollover
+      io.tcnt[i]=0;
+      io.tifr[i]&=~(1<<SIM_ROLL);
+    }
+    else {
+      io.tcnt[i]++;
+    }
+    io.tcnt[i]&=bitmask; //limit the 8 bit timers
 
-   	  case wgm_CTC: //CTC mode
-			 if (io.tcnt[i]==io.ocra[i]) io.tcnt[i]=0;
-			 break;
-  		  default:  //unsupported
-			 break; 
-		}
-		//call any triggered interupts
-		if (ien && io.tifr[i]) {
-		  if (compa_vect[i] && (io.tifr[i]&(1<<SIM_OCA))) {
-			 compa_vect[i]();
-			 io.tifr[i]&=~(1<<SIM_OCA);
-			 //TODO: insert port_monitor call here
-		  }
-		  if (compb_vect[i] && (io.tifr[i]&(1<<SIM_OCB))) {
-			 compb_vect[i]();
-			 io.tifr[i]&=~(1<<SIM_OCB);
-		  }
-		  if (ovf_vect[i] && (io.tifr[i]&(1<<SIM_TOV))) {
-			 ovf_vect[i]();
-			 io.tifr[i]&=~(1<<SIM_TOV);
-		  }
-		}
-	 }
+    switch (mode) {
+      case wgm_NORMAL: //Normal mode, ovf on rollover
+        if (io.tcnt[i]==0) io.tifr[i]|=(1<<SIM_TOV);  //overflow
+       break;
+
+        case wgm_CTC: //CTC mode, ovf at TOP, 0 next tick
+        if (io.tcnt[i]==(io.ocra[i]&bitmask)) {
+          io.tifr[i]|=(1<<SIM_TOV)|(1<<SIM_ROLL);  //overflow
+        }
+       break;
+        default:  //unsupported
+       break;
+    }
+
+    //comparators
+    if ((io.timsk[i]&(1<<SIM_OCA)) && io.tcnt[i]==(io.ocra[i]&bitmask)) io.tifr[i]|=(1<<SIM_OCA);
+    if ((io.timsk[i]&(1<<SIM_OCB)) && io.tcnt[i]==(io.ocrb[i]&bitmask)) io.tifr[i]|=(1<<SIM_OCB);
+    if ((io.timsk[i]&(1<<SIM_OCC)) && io.tcnt[i]==(io.ocrc[i]&bitmask)) io.tifr[i]|=(1<<SIM_OCC);
+
+    //call any triggered interupts
+    if (ien && io.tifr[i]) {
+      if (compa_vect[i] && (io.tifr[i]&(1<<SIM_OCA))) {
+       compa_vect[i]();
+       io.tifr[i]&=~(1<<SIM_OCA);
+       //TODO: insert port_monitor call here
+      }
+      if (compb_vect[i] && (io.tifr[i]&(1<<SIM_OCB))) {
+       compb_vect[i]();
+       io.tifr[i]&=~(1<<SIM_OCB);
+      }
+      if (ovf_vect[i] && (io.tifr[i]&(1<<SIM_TOV))) {
+       ovf_vect[i]();
+       io.tifr[i]&=~(1<<SIM_TOV);
+      }
+    }
+   }
   }
-	 //// TODO for more complete timer sim. 
-	 // pwm modes. (only used for variable spindle, I think).
+   //// TODO for more complete timer sim.
+   // pwm modes. (only used for variable spindle, I think).
     // -- would require fixing wgm mode for Timers1..5
     // -- phase correct modes need updown counter.
     // output pins (also only for variable spindle, I think).
 
     //// Other chip features not needed yet for grbl:
-	 // writes to TCNT0 prevent compare match (need write detector.)
+   // writes to TCNT0 prevent compare match (need write detector.)
     // force output compare (unused)
     // input capture (unused and how would we signal it?)
     // define the other output compare registers.
-	 // usercode can clear unhandled interrupt flags by writing 1. 
+   // usercode can clear unhandled interrupt flags by writing 1.
     //  --(this may be impossible, since bit was 1 before the write.)
     // prescaler reset.
     // maybe need to cli on interrupt entry
-  
+
 }
 
-
-
-
diff --git a/sim/avr/interrupt.h b/sim/avr/interrupt.h
index 4033857..2716bcf 100644
--- a/sim/avr/interrupt.h
+++ b/sim/avr/interrupt.h
@@ -28,12 +28,14 @@
 //#define TIMER1_COMPA_vect
 #define ISR(a) void interrupt_ ## a ()
 
-// Stub of the timer interrupt functions we need
+// Stubs of the hardware interrupt functions we are using
 void interrupt_TIMER0_COMPA_vect();
 void interrupt_TIMER1_COMPA_vect();
 void interrupt_TIMER0_OVF_vect();
 void interrupt_SERIAL_UDRE();
 void interrupt_SERIAL_RX();
+void interrupt_LIMIT_INT_vect();
+void interrupt_WDT_vect();
 
 
 //pseudo-Interrupt vector table  
@@ -41,7 +43,8 @@ typedef void(*isr_fp)(void);
 extern isr_fp compa_vect[6];
 extern isr_fp compb_vect[6];
 extern isr_fp ovf_vect[6];
-
+extern isr_fp wdt_vect;
+extern isr_fp pc_vect; //pin change
 
 // enable interrupts now does something in the simulation environment
 #define SEI 0x80
@@ -52,5 +55,4 @@ void cli();
 void timer_interrupts();
 
 
-
 #endif
diff --git a/sim/avr/io.h b/sim/avr/io.h
index df8a515..29332e4 100644
--- a/sim/avr/io.h
+++ b/sim/avr/io.h
@@ -4,7 +4,7 @@
 
   Part of Grbl Simulator
 
-  Copyright (c) 2012 Jens Geisler
+  Copyright (c) 2012-2104 Jens Geisler, Adam Shelly
 
   Grbl is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -40,7 +40,7 @@ enum {
   SIM_PORT_COUNT
 };
 
-#define SIM_N_TIMERS 6
+#define SIM_N_TIMERS 3 //328p has 3, Mega has 6
 
 
 // dummy register variables
@@ -60,12 +60,14 @@ typedef struct io_sim {
   uint8_t pcmsk[3];
   uint8_t ucsr0[3];
   uint8_t udr[3];
+  uint8_t gpior[3];
+  uint8_t mcusr;
+  uint8_t wdtcsr;
   union hilo16 ubrr0;
 
   uint16_t prescaler; //continuously running
   uint8_t sreg;
 
-				  
 } io_sim_t;
 volatile extern io_sim_t io;
 
@@ -94,6 +96,8 @@ volatile extern io_sim_t io;
 #define DDRG io.ddr[SIM_G]
 #define DDRH io.ddr[SIM_H]
 #define DDRJ io.ddr[SIM_J]
+#define DDRK io.ddr[SIM_K]
+#define DDRL io.ddr[SIM_L]
 
 #define PINA io.pin[SIM_A]
 #define PINB io.pin[SIM_B]
@@ -121,6 +125,7 @@ volatile extern io_sim_t io;
 #define SIM_OCB 2
 #define SIM_OCC 3
 #define SIM_ICI 5
+#define SIM_ROLL 7  //stealing reserved TIFR bit
 
 #define OCIE0A SIM_OCA
 #define OCIE0B SIM_OCB
@@ -148,8 +153,8 @@ volatile extern io_sim_t io;
 #define TCNT1  io.tcnt[1]
 #define TCNT2  io.tcnt[2]
 
-#define TCCR0B io.tccra[0]
-#define TCCR0A io.tccrb[0]
+#define TCCR0A io.tccra[0]
+#define TCCR0B io.tccrb[0]
 #define TCCR1A io.tccra[1]
 #define TCCR1B io.tccrb[1]
 #define TCCR2A io.tccra[2]
@@ -179,6 +184,7 @@ volatile extern io_sim_t io;
 #define PCICR io.pcicr
 #define PCIE0 0
 #define PCIE1 1
+#define PCIE2 2
 
 //serial channel
 #define UCSR0A io.ucsr0[SIM_A]
@@ -196,6 +202,23 @@ volatile extern io_sim_t io;
 #define PCMSK1 io.pcmsk[1]
 #define PCMSK2 io.pcmsk[2]
 
+//GPIO
+#define GPIOR0 io.gpior[0]
+#define GPIOR1 io.gpior[1]
+#define GPIOR2 io.gpior[2]
+
+//MCU Status
+#define MCUSR io.mcusr
+
+#define PORF 0
+#define EXTRF 1
+#define BORF 2
+#define WDRF 3
+#define JTRF 4
+
+//Interrupt Status
+#define SREG io.sreg
+
 
 
 #endif
diff --git a/sim/avr/wdt.h b/sim/avr/wdt.h
index 482f4fd..0df7bcb 100644
--- a/sim/avr/wdt.h
+++ b/sim/avr/wdt.h
@@ -1 +1,11 @@
+#define WDTCSR wdt
+#define WDP0 0
+#define WDP1 1
+#define WDP2 2
+#define WDE  3
+#define WDCE 4
+#define WDP3 5
+#define WDIE 6
+#define WDIF 7
+
 uint16_t wdt;
diff --git a/sim/simulator.c b/sim/simulator.c
index 87d85bb..3364477 100644
--- a/sim/simulator.c
+++ b/sim/simulator.c
@@ -4,7 +4,7 @@
 
   Part of Grbl Simulator
 
-  Copyright (c) 2012 Jens Geisler
+  Copyright (c) 2012-2014 Jens Geisler, Adam Shelly
 
   Grbl is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -32,7 +32,7 @@
 #include "eeprom.h"
 
 
-int block_position[]= {0,0,0}; //step count after most recently planned block
+int block_position[N_AXIS]= {0}; //step count after most recently planned block
 uint32_t block_number= 0;
 
 sim_vars_t sim={0};
@@ -50,6 +50,11 @@ void init_simulator(float time_multiplier) {
 #ifdef STEP_PULSE_DELAY
   compa_vect[0] = interrupt_TIMER0_COMPA_vect;
 #endif
+#ifdef ENABLE_SOFTWARE_DEBOUNCE
+  wdt_vect = interrupt_WDT_vect;
+#endif
+  pc_vect = interrupt_LIMIT_INT_vect;
+
 
   sim.next_print_time = args.step_time;
   sim.speedup = time_multiplier;
@@ -94,43 +99,43 @@ void sim_loop(){
 
   while (!sim.exit || sys.state>2 ) { //don't quit until idle
 
-	 if (sim.speedup) {
-		//calculate how many ticks to do.
-		uint32_t ns_now = platform_ns();
-		uint32_t ns_elapsed = (ns_now-ns_prev)*sim.speedup; //todo: try multipling nsnow
-		simulated_ticks += F_CPU/1e9*ns_elapsed;
-		ns_prev = ns_now;
-	 }
-	 else {
-		simulated_ticks++;  //as fast as possible
-	 }
-		
-	 while (sim.masterclock < simulated_ticks){
+   if (sim.speedup) {
+    //calculate how many ticks to do.
+    uint32_t ns_now = platform_ns();
+    uint32_t ns_elapsed = (ns_now-ns_prev)*sim.speedup; //todo: try multipling nsnow
+    simulated_ticks += F_CPU/1e9*ns_elapsed;
+    ns_prev = ns_now;
+   }
+   else {
+    simulated_ticks++;  //as fast as possible
+   }
+    
+   while (sim.masterclock < simulated_ticks){
 
-		//only read serial port as fast as the baud rate allows
-		bool read_serial = (sim.masterclock >= next_byte_tick);
+    //only read serial port as fast as the baud rate allows
+    bool read_serial = (sim.masterclock >= next_byte_tick);
 
-		//do low level hardware
-		simulate_hardware(read_serial);
+    //do low level hardware
+    simulate_hardware(read_serial);
 
-		//print the steps. 
-		//For further decoupling, could maintain own counter of STEP_PORT pulses, 
+    //print the steps. 
+    //For further decoupling, could maintain own counter of STEP_PORT pulses, 
       // print that instead of sys.position.
-		print_steps(0);  
-		
-		if (read_serial){
-		  next_byte_tick+=sim.baud_ticks;
-		  //recent block can only change after input, so check here.
-		  printBlock();
-		}
+    print_steps(0);  
+    
+    if (read_serial){
+      next_byte_tick+=sim.baud_ticks;
+      //recent block can only change after input, so check here.
+      printBlock();
+    }
 
-		//TODO:
-		//  set limit pins based on position,
-		//  set probe pin when probing.
-		//  if VARIABLE_SPINDLE, measure pwm pin to report speed?
-	 }
+    //TODO:
+    //  set limit pins based on position,
+    //  set probe pin when probing.
+    //  if VARIABLE_SPINDLE, measure pwm pin to report speed?
+   }
 
-	 platform_sleep(0); //yield
+   platform_sleep(0); //yield
   }
 }
 
@@ -143,22 +148,22 @@ void print_steps(bool force)
 
   if (sim.next_print_time == 0.0) { return; }  //no printing
   if (current_block != printed_block ) {
-	 //new block. 
-	 if (block_number) { //print values from the end of prev block
-		fprintf(args.step_out_file, "%20.15f %d, %d, %d\n", sim.sim_time, sys.position[X_AXIS], sys.position[Y_AXIS], sys.position[Z_AXIS]);
-	 }
-	 printed_block = current_block;
-	 if (current_block == NULL) { return; }
-	 // print header
-	 fprintf(args.step_out_file, "# block number %d\n", block_number++);
+   //new block. 
+   if (block_number) { //print values from the end of prev block
+    fprintf(args.step_out_file, "%20.15f %d, %d, %d\n", sim.sim_time, sys.position[X_AXIS], sys.position[Y_AXIS], sys.position[Z_AXIS]);
+   }
+   printed_block = current_block;
+   if (current_block == NULL) { return; }
+   // print header
+   fprintf(args.step_out_file, "# block number %d\n", block_number++);
   }
   //print at correct interval while executing block
   else if ((current_block && sim.sim_time>=sim.next_print_time) || force ) {
-	 fprintf(args.step_out_file, "%20.15f %d, %d, %d\n", sim.sim_time, sys.position[X_AXIS], sys.position[Y_AXIS], sys.position[Z_AXIS]);
-	 fflush(args.step_out_file);
+   fprintf(args.step_out_file, "%20.15f %d, %d, %d\n", sim.sim_time, sys.position[X_AXIS], sys.position[Y_AXIS], sys.position[Z_AXIS]);
+   fflush(args.step_out_file);
 
-	 //make sure the simulation time doesn't get ahead of next_print_time
-	 while (sim.next_print_time<=sim.sim_time) sim.next_print_time += args.step_time;
+   //make sure the simulation time doesn't get ahead of next_print_time
+   while (sim.next_print_time<=sim.sim_time) sim.next_print_time += args.step_time;
   }
 }
 
@@ -189,15 +194,15 @@ void printBlock() {
 
   b= plan_get_recent_block();
   if(b!=last_block && b!=NULL) {
-	 int i;
-	 for (i=0;i<N_AXIS;i++){
-		if(b->direction_bits & get_direction_mask(i)) block_position[i]-= b->steps[i];
-		else block_position[i]+= b->steps[i];
-		fprintf(args.block_out_file,"%d, ", block_position[i]);
-	 }
-	 fprintf(args.block_out_file,"%f", b->entry_speed_sqr);
-	 fprintf(args.block_out_file,"\n");
-	 fflush(args.block_out_file); //TODO: needed?
+   int i;
+   for (i=0;i<N_AXIS;i++){
+    if(b->direction_bits & get_direction_mask(i)) block_position[i]-= b->steps[i];
+    else block_position[i]+= b->steps[i];
+    fprintf(args.block_out_file,"%d, ", block_position[i]);
+   }
+   fprintf(args.block_out_file,"%f", b->entry_speed_sqr);
+   fprintf(args.block_out_file,"\n");
+   fflush(args.block_out_file); //TODO: needed?
 
     last_block= b;
   }
@@ -212,12 +217,12 @@ void grbl_out(uint8_t data){
 
   buf[len++]=data;
   if(data=='\n' || data=='\r' || len>=127) {
-	 if (args.comment_char && !continuation){
-		fprintf(args.grbl_out_file,"%c ",args.comment_char);
-	 }
-	 buf[len]=0;
-	 fprintf(args.grbl_out_file,"%s",buf);
-	 continuation = (len>=128); //print comment on next line unless we are only printing to avoid buffer overflow)
-	 len=0;
+   if (args.comment_char && !continuation){
+    fprintf(args.grbl_out_file,"%c ",args.comment_char);
+   }
+   buf[len]=0;
+   fprintf(args.grbl_out_file,"%s",buf);
+   continuation = (len>=128); //print comment on next line unless we are only printing to avoid buffer overflow)
+   len=0;
   }
 }