diff --git a/src/main.rs b/src/main.rs
index 5a2a4cb..6d0a457 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -389,7 +389,7 @@ fn process_file(
         } else if trimmed.starts_with("#ENDIF") || trimmed.starts_with("//ENDIF") {
             include.pop();
         // Trim #+ and //+
-        } else if *include.last().unwrap() {
+        } else if include.iter().all(|v| *v) {
             let mut line = line.to_string();
 
             if trimmed.starts_with("#+") {
@@ -431,3 +431,156 @@ fn process_options(args: &Args) {
         }
     }
 }
+
+#[cfg(test)]
+mod test {
+    use super::*;
+
+    #[test]
+    fn test_nested_if_else1() {
+        let res = process_file(
+            "/foo",
+            r#"
+        #IF option("opt1")
+        opt1
+        #IF option("opt2")
+        opt2
+        #ELSE
+        !opt2
+        #ENDIF
+        #ELSE
+        !opt1
+        #ENDIF
+        "#,
+            &["opt1".to_string(), "opt2".to_string()],
+            &[],
+        )
+        .unwrap();
+
+        assert_eq!(
+            r#"
+        opt1
+        opt2
+        "#
+            .trim(),
+            res.trim()
+        );
+    }
+
+    #[test]
+    fn test_nested_if_else2() {
+        let res = process_file(
+            "/foo",
+            r#"
+        #IF option("opt1")
+        opt1
+        #IF option("opt2")
+        opt2
+        #ELSE
+        !opt2
+        #ENDIF
+        #ELSE
+        !opt1
+        #ENDIF
+        "#,
+            &[],
+            &[],
+        )
+        .unwrap();
+
+        assert_eq!(
+            r#"
+        !opt1
+        "#
+            .trim(),
+            res.trim()
+        );
+    }
+
+    #[test]
+    fn test_nested_if_else3() {
+        let res = process_file(
+            "/foo",
+            r#"
+        #IF option("opt1")
+        opt1
+        #IF option("opt2")
+        opt2
+        #ELSE
+        !opt2
+        #ENDIF
+        #ELSE
+        !opt1
+        #ENDIF
+        "#,
+            &["opt1".to_string()],
+            &[],
+        )
+        .unwrap();
+
+        assert_eq!(
+            r#"
+        opt1
+        !opt2
+        "#
+            .trim(),
+            res.trim()
+        );
+    }
+
+    #[test]
+    fn test_nested_if_else4() {
+        let res = process_file(
+            "/foo",
+            r#"
+        #IF option("opt1")
+        #IF option("opt2")
+        opt2
+        #ELSE
+        !opt2
+        #ENDIF
+        opt1
+        #ENDIF
+        "#,
+            &["opt1".to_string()],
+            &[],
+        )
+        .unwrap();
+
+        assert_eq!(
+            r#"
+        !opt2
+        opt1
+        "#
+            .trim(),
+            res.trim()
+        );
+    }
+
+    #[test]
+    fn test_nested_if_else5() {
+        let res = process_file(
+            "/foo",
+            r#"
+        #IF option("opt1")
+        #IF option("opt2")
+        opt2
+        #ELSE
+        !opt2
+        #ENDIF
+        opt1
+        #ENDIF
+        "#,
+            &["opt2".to_string()],
+            &[],
+        )
+        .unwrap();
+
+        assert_eq!(
+            r#"
+        "#
+            .trim(),
+            res.trim()
+        );
+    }
+}
diff --git a/template/.cargo/config.toml b/template/.cargo/config.toml
index f9f8b2f..1ad5218 100644
--- a/template/.cargo/config.toml
+++ b/template/.cargo/config.toml
@@ -11,7 +11,7 @@ runner = "probe-rs run --chip=esp32c6"
 #IF option("probe-rs")
 DEFMT_LOG="info"
 #ELSE
-ESP_LOGLEVEL="INFO"
+ESP_LOG="INFO"
 #ENDIF
 
 [build]
diff --git a/template/.vscode/settings.json b/template/.vscode/settings.json
new file mode 100644
index 0000000..be03535
--- /dev/null
+++ b/template/.vscode/settings.json
@@ -0,0 +1,5 @@
+{
+    "rust-analyzer.cargo.allTargets": false,
+    //REPLACE riscv32imac-unknown-none-elf rust_target
+    "rust-analyzer.cargo.target": "riscv32imac-unknown-none-elf",
+}
\ No newline at end of file
diff --git a/template/Cargo.toml b/template/Cargo.toml
index f5e2c46..2bc733b 100644
--- a/template/Cargo.toml
+++ b/template/Cargo.toml
@@ -17,7 +17,7 @@ esp-backtrace = { version = "0.14.1", features = [
     #ENDIF
 ]}
 
-esp-hal = { version = "0.20.1", features = [
+esp-hal = { version = "0.21.0", features = [
     #REPLACE esp32c6 mcu
     "esp32c6",
     #IF option("probe-rs")
@@ -26,20 +26,25 @@ esp-hal = { version = "0.20.1", features = [
 ] }
 #IF !option("probe-rs")
 #REPLACE esp32c6 mcu
-esp-println = { version = "0.11.0", features = ["esp32c6", "log"] }
+esp-println = { version = "0.12.0", features = ["esp32c6", "log"] }
 log = { version = "0.4.21" }
 #ENDIF
 #IF option("alloc")
-esp-alloc = { version = "0.4.0" }
+esp-alloc = { version = "0.5.0" }
 #ENDIF
-#IF option("wifi")
+#IF option("wifi") || option("ble")
 embedded-io = "0.6.1"
-esp-wifi = { version = "0.9.1", default-features=false, features = [
+esp-wifi = { version = "0.10.1", default-features=false, features = [
     #REPLACE esp32c6 mcu
     "esp32c6",
     "phy-enable-usb",
     "utils",
+    #IF option("wifi")
     "wifi",
+    #ENDIF
+    #IF option("ble")
+    "ble",
+    #ENDIF
     "esp-alloc",
     #IF option("probe-rs")
     #+"defmt",
@@ -62,21 +67,6 @@ smoltcp = { version = "0.11.0", default-features = false, features = [
 ] }
 #ENDIF
 #IF option("ble")
-#+ embedded-io = "0.6.1"
-#+ esp-wifi = { version = "0.9.1", default-features=false, features = [
-#REPLACE esp32c6 mcu
-#+     "esp32c6",
-#+     "phy-enable-usb",
-#+     "ble",
-#+     "esp-alloc",
-#IF option("probe-rs")
-#+     #+"defmt",
-#ENDIF
-#IF !option("probe-rs")
-#+     "log",
-#ENDIF
-#+ ] }
-#+ heapless = { version = "0.8.0", default-features = false }
 #+ bleps = { git = "https://github.com/bjoernQ/bleps", package = "bleps", rev = "a5148d8ae679e021b78f53fd33afb8bb35d0b62e", features = [ "macros", "async"] }
 #ENDIF
 #IF option("probe-rs")
@@ -91,7 +81,7 @@ embassy-executor = { version = "0.6.0",  features = [
 ] }
 embassy-time     = { version = "0.3.1",  features = ["generic-queue-8"] }
 #REPLACE esp32c6 mcu
-esp-hal-embassy  = { version = "0.3.0",  features = ["esp32c6"] }
+esp-hal-embassy  = { version = "0.4.0",  features = ["esp32c6"] }
 static_cell      = { version = "2.1.0",  features = ["nightly"] }
 #ENDIF
 
@@ -108,9 +98,3 @@ incremental = false
 lto = 'fat'
 opt-level = 's'
 overflow-checks = false
-
-[patch.crates-io]
-esp-hal = { git = "https://github.com/esp-rs/esp-hal.git", rev = "be82a6521a5859129ca4f98e54e4d4d6cedffe76"}
-esp-wifi = { git = "https://github.com/esp-rs/esp-hal.git", rev = "be82a6521a5859129ca4f98e54e4d4d6cedffe76"}
-esp-hal-embassy = { git = "https://github.com/esp-rs/esp-hal.git", rev = "be82a6521a5859129ca4f98e54e4d4d6cedffe76"}
-esp-alloc = { git = "https://github.com/esp-rs/esp-hal.git", rev = "be82a6521a5859129ca4f98e54e4d4d6cedffe76"}
diff --git a/template/rust-toolchain.toml b/template/rust-toolchain.toml
index 2e7fa0f..1d0f5c5 100644
--- a/template/rust-toolchain.toml
+++ b/template/rust-toolchain.toml
@@ -1,10 +1,9 @@
 [toolchain]
 #IF option("riscv")
-#IF !option("stack-protector")
-channel = "stable"
-#ENDIF
 #IF option("stack-protector")
-#+channel = "nightly"
+channel = "nightly"
+#ELSE
+#+channel = "stable"
 #ENDIF
 components = ["rust-src"]
 #REPLACE riscv32imac-unknown-none-elf rust_target
diff --git a/template/src/bin/async_main.rs b/template/src/bin/async_main.rs
index 4f0788f..b5155b8 100644
--- a/template/src/bin/async_main.rs
+++ b/template/src/bin/async_main.rs
@@ -39,10 +39,17 @@ async fn main(spawner: Spawner) {
     esp_hal_embassy::init(timg0.timer0);
     info!("Embassy initialized!");
 
-    //IF option("wifi")
+    //IF option("wifi") || option("ble")
+    let peripherals = esp_hal::init(esp_hal::Config::default());
+
     let timg1 = esp_hal::timer::timg::TimerGroup::new(peripherals.TIMG1);
-    let _init = esp_wifi::initialize(
+    let _init = esp_wifi::init(
+        //IF option("wifi")
         esp_wifi::EspWifiInitFor::Wifi,
+        //ENDIF
+        //IF option("ble")
+        //+esp_wifi::EspWifiInitFor::Ble,
+        //ENDIF
         timg1.timer0,
         esp_hal::rng::Rng::new(peripherals.RNG),
         peripherals.RADIO_CLK,
@@ -50,19 +57,6 @@ async fn main(spawner: Spawner) {
     .unwrap();
     //ENDIF
 
-    //IF option("ble")
-    //+ let peripherals = esp_hal::init(esp_hal::Config::default());
-    //+
-    //+ let timg1 = esp_hal::timer::timg::TimerGroup::new(peripherals.TIMG1);
-    //+ let _init = esp_wifi::initialize(
-    //+         esp_wifi::EspWifiInitFor::Ble,
-    //+ timg1.timer0,
-    //+ esp_hal::rng::Rng::new(peripherals.RNG),
-    //+ peripherals.RADIO_CLK,
-    //+ )
-    //+ .unwrap();
-    //ENDIF
-
     // TODO: Spawn some tasks
     let _ = spawner;
 
diff --git a/template/src/bin/main.rs b/template/src/bin/main.rs
index 8efbdcd..4aee947 100644
--- a/template/src/bin/main.rs
+++ b/template/src/bin/main.rs
@@ -32,12 +32,17 @@ fn main() -> ! {
     esp_alloc::heap_allocator!(72 * 1024);
     //ENDIF
 
-    //IF option("wifi")
+    //IF option("wifi") || option("ble")
     let peripherals = esp_hal::init(esp_hal::Config::default());
 
     let timg0 = TimerGroup::new(peripherals.TIMG0);
-    let _init = esp_wifi::initialize(
+    let _init = esp_wifi::init(
+        //IF option("wifi")
         esp_wifi::EspWifiInitFor::Wifi,
+        //ENDIF
+        //IF option("ble")
+        //+esp_wifi::EspWifiInitFor::Ble,
+        //ENDIF
         timg0.timer0,
         esp_hal::rng::Rng::new(peripherals.RNG),
         peripherals.RADIO_CLK,
@@ -45,19 +50,6 @@ fn main() -> ! {
     .unwrap();
     //ENDIF
 
-    //IF option("ble")
-    //+ let peripherals = esp_hal::init(esp_hal::Config::default());
-    //+
-    //+ let timg0 = TimerGroup::new(peripherals.TIMG0);
-    //+ let _init = esp_wifi::initialize(
-    //+         esp_wifi::EspWifiInitFor::Ble,
-    //+ timg0.timer0,
-    //+ esp_hal::rng::Rng::new(peripherals.RNG),
-    //+ peripherals.RADIO_CLK,
-    //+ )
-    //+ .unwrap();
-    //ENDIF
-
     let delay = Delay::new();
     loop {
         info!("Hello world!");