830 changed files with 77043 additions and 263738 deletions
--- a/.cmake-format.py
+++ b/.cmake-format.py
@ -1,21 +0,0 @@
-# If a statement is wrapped to more than one line, than dangle the closing
-# parenthesis on it's own line.
-dangle_parens = True
-dangle_align = 'child'
-
-# If true, the parsers may infer whether or not an argument list is sortable
-# (without annotation).
-autosort = True
-
-# How wide to allow formatted cmake files
-line_width = 100
-
-additional_commands = {
-    "target_sources": {
-        "kwargs": {
-            "PUBLIC": "*",
-            "PRIVATE": "*",
-            "INTERFACE": "*",
-        }
-    },
-}
--- a/.editorconfig
+++ b/.editorconfig
@ -15,3 +15,4 @@ max_line_length             = 100

 [lang/po/*.po]
 end_of_line                 = crlf
+trim_trailing_whitespace    = false
--- a/.gitattributes
+++ b/.gitattributes
@ -1,2 +1 @@
-lang/po/*.po text eol=crlf diff=po
-lang/po/*.pot text diff=po
+lang/po/*.po text eol=crlf
--- a/.github/ISSUE_TEMPLATE/bug_report.md
+++ b/.github/ISSUE_TEMPLATE/bug_report.md
@ -0,0 +1,34 @@
+---
+name: Bug report
+about: Create a report to help us improve
+title: "[BUG]<Enter comprehensive title>"
+labels: bug
+assignees: ''
+
+---
+
+Please, before you create a new bug report, please make sure you searched in open and closed issues and couldn't find anything that matches.
+
+**Printer type** - [e.g. MK3S, MK3, MK2.5S, MK2.5, MK2S, MK2]
+**Printer firmware version**-  [e.g. 3.8.1, 3.8.1-RC1, ...]
+
+**MMU Upgrade** - [e.g. MMU2S, MMU2, MMU1]
+**MMU upgrade firmware version [e.g. 1.0.6, 1.0.6-RC2, ...]
+
+**SD card or USB/Octoprint**
+  Please let us know if you print via SD card or USB/Octoprint
+
+**Describe the bug**
+  A clear and concise description of what the bug is.
+  
+**To Reproduce**
+  Please describe steps to reproduce the behavior.
+  
+**Expected behavior**
+  A clear and concise description of what you expected to happen.
+  
+**G-code**
+  Please attach a G-code. This will make it easier for us to replicate the error.
+
+**Video**
+  Please attach a video. It usually helps to solve the problem.
--- a/.github/ISSUE_TEMPLATE/bug_report.yml
+++ b/.github/ISSUE_TEMPLATE/bug_report.yml
@ -1,96 +0,0 @@
-name: Bug report
-description: File a bug report
-title: '[BUG] '
-labels:
-  - bug
-body:
-  - type: markdown
-    attributes:
-      value: |
-        Before you create a new bug report, please check if an issue with it already exists (either open or closed) by using the search bar on the issues page. If it does, comment there. Even if it's closed, we can reopen it based on your comment.
-  - type: dropdown
-    id: printer_model
-    attributes:
-      label: Printer model
-      description: Enter the printer model(s) where you encountered the issue
-      options:
-        - MK3S/+
-        - MK3
-        - MK2.5S
-        - MK2.5
-    validations:
-      required: true
-  - type: dropdown
-    id: mmu_model
-    attributes:
-      label: MMU model
-      description: Enter the MMU model(s) where you encountered the issue
-      multiple: false
-      options:
-        - No-MMU
-        - MMU3
-        - MMU2S
-    validations:
-      required: true
-  - type: input
-    id: firmware_version
-    attributes:
-      label: Firmware versions
-      description: |
-        If you're using a custom firmware (not downloaded from Prusa), please note that!
-      placeholder: '3.14.0, 3.12.0-RC1, 3.14.0 and 3.0.2...'
-    validations:
-      required: true
-  - type: input
-    id: optional_upgrades
-    attributes:
-      label: Upgrades and modifications
-      description: 'If your printer has upgrades or was modified, please note that!  If not, please write None or leave blank.'
-      placeholder: |
-        None, custom extruder/hotend (which), different frame, ...
-  - type: dropdown
-    id: printing_from
-    attributes:
-      label: Printing from...
-      multiple: false
-      options:
-        - SD Card
-        - PrusaLink
-        - PrusaConnect
-        - OctoPrint
-        - Other host software
-    validations:
-      required: true
-  - type: textarea
-    id: description
-    attributes:
-      label: Describe the bug
-      description: Write a concise description of the bug.
-    validations:
-      required: true
-  - type: textarea
-    id: reproduction
-    attributes:
-      label: How to reproduce
-      description: |
-        If you are able to reproduce the issue - meaning that you can trigger it yourself by following certain steps - please describe the step-by-step process in as much detail as possible!
-  - type: textarea
-    id: expected_behavior
-    attributes:
-      label: Expected behavior
-      description: |
-        If the printer did something unexpected, or if the procedure is missing a step that would resolve the issue, please provide a step-by-step process of how it should behave.
-  - type: textarea
-    id: files
-    attributes:
-      label: Files
-      description: |
-        Provide at least one of these (ideally as many as you can) to help developers track down the bug.
-        *To upload files, compress them into a .zip file and drag them to the text area to upload.* Images and videos can be uploaded directly.
-
-        - **gcode** file
-        - **3mf project** which includes the model and printer settings
-        - **video** or **photos** of the issue happening
-
-        **Crash dump: Do not share the file publicly**, as the crash dump contains a raw snapshot of the printer's memory and may include unencrypted sensitive information.
-        Please send the crash dump file to Prusa by emailing it to reports@prusa3d.com and referencing this issue. In reply to the email, you will be automatically assigned a reference code to be mentioned in the issue. Sharing this file is important and helps us investigate the bug.
--- a/.github/ISSUE_TEMPLATE/community.yml
+++ b/.github/ISSUE_TEMPLATE/community.yml
@ -1,25 +0,0 @@
-name: Community
-description: Related to "Community made" features
-title: '[Community made] '
-labels:
-  - community_made
-body:
-  - type: markdown
-    attributes:
-      value: >
-        Prusa Research will NOT follow up these issues!
-        The maintainers of the "Community made" feature should/will react.
-
-        Please, before you create a new "Community made" ticket, please make sure you searched in open and closed issues and couldn't find anything that matches.
-  - type: textarea
-    id: which_community
-    attributes:
-      label: Which Community made feature do you want to address?
-    validations:
-      required: true
-  - type: textarea
-    id: What_about_community
-    attributes:
-      label: What is your request/question/suggestion?
-    validations:
-      required: true
--- a/.github/ISSUE_TEMPLATE/config.yml
+++ b/.github/ISSUE_TEMPLATE/config.yml
@ -1,11 +0,0 @@
-contact_links:
-  - name: Do you need Support?
-    url: https://help.prusa3d.com/article/customer-support_2287
-    about: If you are not sure whether what you are reporting is a bug, please contact our support team first. We are providing full 24/7 customer support.
-  - name: Knowledge Base
-    url: https://help.prusa3d.com/
-    about: We have a comprehensive help documentation that could be helpful for troubleshooting.
-  - name: Prusa Forum
-    url: https://forum.prusa3d.com/
-    about: Please get in touch on our Prusa Forum! (Not an official support channel)
-blank_issues_enabled: false
--- a/.github/ISSUE_TEMPLATE/enhancement.md
+++ b/.github/ISSUE_TEMPLATE/enhancement.md
@ -0,0 +1,20 @@
+---
+name: Enhancement
+about: Suggest an idea for this project
+title: "  [ENHANCEMENT]<Enter comprehensive title>"
+labels: enhancement
+assignees: ''
+
+---
+
+Please, before you create a new feature request, please make sure you searched in open and closed issues and couldn't find anything that matches.
+
+Enter what type of printer or upgrade the enhancement applies to.
+**Printer type** - [e.g. MK3S, MK3, MK2.5S, MK2.5, MK2S, MK2]
+**MMU Upgrade** - [e.g. MMU2S, MMU2, MMU1]
+
+**Is your enhancement related to a problem? Please describe.**
+  A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
+  
+**Describe the solution you'd like**
+  A clear and concise description of what you want to happen.
--- a/.github/ISSUE_TEMPLATE/enhancement.yml
+++ b/.github/ISSUE_TEMPLATE/enhancement.yml
@ -1,35 +0,0 @@
-name: Enhancement
-description: Suggest an improvement of the existing functionality
-title: '[ENHANCEMENT] '
-labels:
-  - enhancement
-body:
-  - type: markdown
-    attributes:
-      value: |
-        Before you create a new enhancement, please check if an issue with it already exists (either open or closed) by using the search bar on the issues page.
-  - type: checkboxes
-    id: printer_model
-    attributes:
-      label: Printer model
-      description: Select the printer model(s) where you would like this enhancement
-      options:
-        - label: MK3S/+
-        - label: MK3
-        - label: MK2.5S
-        - label: MK2.5
-    validations:
-      required: true
-  - type: textarea
-    id: description
-    attributes:
-      label: Describe the enhancement
-      description: How would this enhancement improve your experience? Do you have a specific use case where this enhancemnet is especially needed?
-    validations:
-      required: true
-  - type: textarea
-    id: expected_functionality
-    attributes:
-      label: Expected functionality
-      description: |
-        Describe the way the enhancement would change existing functionality. You can also describe it in a step-by-step basis if applicable.
--- a/.github/ISSUE_TEMPLATE/feature_request.md
+++ b/.github/ISSUE_TEMPLATE/feature_request.md
@ -0,0 +1,20 @@
+---
+name: Feature request
+about: Suggest an idea for this project
+title: "[FEATURE REQUEST]<Enter comprehensive title>"
+labels: feature request
+assignees: ''
+
+---
+
+Please, before you create a new feature request, please make sure you searched in open and closed issues and couldn't find anything that matches.
+
+If it makes sense, enter what type of printer or upgrade the feature request applies to.
+**Printer type** - [e.g. MK3S, MK3, MK2.5S, MK2.5, MK2S, MK2]
+**MMU Upgrade** - [e.g. MMU2S, MMU2, MMU1]
+
+**Is your feature request related to a problem? Please describe.**
+  A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
+  
+**Describe the solution you'd like**
+  A clear and concise description of what you want to happen.
--- a/.github/ISSUE_TEMPLATE/feature_request.yml
+++ b/.github/ISSUE_TEMPLATE/feature_request.yml
@ -1,35 +0,0 @@
-name: Feature request
-description: Suggest new functionality
-title: '[FEATURE REQUEST] '
-labels:
-  - feature request
-body:
-  - type: markdown
-    attributes:
-      value: |
-        Before you create a new feature request, please check if an issue with it already exists (either open or closed) by using the search bar on the issues page.
-  - type: checkboxes
-    id: printer_model
-    attributes:
-      label: Printer model
-      description: Select the printer model(s) where you would like this feature
-      options:
-        - label: MK3S/+
-        - label: MK3
-        - label: MK2.5S
-        - label: MK2.5
-    validations:
-      required: true
-  - type: textarea
-    id: description
-    attributes:
-      label: Describe the feature
-      description: How would this feature improve the printer? Are there specific use cases where this would be beneficial? Describe how you would use it.
-    validations:
-      required: true
-  - type: textarea
-    id: expected_functionality
-    attributes:
-      label: Expected functionality
-      description: |
-        Describe the way the feature would work. You can also describe it in a step-by-step basis if applicable.
--- a/.github/ISSUE_TEMPLATE/question.md
+++ b/.github/ISSUE_TEMPLATE/question.md
@ -0,0 +1,12 @@
+---
+name: Question
+about: What do you want to know?
+title: "[QUESTION]<Enter comprehensive title>"
+labels: question
+assignees: ''
+
+---
+
+Please, before you create a new question, please make sure you searched in open and closed issues and couldn't find anything that matches.
+
+**What is your question?**
--- a/.github/ISSUE_TEMPLATE/question.yml
+++ b/.github/ISSUE_TEMPLATE/question.yml
@ -1,16 +0,0 @@
-name: Question
-description: Ask a firmware specific question
-title: '[QUESTION] '
-labels:
-  - question
-body:
-  - type: markdown
-    attributes:
-      value: |
-        Before you create a new question, please check if an issue with it already exists (either open or closed) by using the search bar on the issues page.
-  - type: textarea
-    id: question
-    attributes:
-      label: Question
-    validations:
-      required: true
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@ -1,178 +0,0 @@
-name: ci-build
-
-on:
-  pull_request:
-    branches:
-    - '*'
-  push:
-    branches: [ MK3, MK3_* ]
-    tags:
-    - 'v*'
-    - 't*'
-    - 'c*'
-
-env:
-  GH_ANNOTATIONS: 1
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    steps:
-
-    # setup base required dependencies
-    - name: Setup dependencies
-      run: |
-          sudo apt-get update
-          sudo apt-get install cmake ninja-build python3-pyelftools python3-regex python3-polib
-
-    # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
-    - name: Checkout ${{ github.event.pull_request.head.ref }}
-      uses: actions/checkout@v4
-      if: ${{ github.event.pull_request }}
-      with:
-        ref: ${{ github.event.pull_request.head.sha }}
-        submodules: true
-        fetch-depth: 0
-
-    - name: Checkout ${{ github.event.ref }}
-      uses: actions/checkout@v4
-      if: ${{ !github.event.pull_request }}
-      with:
-        ref: ${{ github.event.ref }}
-        submodules: true
-        fetch-depth: 0
-
-    - name: Cache Dependencies
-      uses: actions/cache@v4
-      id: cache-pkgs
-      with:
-          path: ".dependencies"
-          key: "build-deps-1_0_0-linux"
-
-    - name: Setup build dependencies
-      run: |
-          ./utils/bootstrap.py
-
-    - name: Cache permissions
-      run: sudo chmod -R 744 .dependencies
-
-    - name: Build
-      run: |
-          mkdir build
-          cd build
-          cmake .. -DCMAKE_TOOLCHAIN_FILE="../cmake/AvrGcc.cmake" -DCMAKE_BUILD_TYPE=Release -G Ninja
-          ninja
-
-    - name: Upload artifacts
-      if: ${{ !github.event.pull_request }}
-      uses: actions/upload-artifact@v4
-      with:
-        name: Firmware
-        path: build/*.hex
-
-    - name: RELEASE THE KRAKEN
-      if: startsWith(github.ref, 'refs/tags/v') || startsWith(github.ref, 'refs/tags/t') || startsWith(github.ref, 'refs/tags/c')
-      uses: "marvinpinto/action-automatic-releases@latest"
-      with:
-        repo_token: "${{ secrets.GITHUB_TOKEN }}"
-        automatic_release_tag: ${{ github.ref_name }}
-        draft: true
-        files: |
-          ${{ github.workspace }}/build/release/*.hex
-          ${{ github.workspace }}/build/release/*.zip
-
-  check-lang:
-    runs-on: ubuntu-latest
-
-    steps:
-    # setup base required dependencies
-    - name: Setup dependencies
-      run: |
-          sudo apt-get update
-          sudo apt-get install gcc-11 g++11 lcov cmake ninja-build python3-pyelftools python3-regex python3-polib
-
-    # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
-    - name: Checkout ${{ github.event.pull_request.head.ref }}
-      uses: actions/checkout@v4
-      if: ${{ github.event.pull_request }}
-      with:
-        ref: ${{ github.event.pull_request.head.sha }}
-        submodules: true
-        fetch-depth: 0
-
-    - name: Checkout ${{ github.event.ref }}
-      uses: actions/checkout@v4
-      if: ${{ !github.event.pull_request }}
-      with:
-        ref: ${{ github.event.ref }}
-        submodules: true
-        fetch-depth: 0
-
-    - name: Cache Dependencies
-      uses: actions/cache@v4
-      id: cache-pkgs
-      with:
-          path: ".dependencies"
-          key: "build-deps-1_0_0-linux"
-
-    - name: Setup build dependencies
-      run: |
-          ./utils/bootstrap.py
-
-    - name: Cache permissions
-      run: sudo chmod -R 744 .dependencies
-
-    - name: Run check
-      run: |
-          mkdir build
-          cd build
-          cmake .. -G Ninja -DCMAKE_TOOLCHAIN_FILE="../cmake/AvrGcc.cmake" -DCMAKE_BUILD_TYPE=Release -G Ninja
-          ninja check_lang
-
-  tests:
-    runs-on: ubuntu-latest
-
-    steps:
-    # setup base required dependencies
-    - name: Setup dependencies
-      run: |
-          sudo apt-get update
-          sudo apt-get install gcc-11 g++11 lcov cmake ninja-build python3-pyelftools python3-regex python3-polib
-
-    # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
-    - name: Checkout ${{ github.event.pull_request.head.ref }}
-      uses: actions/checkout@v4
-      if: ${{ github.event.pull_request }}
-      with:
-        ref: ${{ github.event.pull_request.head.sha }}
-        submodules: true
-        fetch-depth: 0
-
-    - name: Checkout ${{ github.event.ref }}
-      uses: actions/checkout@v4
-      if: ${{ !github.event.pull_request }}
-      with:
-        ref: ${{ github.event.ref }}
-        submodules: true
-        fetch-depth: 0
-
-    - name: Cache Dependencies
-      uses: actions/cache@v4
-      id: cache-pkgs
-      with:
-          path: ".dependencies"
-          key: "build-deps-1_0_0-linux"
-
-    - name: Setup build dependencies
-      run: |
-          ./utils/bootstrap.py
-
-    - name: Cache permissions
-      run: sudo chmod -R 744 .dependencies
-
-    - name: Run check
-      run: |
-          mkdir build
-          cd build
-          cmake .. -G Ninja
-          ninja test_run_all
--- a/.github/workflows/pr-size.sh
+++ b/.github/workflows/pr-size.sh
@ -1,56 +0,0 @@
-#!/bin/sh
-MESSAGE=$1
-BASE_DIR=$2
-PR_DIR=$3
-shift 3
-
-# this assumes we're running from the repository root
-AVR_SIZE=$(echo .dependencies/avr-gcc-*/bin/avr-size)
-test -x "$AVR_SIZE" || exit 2
-
-avr_size()
-{
-    "$AVR_SIZE" --mcu=atmega2560 -C "$@"
-}
-
-avr_flash()
-{
-    avr_size "$@" | sed -ne 's/^Program: *\([0-9]\+\).*/\1/p'
-}
-
-avr_ram()
-{
-    avr_size "$@" | sed -ne 's/^Data: *\([0-9]\+\).*/\1/p'
-}
-
-cat <<EOF > "$MESSAGE"
-All values in bytes. Δ Delta to base
-
-| Target| ΔFlash | ΔSRAM | Used Flash | Used SRAM | Free Flash | Free SRAM |
-| ------| ------ | ----- | -----------| --------- | ---------- | --------- |
-EOF
-
-einsy_max_upload_size=$(grep "prusa_einsy_rambo.upload.maximum_size" .dependencies/prusa3dboards-*/boards.txt | cut -d "=" -f2)
-einsy_max_upload_data_size=8192
-
-for TARGET in $@
-do
-    # strip the multilang prefix
-    variant=${TARGET%_MULTILANG}
-
-    base_bin=$(echo ${BASE_DIR}/build_gen/$variant/${variant}_lang_base)
-    base_flash=$(avr_flash "$base_bin")
-    base_ram=$(avr_ram "$base_bin")
-
-    pr_bin=$(echo ${PR_DIR}/build_gen/$variant/${variant}_lang_base)
-    pr_flash=$(avr_flash "$pr_bin")
-    pr_ram=$(avr_ram "$pr_bin")
-
-    flash_d=$(($pr_flash - $base_flash))
-    ram_d=$(($pr_ram - $base_ram))
-
-    flash_free=$(($einsy_max_upload_size - $pr_flash))
-    ram_free=$(($einsy_max_upload_data_size - $pr_ram))
-
-    echo "| \`$TARGET\` | $flash_d | $ram_d | $pr_flash | $pr_ram | $flash_free | $ram_free |" >> "$MESSAGE"
-done
--- a/.github/workflows/pr-size.yml
+++ b/.github/workflows/pr-size.yml
@ -1,79 +0,0 @@
-name: pr-size
-
-on:
-  pull_request_target:
-    branches: [ MK3, MK3_* ]
-
-env:
-    TARGETS: "MK3S_MULTILANG MK3_MULTILANG"
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    permissions:
-      pull-requests: write
-
-    steps:
-
-    # setup base required dependencies
-    - name: Setup dependencies
-      run: |
-          sudo apt-get update
-          sudo apt-get install cmake ninja-build python3-pyelftools python3-regex python3-polib
-
-    # build the base branch
-    - name: Checkout base
-      uses: actions/checkout@v4
-
-    - name: Cache Dependencies
-      uses: actions/cache@v4
-      id: cache-pkgs
-      with:
-          path: ".dependencies"
-          key: "build-deps-1_0_0-linux"
-
-    - name: Setup build dependencies
-      run: |
-          ./utils/bootstrap.py
-
-    - name: Cache permissions
-      run: sudo chmod -R 744 .dependencies
-
-    - name: Build base
-      run: |
-          rm -rf build-base
-          mkdir build-base
-          cd build-base
-          cmake .. -DCMAKE_TOOLCHAIN_FILE="../cmake/AvrGcc.cmake" -DCMAKE_BUILD_TYPE=Release -G Ninja
-          ninja $TARGETS
-
-    # save pr-size for later use
-    - name: Save base data
-      run: |
-          cp -f ./.github/workflows/pr-size.sh build-base
-
-    # build the PR branch
-    - name: Checkout PR
-      uses: actions/checkout@v4
-      with:
-        clean: false
-        ref: ${{ github.event.pull_request.head.sha }}
-
-    - name: Build PR
-      run: |
-          rm -rf build-pr
-          mkdir build-pr
-          cd build-pr
-          cmake .. -DCMAKE_TOOLCHAIN_FILE="../cmake/AvrGcc.cmake" -DCMAKE_BUILD_TYPE=Release -G Ninja
-          ninja $TARGETS
-
-    # extract/show build differences
-    - name: Calculate binary changes
-      run: |
-          rm -rf build-changes
-          ./build-base/pr-size.sh build-changes build-base build-pr $TARGETS
-
-    - name: Add PR Comment
-      uses: mshick/add-pr-comment@v2
-      with:
-        message-path: build-changes
--- a/.github/workflows/stale.yml
+++ b/.github/workflows/stale.yml
@ -1,30 +0,0 @@
-name: Mark stale issues
-
-on:
-  schedule:
-  # 1:30 AM on MON/THU
-  - cron: "30 1 * * 1,2,3,4"
-
-jobs:
-  stale:
-
-    runs-on: ubuntu-latest
-
-    steps:
-    - uses: actions/stale@v9
-      with:
-        repo-token: ${{ secrets.GITHUB_TOKEN }}
-        # Don't ever mark PRs as stale.
-        days-before-pr-stale: -1
-        stale-issue-message: 'Thank you for your contribution to our project. This issue has not received any updates for 60 days and may be considered "stale." If this issue is still important to you, please add an update within the next 7 days to keep it open. Administrators can manually reopen the issue if necessary.'
-        close-issue-message: 'This issue has been closed due to lack of recent activity. Please consider opening a new one if needed.'
-        # Don't act on things assigned to a milestone or assigned to someone.
-        exempt-all-milestones: true
-        exempt-all-assignees: true
-        enable-statistics: true
-        # Disable this and change the operations per run back to 30 when this goes live.
-        debug-only: false
-        operations-per-run: 200
-        stale-issue-label: 'stale-issue'
-        stale-pr-label: 'stale-pr'
-        ascending: true
--- a/.gitignore
+++ b/.gitignore
@ -1,31 +1,54 @@
-# IDE data
-/.settings
-/.project
-/.cproject
-
-# cmake
-/build/
-/build_gen/
-/.dependencies
-/compile_commands.json
-
-# Temporary configuration
-/Firmware/Configuration_prusa.h
-
-# Temporary language files
-/lang/po/*.mo
-/lang/tmp/
-/lang/Firmware-intl.hex
-/lang/Firmware-intl-en_*.hex
-/lang/*.map
-
-# Temporary files and directories
-*[~#]
-*.tmp
-*.bak
-.DS_Store
-__pycache__
-
-# Generated files
+.settings
+.project
+.cproject
+Debug
+Firmware/Configuration_prusa.h
+Firmware/Doc
+/Firmware/.vs/Firmware/v14
+/Firmware/__vm
+/Firmware/Firmware.sln
+/Firmware/Firmware.vcxproj
+/Firmware/Firmware.vcxproj.filters
+/Firmware/Firmware - Shortcut.lnk
+/Firmware/variants/1_75mm_MK3-MMU-EINSy10a-E3Dv6full.h.bak
+/Firmware/Marlin_main.cpp~RF12cfae7.TMP
+/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h.bak
+/html
+/latex
+/Doxyfile
+/Firmware/builds/1_75mm_MK3-EINY04-E3Dv6full
+/Firmware/Configuration_prusa.h.bak
+/Firmware/Configuration_prusa_backup.h
+/Firmware/ultralcd_implementation_hitachi_HD44780.h.bak
+/Firmware/ultralcd.cpp.bak
+/Firmware/temperature.cpp.bak
+/Firmware/pins.h.bak
+/Firmware/Marlin_main.cpp.bak
+/Firmware/language_pl.h.bak
+/Firmware/language_it.h.bak
+/Firmware/language_es.h.bak
+/Firmware/language_en.h.bak
+/Firmware/language_de.h.bak
+/Firmware/language_cz.h.bak
+/Firmware/variants/1_75mm_MK2-MultiMaterial-RAMBo13a-E3Dv6full.h
+/Firmware/variants/1_75mm_MK2-MultiMaterial-RAMBo10a-E3Dv6full.h
+/Firmware/variants/1_75mm_MK2-EINY01-E3Dv6full.h.bak
+/Firmware/variants/1_75mm_MK1-RAMBo13a-E3Dv6full.h
+/Firmware/variants/1_75mm_MK1-RAMBo10a-E3Dv6full.h
+/lang/*.bin
+/lang/*.hex
+/lang/*.dat
+/lang/*.tmp
+/lang/*.out
+/lang/not_tran*.txt
+/lang/not_used*.txt
+/lang/progmem1.chr
+/lang/progmem1.lss
+/lang/progmem1.txt
+/lang/progmem1.var
+/lang/text.sym
+/lang/textaddr.txt
 /build-env/
-/Firmware/Doc/
+/Firmware/Firmware.vcxproj
+/Firmware/Configuration_prusa_bckp.h
+/Firmware/variants/printers.h
--- a/.travis.yml
+++ b/.travis.yml
@ -0,0 +1,35 @@
+dist: trusty
+before_install:
+  - sudo apt-get install -y ninja-build
+  # Arduino IDE adds a lot of noise caused by network traffic, trying to firewall it off
+  - sudo iptables -P INPUT DROP
+  - sudo iptables -P FORWARD DROP
+  - sudo iptables -P OUTPUT ACCEPT
+  - sudo iptables -A INPUT -i lo -j ACCEPT
+  - sudo iptables -A OUTPUT -o lo -j ACCEPT
+  - sudo iptables -A INPUT -m conntrack --ctstate ESTABLISHED,RELATED -j ACCEPT
+script:
+  - bash -x test.sh
+  - cp Firmware/variants/1_75mm_MK3S-EINSy10a-E3Dv6full.h Firmware/Configuration_prusa.h
+  - bash -x build.sh || { echo "1_75mm_MK3S-EINSy10a-E3Dv6full variant failed" && false; }
+  - rm Firmware/Configuration_prusa.h
+  - cp Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h Firmware/Configuration_prusa.h
+  - bash -x build.sh || { echo "1_75mm_MK3-EINSy10a-E3Dv6full variant failed" && false; }
+  - rm Firmware/Configuration_prusa.h
+  - cp Firmware/variants/1_75mm_MK25S-RAMBo13a-E3Dv6full.h Firmware/Configuration_prusa.h
+  - bash -x build.sh || { echo "1_75mm_MK25S-RAMBo13a-E3Dv6full variant failed" && false; }
+  - rm Firmware/Configuration_prusa.h
+  - cp Firmware/variants/1_75mm_MK25S-RAMBo10a-E3Dv6full.h Firmware/Configuration_prusa.h
+  - bash -x build.sh || { echo "1_75mm_MK25S-RAMBo10a-E3Dv6full variant failed" && false; }
+  - rm Firmware/Configuration_prusa.h
+  - cp Firmware/variants/1_75mm_MK25-RAMBo13a-E3Dv6full.h Firmware/Configuration_prusa.h
+  - bash -x build.sh || { echo "1_75mm_MK25-RAMBo13a-E3Dv6full variant failed" && false; }
+  - rm Firmware/Configuration_prusa.h
+  - cp Firmware/variants/1_75mm_MK25-RAMBo10a-E3Dv6full.h Firmware/Configuration_prusa.h
+  - bash -x build.sh || { echo "1_75mm_MK25-RAMBo10a-E3Dv6full variant failed" && false; }
+  - rm Firmware/Configuration_prusa.h
+  - cp Firmware/variants/1_75mm_MK2-RAMBo13a-E3Dv6full.h Firmware/Configuration_prusa.h
+  - bash -x build.sh || { echo "1_75mm_MK2-RAMBo13a-E3Dv6full variant failed" && false; }
+  - rm Firmware/Configuration_prusa.h
+  - cp Firmware/variants/1_75mm_MK2-RAMBo10a-E3Dv6full.h Firmware/Configuration_prusa.h
+  - bash -x build.sh || { echo "1_75mm_MK2-RAMBo10a-E3Dv6full variant failed" && false; }
--- a/.vscode/cmake-kits.json
+++ b/.vscode/cmake-kits.json
@ -1,10 +0,0 @@
-[
-    {
-        "name": "avr-gcc",
-        "toolchainFile": "${workspaceFolder}/cmake/AvrGcc.cmake",
-        "cmakeSettings": {
-            "CMAKE_MAKE_PROGRAM": "${workspaceFolder}/.dependencies/ninja-1.12.1/ninja",
-            "CMAKE_BUILD_TYPE": "Release"
-        }
-    }
-]
--- a/.vscode/cmake-variants.yaml
+++ b/.vscode/cmake-variants.yaml
@ -1,11 +0,0 @@
-buildType:
-  default: debug
-  choices:
-    debug:
-      short: Debug
-      long: Emit debug information
-      buildType: Debug
-    release:
-      short: Release
-      long: Optimize generated code
-      buildType: Release
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@ -1,10 +0,0 @@
-{
-    "cmake.configureOnOpen": true,
-    "cmake.copyCompileCommands": "${workspaceFolder}/compile_commands.json",
-    "cmake.cmakePath": "${workspaceFolder}/.dependencies/cmake-3.22.5/bin/cmake",
-    "cmake.generator": "Ninja",
-    "files.insertFinalNewline": true,
-    "files.associations": {
-        "xlocale": "cpp"
-    }
-}
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -1,658 +1,24 @@
-cmake_minimum_required(VERSION 3.19)
-include(cmake/Utilities.cmake)
-include(cmake/GetGitRevisionDescription.cmake)
-include(cmake/ReproducibleBuild.cmake)
-include(cmake/ProjectVersion.cmake)
-resolve_version_variables()
-
-OPTION(ENFORCE_VERSION_MATCH "Enforce checking that configuration.h matches any parsed git tags" OFF)
-OPTION(NO_TAG_IS_FATAL "If tag parsing fails, issue a fatal error" OFF)
-
-set(PROJECT_VERSION_HASH
-    "<auto>"
-    CACHE
-      STRING
-      "Version suffix to be appended to the final filename (<ver+PROJECT_VERSION_HASH>). Overrides git hash if set."
-    )
-if(PROJECT_VERSION_HASH STREQUAL "<auto>")
-  set(PROJECT_VERSION_HASH "${FW_COMMIT_HASH}")
-endif()
-set(PROJECT_VERSION_FULL
-    "<auto>"
-    CACHE
-      STRING
-      "Full version string to be shown on the info screen in settings. Overrides git version if set."
-    )
-if(PROJECT_VERSION_FULL STREQUAL "<auto>")
-  set(PROJECT_VERSION_FULL "${FW_COMMIT_DSC}")
-endif()
-set(PROJECT_REPOSITORY
-    "Unknown"
-    CACHE STRING "Repository string to be shown on the info screen in settings."
-    )
-
-set(CUSTOM_COMPILE_OPTIONS
-    ""
-    CACHE STRING "Allows adding custom C/C++ flags"
-    )
-
-#set(FN_VERSION_SUFFIX "FW${PROJECT_VERSION}+${PROJECT_VERSION_HASH}")
-set(FN_VERSION_SUFFIX "FW_${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}.${PROJECT_VERSION_REV}")
-if(PROJECT_VERSION_FLV AND PROJECT_VERSION_FLV_VER)
-  set (FN_VERSION_SUFFIX "${FN_VERSION_SUFFIX}-${PROJECT_VERSION_FLV}${PROJECT_VERSION_FLV_VER}")
-endif()
-set(FN_VERSION_DEBUG_SUFFIX "${FN_VERSION_SUFFIX}+${PROJECT_VERSION_COMMIT}")
-
-# Inform user about the resolved settings from Configuration.h
-message(STATUS "Project version (Configuration.h): ${PROJECT_VERSION}")
-#message(STATUS "Project version major............: ${PROJECT_VERSION_MAJOR}") #For debuging
-#message(STATUS "Project version minor............: ${PROJECT_VERSION_MINOR}") #For debuging
-#message(STATUS "Project version revision.........: ${PROJECT_VERSION_REV}") #For debuging
-#message(STATUS "Project version flavor...........: ${PROJECT_VERSION_FLV}") #For debuging
-#message(STATUS "Project version fla-revison......: ${PROJECT_VERSION_FLV_VER}") #For debuging
-#message(STATUS "Project version commit number....: ${PROJECT_VERSION_COMMIT}") #For debuging
-message(STATUS "Filename suffix..................: ${FN_VERSION_SUFFIX}")
-message(STATUS "Filename debug suffix ...........: ${FN_VERSION_DEBUG_SUFFIX}")
-#message(STATUS "Host OS .........................: ${CMAKE_HOST_SYSTEM_NAME}")
-# testing
-# SET(FW_COMMIT_DSC "v3.13.0-1234")
-
-if(NOT "${PROJECT_VERSION_HASH}" STREQUAL "UNKNOWN" AND NOT "${FW_COMMIT_DSC}" MATCHES ".+NOTFOUND.+") # else -> no commit hash is known... likely no git.
-string(REGEX MATCH "[v|t|c]([0-9]+)\.([0-9]+)\.([0-9]+)-?(${DEV_TAG_REGEX})?([0-9]+)?-([0-9]+)" TAG_VERSION "${FW_COMMIT_DSC}")
-
-if (CMAKE_MATCH_4) # Do we have a build type?
-    decode_flavor_code(PROJECT_VER_TAG_FLV "${CMAKE_MATCH_4}" "${CMAKE_MATCH_5}")
-else()
-    # No dev status found, it must be a final tag.
-    decode_flavor_code(PROJECT_VER_TAG_FLV "RELEASED" "0")
-endif()
-
-if(ENFORCE_VERSION_MATCH)
-    if(NOT ${CMAKE_MATCH_1} STREQUAL ${PROJECT_VERSION_MAJOR})
-        message(FATAL_ERROR "Major version of current tag disagrees with Configuration.h ${CMAKE_MATCH_1}!=${PROJECT_VERSION_MAJOR}")
-    endif()
-
-    if(NOT ${CMAKE_MATCH_2} STREQUAL ${PROJECT_VERSION_MINOR})
-        message(FATAL_ERROR "Minor version of current tag disagrees with Configuration.h ${CMAKE_MATCH_2}!=${PROJECT_VERSION_MINOR}")
-    endif()
-
-    if(NOT ${CMAKE_MATCH_3} STREQUAL ${PROJECT_VERSION_REV})
-        message(FATAL_ERROR "Rev version of current tag disagrees with Configuration.h ${CMAKE_MATCH_3}!=${PROJECT_VERSION_REV}")
-    endif()
-
-    if(NOT ${PROJECT_VER_TAG_FLV} STREQUAL ${PROJECT_VERSION_TWEAK})
-        message(FATAL_ERROR "Dev status of current tag disagrees with Configuration.h ${PROJECT_VER_TAG_FLV}!=${PROJECT_VERSION_TWEAK}")
-    endif()
-    # Note - we don't check the commit counter, that'd be too much of a headache. Maybe it
-    # should be an error only on a tagged build?
-    MESSAGE(STATUS "Configuration.h and tag match: OK (${PROJECT_VERSION}/${CMAKE_MATCH_1}.${CMAKE_MATCH_2}.${CMAKE_MATCH_3}.${PROJECT_VER_TAG_FLV})")
-else()
-    MESSAGE(STATUS "Configuration.h and tag (not enforced): (${PROJECT_VERSION}/${CMAKE_MATCH_1}.${CMAKE_MATCH_2}.${CMAKE_MATCH_3}.${PROJECT_VER_TAG_FLV})")
-endif()
-
-MESSAGE(STATUS "Commit Nr: Configuration.h: ${PROJECT_VERSION_COMMIT} Tag: ${CMAKE_MATCH_6}")
-MESSAGE(STATUS "These tag values will override Configuration.h")
-SET(PROJECT_VERSION ${CMAKE_MATCH_1}.${CMAKE_MATCH_2}.${CMAKE_MATCH_3}.${PROJECT_VER_TAG_FLV})
-SET(PROJECT_VERSION_COMMIT ${CMAKE_MATCH_6})
-
-git_get_repository(PROJECT_REPOSITORY)
-else(GIT_FOUND)
-if (NO_TAG_IS_FATAL)
-    MESSAGE(FATAL_ERROR "Git was not found or an error occurred parsing the tag. This is a fatal error according to the settings.")
-else()
-    MESSAGE(STATUS "Git was not found or an error occurred parsing the tag. Falling back to Configuration.h values (${PROJECT_VERSION}).")
-endif()
-set(FW_COMMIT_HASH ${FW_COMMIT_HASH_UNKNOWN}) # Clear it, the code expects a binary...
-set(PROJECT_VERSION_TIMESTAMP "0")
-endif()
-
-if(CMAKE_MATCH_1 AND CMAKE_MATCH_2)
-  set(FN_VERSION_SUFFIX "FW_${CMAKE_MATCH_1}.${CMAKE_MATCH_2}.${CMAKE_MATCH_3}")
-endif()
-if(CMAKE_MATCH_4 AND CMAKE_MATCH_5)
-  set (FN_VERSION_SUFFIX "${FN_VERSION_SUFFIX}-${CMAKE_MATCH_4}${CMAKE_MATCH_5}")
-endif()
-if(CMAKE_MATCH_6 AND PROJECT_VERSION_HASH)
-  set(FN_VERSION_DEBUG_SUFFIX "${FN_VERSION_SUFFIX}+${CMAKE_MATCH_6}_${PROJECT_VERSION_HASH}")
-endif()
-# Inform user about the resolved settings from github
-message(STATUS "Project version git..............: ${PROJECT_VERSION}")
-message(STATUS "Project version git hash.........: ${PROJECT_VERSION_HASH}")
-message(STATUS "Project version git description..: ${PROJECT_VERSION_FULL}")
-#message(STATUS "Project version git major........: ${CMAKE_MATCH_1}") #For debuging
-#message(STATUS "Project version git minor........: ${CMAKE_MATCH_2}") #For debuging
-#message(STATUS "Project version git revision.....: ${CMAKE_MATCH_3}") #For debuging
-#message(STATUS "Project version git flavor.......: ${CMAKE_MATCH_4}") #For debuging
-#message(STATUS "Project version git fla-revison..: ${CMAKE_MATCH_5}") #For debuging
-#message(STATUS "Project version git commit number: ${CMAKE_MATCH_6}") #For debuging
-message(STATUS "Filename suffix .................: ${FN_VERSION_SUFFIX}")
-message(STATUS "Filename debug suffix ...........: ${FN_VERSION_DEBUG_SUFFIX}")
-
-# Language configuration
-set(MAIN_LANGUAGES
-    cs de es fr it pl
-    CACHE STRING "The list of 'main' languages to be included, in the correct order"
-    )
-set(COMMUNITY_LANGUAGES
-    nl
-    ro
-    hu
-    hr
-    sk
-    sv
-    no
-    CACHE STRING "The list of community languages to be included, in the correct order"
-    )
-set(SELECTED_LANGUAGES ${MAIN_LANGUAGES} ${COMMUNITY_LANGUAGES})
-
-get_dependency_directory(prusa3dboards PRUSA_BOARDS_DIR)
-project(Prusa-Firmware VERSION ${PROJECT_VERSION})
-add_subdirectory(lib)
-
-# Get LANG_MAX_SIZE from sources
-file(STRINGS ${CMAKE_CURRENT_SOURCE_DIR}/Firmware/config.h MAX_SIZE_LINE
-     REGEX "^#define \+LANG_SIZE_RESERVED \+"
-     )
-string(REGEX MATCH "0x[0-9]+" MAX_SIZE_HEX "${MAX_SIZE_LINE}")
-math(EXPR LANG_MAX_SIZE "${MAX_SIZE_HEX}" OUTPUT_FORMAT DECIMAL)
-message("Language maximum size (from config.h): ${LANG_MAX_SIZE} bytes")
-
-# Ditto, this in xflash_layout.h but needs invocation of the preprocessor... :-/
-set(LANG_BIN_MAX 249856)
-
-# Check GCC Version
-get_recommended_gcc_version(RECOMMENDED_TOOLCHAIN_VERSION)
-if(CMAKE_CROSSCOMPILING AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL
-                            ${RECOMMENDED_TOOLCHAIN_VERSION}
-   )
-  message(WARNING "Recommended AVR toolchain is ${RECOMMENDED_TOOLCHAIN_VERSION}"
-                  ", but you have ${CMAKE_CXX_COMPILER_VERSION}"
-          )
-
-elseif(NOT CMAKE_CROSSCOMPILING AND NOT CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
-  message(
-    WARNING
-      "Recommended compiler for host tools and unittests is GCC, you have ${CMAKE_CXX_COMPILER_ID}."
-    )
-endif()
-
-# append custom C/C++ flags
-if(CUSTOM_COMPILE_OPTIONS)
-  string(REPLACE " " ";" CUSTOM_COMPILE_OPTIONS "${CUSTOM_COMPILE_OPTIONS}")
-  add_compile_options(${CUSTOM_COMPILE_OPTIONS})
-endif()
-
-#
-# Global Compiler & Linker Configuration
-#
-
-# enable warnings
-add_compile_options(-Wall -Wextra -Wno-expansion-to-defined -Wsign-compare)
-
-# default standards for all targets
-set(CMAKE_C_STANDARD 11)
-set(CMAKE_CXX_STANDARD 17)
-
-# support _DEBUG macro (some code uses to recognize debug builds)
-if(CMAKE_BUILD_TYPE STREQUAL "Debug")
-  add_compile_definitions(_DEBUG)
-endif()
-
-#
-# Firmware - get file lists.
-#
-set(FW_SOURCES
-    adc.cpp
-    backlight.cpp
-    BlinkM.cpp
-    bootapp.c
-    cardreader.cpp
-    cmdqueue.cpp
-    Configuration.cpp
-    ConfigurationStore.cpp
-    Dcodes.cpp
-    eeprom.cpp
-    fancheck.cpp
-    Filament_sensor.cpp
-    first_lay_cal.cpp
-    heatbed_pwm.cpp
-    host.cpp
-    la10compat.cpp
-    language.c
-    lcd.cpp
-    Marlin_main.cpp
-    MarlinSerial.cpp
-    meatpack.cpp
-    menu.cpp
-    mesh_bed_calibration.cpp
-    mesh_bed_leveling.cpp
-    messages.cpp
-    mmu2.cpp
-    mmu2_crc.cpp
-    mmu2_error_converter.cpp
-    mmu2_fsensor.cpp
-    mmu2_log.cpp
-    mmu2_marlin1.cpp
-    mmu2_power.cpp
-    mmu2_progress_converter.cpp
-    mmu2_protocol.cpp
-    mmu2_protocol_logic.cpp
-    mmu2_reporting.cpp
-    mmu2_serial.cpp
-    motion_control.cpp
-    optiboot_xflash.cpp
-    pat9125.cpp
-    planner.cpp
-    power_panic.cpp
-    printer_state.cpp
-    Prusa_farm.cpp
-    rbuf.c
-    Sd2Card.cpp
-    SdBaseFile.cpp
-    SdFatUtil.cpp
-    SdFile.cpp
-    SdVolume.cpp
-    sm4.cpp
-    sound.cpp
-    speed_lookuptable.cpp
-    spi.c
-    SpoolJoin.cpp
-    stepper.cpp
-    stopwatch.cpp
-    strtod.c
-    swi2c.c
-    Tcodes.cpp
-    temperature.cpp
-    timer02.c
-    Timer.cpp
-    tmc2130.cpp
-    tone04.c
-    twi.cpp
-    uart2.cpp
-    ultralcd.cpp
-    util.cpp
-    xflash.c
-    xflash_dump.cpp
-    xyzcal.cpp
-    )
-list(TRANSFORM FW_SOURCES PREPEND ${CMAKE_CURRENT_SOURCE_DIR}/Firmware/)
-
-set(AVR_SOURCES
-    wiring_digital.c
-    WInterrupts.c
-    wiring_pulse.c
-    hooks.c
-    wiring.c
-    wiring_analog.c
-    wiring_shift.c
-    CDC.cpp
-    PluggableUSB.cpp
-    HardwareSerial.cpp
-    HardwareSerial0.cpp
-    HardwareSerial1.cpp
-    HardwareSerial3.cpp
-    IPAddress.cpp
-    HardwareSerial2.cpp
-    Print.cpp
-    Stream.cpp
-    Tone.cpp
-    USBCore.cpp
-    WMath.cpp
-    WString.cpp
-    abi.cpp
-    main.cpp
-    )
-list(TRANSFORM AVR_SOURCES PREPEND ${PRUSA_BOARDS_DIR}/cores/prusa_einsy_rambo/)
-
-#
-# Target configuration
-#
-if(CMAKE_CROSSCOMPILING)
-  set_source_epoch(${PROJECT_VERSION_TIMESTAMP})
-
-  # default optimization flags
-  set(CMAKE_CXX_FLAGS_DEBUG "-Og -g")
-  set(CMAKE_CXX_FLAGS_RELEASE "-Os -g -DNDEBUG")
-  set(CMAKE_C_FLAGS_DEBUG ${CMAKE_CXX_FLAGS_DEBUG})
-  set(CMAKE_C_FLAGS_RELEASE ${CMAKE_CXX_FLAGS_RELEASE})
-
-  # mcu and target-related settings
-  add_compile_options(
-    -mmcu=atmega2560 -DF_CPU=16000000L -DARDUINO=10819 -DARDUINO_AVR_PRUSA_EINSY_RAMBO
-    -DARDUINO_ARCH_AVR
-    )
-  add_link_options(-mmcu=atmega2560 -Wl,-u,vfprintf -lprintf_flt -lm)
-
-  # disable some C++ language features
-  add_compile_options($<$<COMPILE_LANGUAGE:CXX>:-fno-threadsafe-statics>)
-  add_compile_options($<$<COMPILE_LANGUAGE:CXX>:-fno-rtti>)
-
-  # disable exceptions
-  add_compile_options($<$<COMPILE_LANGUAGE:CXX>:-fno-exceptions>)
-  add_compile_options($<$<COMPILE_LANGUAGE:CXX>:-fno-unwind-tables>)
-
-  # split and gc sections
-  add_compile_options(-ffunction-sections -fdata-sections)
-  add_link_options(-ffunction-sections -fdata-sections -Wl,--gc-sections)
-
-  # LTO (with custom options)
-  add_compile_options(-flto -fno-fat-lto-objects)
-  add_link_options(-flto)
-
-  # Create this target before we apply the GC options
-  add_library(avr_core STATIC ${AVR_SOURCES})
-  set_reproducible_target(avr_core)
-  target_include_directories(
-    avr_core PRIVATE ${PRUSA_BOARDS_DIR}/cores/prusa_einsy_rambo/
-                     ${PRUSA_BOARDS_DIR}/variants/prusa_einsy_rambo/
-    )
-endif()
-
-# Meta targets to build absolutely everything
-add_custom_target(ALL_FIRMWARE)
-add_custom_target(ALL_ENGLISH)
-add_custom_target(ALL_MULTILANG)
-add_dependencies(ALL_FIRMWARE ALL_ENGLISH ALL_MULTILANG)
-set_target_properties(ALL_MULTILANG PROPERTIES EXCLUDE_FROM_ALL FALSE)
-
-function(add_base_binary variant_name)
-  add_executable(${variant_name} ${FW_SOURCES} ${FW_HEADERS} ${VARIANT_CFG_DST})
-  set_target_properties(${variant_name} PROPERTIES EXCLUDE_FROM_ALL TRUE)
-  set_reproducible_target(${variant_name})
-
-  target_include_directories(
-    ${variant_name}
-    PRIVATE ${PRUSA_BOARDS_DIR}/cores/prusa_einsy_rambo/
-            ${PRUSA_BOARDS_DIR}/variants/prusa_einsy_rambo/ ${CMAKE_SOURCE_DIR}/Firmware
-    )
-
-  target_link_libraries(${variant_name} avr_core)
-
-  # configure linker script
-  set(LINKER_SCRIPT ${PRUSA_BOARDS_DIR}/ldscripts/avr6.xn)
-  target_link_options(${variant_name} PUBLIC -Wl,-T,${LINKER_SCRIPT})
-
-  # limit the text section to 248K (256K - 8k reserved for the bootloader)
-  target_link_options(${variant_name} PUBLIC -Wl,--defsym=__TEXT_REGION_LENGTH__=248K)
-
-  # produce ASM listing. Note we also specify the .map as a byproduct so it gets cleaned because
-  # link_options doesn't have a "generated outputs" feature.
-  add_custom_command(
-    TARGET ${variant_name}
-    POST_BUILD
-    COMMAND ${CMAKE_OBJDUMP} --prefix ${CMAKE_SOURCE_DIR} -CSd ${variant_name} > ${variant_name}.asm
-    BYPRODUCTS ${variant_name}.asm ${variant_name}.map
-    )
-
-  # inform about the firmware's size in terminal
-  add_custom_command(
-    TARGET ${variant_name}
-    POST_BUILD
-    COMMAND ${CMAKE_COMMAND} -E echo_append "${variant_name} "
-    COMMAND ${CMAKE_SIZE_UTIL} -C --mcu=atmega2560 ${variant_name}
-    )
-  report_size(${variant_name})
-
-  # generate linker map file
-  target_link_options(
-    ${variant_name} PUBLIC -Wl,-Map=${CMAKE_CURRENT_BINARY_DIR}/${variant_name}.map
-    )
-  decode_tweak_version(PROJECT_VERSION_FLV PROJECT_VERSION_FLV_VER)
-  target_compile_definitions(
-    ${variant_name}
-    PRIVATE CMAKE_CONTROL FW_REPOSITORY="${PROJECT_REPOSITORY}"
-            FW_COMMIT_HASH="${FW_COMMIT_HASH}"
-            FW_COMMIT_HASH_LENGTH=${FW_COMMIT_HASH_LENGTH}
-            FW_MAJOR=${PROJECT_VERSION_MAJOR}
-            FW_MINOR=${PROJECT_VERSION_MINOR}
-            FW_REVISION=${PROJECT_VERSION_REV}
-            FW_COMMITNR=${PROJECT_VERSION_COMMIT}
-    )
-    if(NOT PROJECT_VERSION_FLV STREQUAL "RELEASED")
-        target_compile_definitions(
-        ${variant_name}
-        PRIVATE
-        FW_FLAVERSION=${PROJECT_VERSION_FLV_VER}
-        FW_FLAVOR=${PROJECT_VERSION_FLV}
-        )
-    endif()
-endfunction()
-
-function(fw_add_variant variant_name)
-  set(variant_header "variants/${variant_name}.h")
-  string(REPLACE "1_75mm_" "" variant_name "${variant_name}")
-  string(REPLACE "-E3Dv6full" "" variant_name "${variant_name}")
-
-  # Single-language build
-  set(FW_EN "${variant_name}_ENGLISH")
-  #MK3S_MK3S+_FW_3.13.2-RC1_ENGLISH.hex
-  set(hex_variant_name "${variant_name}")
-  if(hex_variant_name STRGREATER_EQUAL "MK3S")
-    string(REPLACE "MK3S" "MK3S_MK3S+" hex_variant_name ${hex_variant_name})
-  endif()
-  set(FW_HEX "${CMAKE_BINARY_DIR}/${hex_variant_name}_${FN_VERSION_SUFFIX}_ENGLISH.hex")
-  #message(STATUS "Hex filename: ${FW_HEX}")
-
-  add_base_binary(${FW_EN})
-  target_compile_definitions(${FW_EN} PUBLIC LANG_MODE=0 FW_VARIANT="${variant_header}")
-  add_custom_command(
-    TARGET ${FW_EN}
-    POST_BUILD
-    COMMAND ${CMAKE_OBJCOPY} -O ihex ${FW_EN} ${FW_EN}.hex
-    COMMAND ${CMAKE_COMMAND} -E create_hardlink ${FW_EN}.hex ${FW_HEX}
-    BYPRODUCTS ${FW_EN}.hex ${FW_HEX}
-    COMMENT "Generating ${FW_EN}.hex"
-    )
-  add_dependencies(ALL_ENGLISH ${FW_EN})
-
-  # Multi-language build/s
-  set(FW_LANG_BASE "${variant_name}_lang_base")
-  set(FW_LANG_PATCH "${variant_name}_lang_patch")
-  add_base_binary(${FW_LANG_BASE})
-  target_compile_definitions(${FW_LANG_BASE} PUBLIC LANG_MODE=1 FW_VARIANT="${variant_header}")
-
-  # Construct language map
-  set(LANG_TMP_DIR lang)
-  set(LANG_MAP ${LANG_TMP_DIR}/${variant_name}_lang.map)
-
-  add_custom_command(
-    OUTPUT ${LANG_MAP}
-    COMMAND ${CMAKE_OBJCOPY} -O binary ${FW_LANG_BASE} ${FW_LANG_PATCH}.bin
-    COMMAND "${Python3_EXECUTABLE}" ${CMAKE_SOURCE_DIR}/lang/lang-map.py ${FW_LANG_BASE} ${FW_LANG_PATCH}.bin > ${LANG_MAP}
-    COMMAND ${CMAKE_OBJCOPY} -I binary -O ihex ${FW_LANG_PATCH}.bin ${FW_LANG_PATCH}.hex
-    DEPENDS ${FW_LANG_BASE}
-    BYPRODUCTS ${FW_LANG_PATCH}.bin ${FW_LANG_PATCH}.hex
-    COMMENT "Generating ${variant_name} language map"
-    )
-
-  # Base targets for language checks
-  add_custom_target(check_lang_${variant_name})
-  add_dependencies(check_lang check_lang_${variant_name})
-
-  # Build language catalogs
-  set(LANG_BINS "")
-  foreach(LANG IN LISTS SELECTED_LANGUAGES)
-    set(LANG_BIN ${LANG_TMP_DIR}/${variant_name}_${LANG}.bin)
-    set(PO_FILE "${CMAKE_SOURCE_DIR}/lang/po/Firmware_${LANG}.po")
-
-    # Full language checks
-    add_custom_target(
-      check_lang_${variant_name}_${LANG}
-      COMMENT "Checking ${variant_name} language ${LANG}"
-      COMMAND "${Python3_EXECUTABLE}" ${CMAKE_SOURCE_DIR}/lang/lang-check.py --map ${LANG_MAP} ${PO_FILE}
-      DEPENDS ${LANG_MAP} ${PO_FILE}
-      USES_TERMINAL
-      )
-    add_dependencies(check_lang_${variant_name} check_lang_${variant_name}_${LANG})
-    add_dependencies(check_lang_${LANG} check_lang_${variant_name}_${LANG})
-
-    add_custom_command(
-      OUTPUT ${LANG_BIN}
-      # Check po file for errors _only_
-      COMMAND "${Python3_EXECUTABLE}" ${CMAKE_SOURCE_DIR}/lang/lang-check.py --errors-only --map ${LANG_MAP} ${PO_FILE}
-      # Build the catalog
-      COMMAND "${Python3_EXECUTABLE}" ${CMAKE_SOURCE_DIR}/lang/lang-build.py ${LANG_MAP} ${PO_FILE} ${LANG_BIN}
-      # Check bin size
-      COMMAND ${CMAKE_COMMAND} -DLANG_MAX_SIZE=${LANG_MAX_SIZE} -DLANG_FILE=${LANG_BIN} -P
-              ${PROJECT_CMAKE_DIR}/Check_lang_size.cmake
-      DEPENDS ${LANG_MAP} ${PO_FILE}
-      COMMENT "Generating ${variant_name}_${LANG}.bin"
-      )
-    list(APPEND LANG_BINS ${LANG_BIN})
-  endforeach()
-
-  string(FIND ${variant_name} "MK3" HAS_XFLASH)
-  if(${HAS_XFLASH} GREATER_EQUAL 0)
-    # X-Flash based build (catalogs appended to patched binary)
-    set(FW_LANG_FINAL "${variant_name}_MULTILANG")
-    set(hex_variant_name "${variant_name}")
-    #MK3S_MK3S+_FW_3.13.2-RC1+7651_deadbeef_MULTILANG.hex
-    if(hex_variant_name STRGREATER_EQUAL "MK3S")
-      string(REPLACE "MK3S" "MK3S_MK3S+" hex_variant_name ${hex_variant_name})
-    endif()
-    set(LANG_HEX ${CMAKE_BINARY_DIR}/${hex_variant_name}_${FN_VERSION_SUFFIX}_MULTILANG.hex)
-    set(LANG_DEBUG_HEX ${CMAKE_BINARY_DIR}/${hex_variant_name}_${FN_VERSION_DEBUG_SUFFIX}_MULTILANG.hex)
-    #message(STATUS "Hex filename .....: ${LANG_HEX}")
-    #message(STATUS "Hex debug filename: ${LANG_DEBUG_HEX}")
-    set(LANG_CATBIN ${LANG_TMP_DIR}/${variant_name}_cat.bin)
-    set(LANG_CATHEX ${LANG_TMP_DIR}/${variant_name}_cat.hex)
-
-    add_custom_command(
-      OUTPUT ${LANG_CATBIN}
-      COMMAND ${CMAKE_COMMAND} -E cat ${LANG_BINS} > ${LANG_CATBIN}
-      DEPENDS ${LANG_BINS}
-      COMMENT "Merging language catalogs"
-      )
-    #[[
-    #add_custom_command(OUTPUT ${LANG_FINAL_BIN}
-    #  COMMAND ${CMAKE_COMMAND} -DLANG_MAX_SIZE=${LANG_BIN_MAX} -DLANG_FILE=${LANG_FINAL_BIN}
-    #                           -P ${PROJECT_CMAKE_DIR}/Check_final_lang_bin_size.cmake
-    #  APPEND)
-    #]]
-    add_custom_command(
-      OUTPUT ${LANG_CATHEX}
-      COMMAND ${CMAKE_OBJCOPY} -I binary -O ihex ${LANG_CATBIN} ${LANG_CATHEX}
-      DEPENDS ${LANG_CATBIN}
-      COMMENT "Generating Hex for language data"
-      )
-
-    add_custom_command(
-      OUTPUT ${FW_LANG_FINAL}.hex
-      COMMAND ${CMAKE_COMMAND} -E cat ${FW_LANG_PATCH}.hex ${LANG_CATHEX} > ${FW_LANG_FINAL}.hex
-      COMMAND ${CMAKE_COMMAND} -E create_hardlink ${FW_LANG_FINAL}.hex ${LANG_HEX}
-      BYPRODUCTS ${LANG_HEX}
-      COMMAND ${CMAKE_COMMAND} -E create_hardlink ${FW_LANG_FINAL}.hex ${LANG_DEBUG_HEX}
-      BYPRODUCTS ${LANG_DEBUG_HEX}
-      COMMAND ${CMAKE_COMMAND} -E create_hardlink ${FW_LANG_FINAL}.hex ${CMAKE_BINARY_DIR}/release/${hex_variant_name}_${FN_VERSION_SUFFIX}_MULTILANG.hex
-      BYPRODUCTS ${CMAKE_BINARY_DIR}/release/${hex_variant_name}_${FN_VERSION_SUFFIX}_MULTILANG.hex
-      DEPENDS ${FW_LANG_PATCH}.hex ${LANG_CATHEX}
-      COMMENT "Generating final ${FW_LANG_FINAL}.hex"
-      )
-
-    add_custom_target(${FW_LANG_FINAL} DEPENDS ${FW_LANG_FINAL}.hex)
-    add_dependencies(ALL_MULTILANG ${FW_LANG_FINAL})
-  else()
-    set(ALL_VARIANT_HEXES "")
-    # Non-xflash, e.g. MK2.5
-    foreach(LANG IN LISTS SELECTED_LANGUAGES)
-      set(FW_LANG_FINAL ${variant_name}_en-${LANG})
-      set(LANG_HEX ${CMAKE_BINARY_DIR}/${variant_name}_${FN_VERSION_SUFFIX}_en-${LANG}.hex)
-      set(LANG_DEBUG_HEX ${CMAKE_BINARY_DIR}/${variant_name}_${FN_VERSION_DEBUG_SUFFIX}_en-${LANG}.hex)
-      set(LANG_BIN ${LANG_TMP_DIR}/${variant_name}_${LANG}.bin)
-
-      # Patched binary with pre-baked secondary language
-      add_custom_command(
-        OUTPUT ${FW_LANG_FINAL}.bin
-        COMMAND ${CMAKE_COMMAND} -E copy ${FW_LANG_PATCH}.bin ${FW_LANG_FINAL}.bin
-        COMMAND "${Python3_EXECUTABLE}" ${CMAKE_SOURCE_DIR}/lang/lang-patchsec.py ${FW_LANG_BASE} ${LANG_BIN}
-                ${FW_LANG_FINAL}.bin
-        DEPENDS ${FW_LANG_BASE} ${FW_LANG_PATCH}.bin ${LANG_BIN}
-        COMMENT "Generating ${FW_LANG_FINAL}.bin"
-        )
-
-      # Final hex files
-      add_custom_command(
-        OUTPUT ${FW_LANG_FINAL}.hex
-        COMMAND ${CMAKE_OBJCOPY} -I binary -O ihex ${FW_LANG_FINAL}.bin ${FW_LANG_FINAL}.hex
-        COMMAND ${CMAKE_COMMAND} -E create_hardlink ${FW_LANG_FINAL}.hex ${LANG_HEX}
-        BYPRODUCTS ${LANG_HEX}
-        COMMAND ${CMAKE_COMMAND} -E create_hardlink ${FW_LANG_FINAL}.hex ${LANG_DEBUG_HEX}
-        BYPRODUCTS ${LANG_DEBUG_HEX}
-        DEPENDS ${FW_LANG_FINAL}.bin
-        COMMENT "Creating ${FW_LANG_FINAL}.hex"
-        )
-
-      add_custom_target(${FW_LANG_FINAL} DEPENDS ${FW_LANG_FINAL}.hex)
-      list(APPEND ALL_VARIANT_HEXES ${FW_LANG_FINAL})
-    endforeach()
-    add_custom_target("${variant_name}-All-Languages" DEPENDS ${ALL_VARIANT_HEXES})
-    if(CMAKE_HOST_SYSTEM_NAME STREQUAL "Linux")
-      add_custom_command(TARGET "${variant_name}-All-Languages"
-        POST_BUILD
-        WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
-        COMMAND ${CMAKE_COMMAND} -E make_directory "release"
-        COMMAND ${CMAKE_COMMAND} -E tar "cfv" "release/${variant_name}_${FN_VERSION_SUFFIX}.zip" --format=zip ${variant_name}_${FN_VERSION_SUFFIX}_en*.hex
-        BYPRODUCTS "${CMAKE_BINARY_DIR}/release/${variant_name}_${FN_VERSION_SUFFIX}.zip"
-        )
-    endif()
-    add_dependencies(ALL_MULTILANG "${variant_name}-All-Languages")
-  endif()
-endfunction()
-
-if(CMAKE_CROSSCOMPILING)
-
-  # Main target for language checks
-  add_custom_target(check_lang)
-  foreach(LANG IN LISTS SELECTED_LANGUAGES)
-    add_custom_target(check_lang_${LANG})
-    add_dependencies(check_lang check_lang_${LANG})
-  endforeach()
-
-  # build a list of all supported variants
-  file(
-    GLOB ALL_VARIANTS
-    RELATIVE ${PROJECT_SOURCE_DIR}/Firmware/variants
-    ${PROJECT_SOURCE_DIR}/Firmware/variants/*.h
-    )
-  list(TRANSFORM ALL_VARIANTS REPLACE "\.h$" "")
-  set(FW_VARIANTS
-      ${ALL_VARIANTS}
-      CACHE STRING "Firmware variants to be built"
-      )
-
-  foreach(THIS_VAR IN LISTS FW_VARIANTS)
-    if(NOT ${THIS_VAR} IN_LIST ALL_VARIANTS)
-      message(FATAL_ERROR "Variant ${THIS_VAR} does not exist")
-    endif()
-
-    message("Variant added: ${THIS_VAR}")
-
-    # Generate a file in a subfolder so that we can organize things a little more neatly in VS code
-    set(DIR_NAME ${THIS_VAR})
-    file(MAKE_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/build_gen/${DIR_NAME})
-    file(WRITE ${CMAKE_CURRENT_SOURCE_DIR}/build_gen/${DIR_NAME}/CMakeLists.txt
-         "project(${DIR_NAME} VERSION ${PROJECT_VERSION})\nfw_add_variant(${THIS_VAR})"
-         )
-    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/build_gen/${DIR_NAME})
-  endforeach(THIS_VAR IN LISTS FW_VARIANTS)
-endif()
-
-SET(REVO_FW_ZIP_NAME "E3D_REVO_FW_MK3_MK3S_MK3S+_${FN_VERSION_SUFFIX}.zip")
-
-if(CMAKE_HOST_SYSTEM_NAME STREQUAL "Linux")
-  add_custom_command(TARGET ALL_MULTILANG
-    POST_BUILD
-    WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/release
-    BYPRODUCTS ${CMAKE_BINARY_DIR}/release/${REVO_FW_ZIP_NAME}
-    COMMAND ${CMAKE_COMMAND} -E tar "cfv" "${REVO_FW_ZIP_NAME}" --format=zip *E3DREVO*.hex
-    COMMAND ${CMAKE_COMMAND} -E rm *E3DREVO*.hex
-    )
-endif()
-
-#
-# Tests
-#
-if(NOT CMAKE_CROSSCOMPILING)
-  enable_testing()
-  add_subdirectory(tests)
-endif()
+cmake_minimum_required(VERSION 3.1)
+
+set (CMAKE_CXX_STANDARD 11)
+
+project(cmake_test)
+
+# Prepare "Catch" library for other executables
+set(CATCH_INCLUDE_DIR Catch2)
+add_library(Catch INTERFACE)
+target_include_directories(Catch INTERFACE ${CATCH_INCLUDE_DIR})
+
+# Make test executable
+set(TEST_SOURCES 
+	Tests/tests.cpp
+	Tests/Example_test.cpp
+	Tests/Timer_test.cpp
+	Tests/AutoDeplete_test.cpp
+	Tests/PrusaStatistics_test.cpp
+	Firmware/Timer.cpp
+	Firmware/AutoDeplete.cpp
+)
+add_executable(tests ${TEST_SOURCES})
+target_include_directories(tests PRIVATE Tests)
+target_link_libraries(tests Catch)
--- a/Catch2/catch.hpp
+++ b/Catch2/catch.hpp
--- a/Community_made.md
+++ b/Community_made.md
@ -1,65 +0,0 @@
-# Community made
-Community made features aren't fully supported by Prusa and the owners are responsible to maintain them.
-In case the feature isn't maintained OR causes issues Prusa will deactivate these.
-## Prusa-Firmware build
- `PF-build.sh`
-  - Maintainers: **@3d-gussner**
-  - Co-maintainers:
-  - Contributors: **@mkbel**, **@ropaha**, **@deliopoulos**, **@DRracer**, **wavexx**, **@leptun**, **@andrewluebke**, **@kuhnmarek**
-  - [X] **Active**      since February 2019
-  - [X] **Maintained**  since January 2019
-
-### How-to use PF-build.sh
-Start `./PF-build.sh` and follow the instructions
-
-Help `./PF-build.sh -h`
-
-# MK404 Simulator
-
-## MK404-build.sh
-**MK404 is a community 3d printer simulator created by @vintagepc**
-Please checkout and support his github repository [MK404](https://github.com/vintagepc/MK404) and the [MK404 Wiki](https://github.com/vintagepc/MK404/wiki)
-
-At this moment the `MK404-build.sh` script is only supported on Linux
- `MK404-build.sh`
-  - Maintainers: **@3d-gussner**
-  - Co-maintainers:
-  - Contributors:
-  - [X] **Active**      since August 2021
-  - [X] **Maintained**  since August 2021
-
-### How-to use MK404-build.sh
-After compiling with `PF-build.sh` you get the option to start the `MK404` simulator with the fresh compiled firmware. (Linux only at this moment)
-
-Help `./MK404-build.sh -h`
-
-# Translations
- see [/lang/Community_made_translations.md](https://github.com/prusa3d/Prusa-Firmware/blob/MK3/lang/Community_made_translations.md)
-
-# Arc interpolation features
-**Arc interpolation features by @FormerLurker**
-Please read more about it [here](https://github.com/prusa3d/Prusa-Firmware/pull/2657) and [here](https://github.com/FormerLurker/ArcWelderPlugin)
-
- Maintainers: **@FormerLurker**
- Co-maintainers:
- Contributors:
- [X] **Active**      since January 2023
- [X] **Maintained**  since May 2020
-
-# MeatPack
-**MeatPack by @scottmudge**
-Please read more about it [here](https://github.com/prusa3d/Prusa-Firmware/pull/2955), [here](https://github.com/prusa3d/Prusa-Firmware/pull/4067) and [here](https://github.com/scottmudge/OctoPrint-MeatPack/)
-
- Maintainers: **@scottmudge**
- Co-maintainers:
- Contributors:
- [X] **Active**      since April 2023
- [X] **Maintained**  since January 2021
-
-# E3D Revo
-**The E3D REVO support is a community effort thanks to these Contributors, E3D and others.**
- Maintainers: **E3D**
- Co-maintainers:
- Contributors: @alexiri @kromeninja @ulab @JWvP @snafu1282 @matthiazzz @sdh2 @jdrozdz @peschkaj @MarcelTh @zuidwijk @davejhilton @WhiterRice @NightSkySK @D-an-W
- [X] **Active**      since June 2023
- [X] **Maintained**  since April 2023
--- a/Firmware/AutoDeplete.cpp
+++ b/Firmware/AutoDeplete.cpp
@ -0,0 +1,79 @@
+//! @file
+//! @author: Marek Bel
+//! @date Jan 3, 2019
+
+#include "AutoDeplete.h"
+#include "assert.h"
+
+//! @brief bit field marking depleted filaments
+//!
+//! binary 1 marks filament as depleted
+//! Zero initialized value means, that no filament is depleted.
+static uint8_t depleted;
+static const uint8_t filamentCount = 5;
+
+//! @return binary 1 for all filaments
+//! @par fCount number of filaments
+static constexpr uint8_t allDepleted(uint8_t fCount)
+{
+    return fCount == 1 ? 1 : ((1 << (fCount - 1)) | allDepleted(fCount - 1));
+}
+
+//! @brief Is filament available for printing?
+//! @par filament Filament number to be checked
+//! @retval true Filament is available for printing.
+//! @retval false Filament is not available for printing.
+static bool loaded(uint8_t filament)
+{
+    if (depleted & (1 << filament)) return false;
+    return true;
+}
+
+//! @brief Mark filament as not available for printing.
+//! @par filament filament to be marked
+void ad_markDepleted(uint8_t filament)
+{
+    assert(filament < filamentCount);
+    if (filament < filamentCount)
+    {
+        depleted |= 1 << filament;
+    }
+}
+
+//! @brief Mark filament as available for printing.
+//! @par filament filament to be marked
+void ad_markLoaded(uint8_t filament)
+{
+    assert(filament < filamentCount);
+    if (filament < filamentCount)
+    {
+        depleted &= ~(1 << filament);
+    }
+}
+
+//! @brief Get alternative filament, which is not depleted
+//! @par filament filament
+//! @return Filament, if it is depleted, returns next available,
+//! if all filaments are depleted, returns filament function parameter.
+uint8_t ad_getAlternative(uint8_t filament)
+{
+    assert(filament < filamentCount);
+    for (uint8_t i = 0; i<filamentCount; ++i)
+    {
+        uint8_t nextFilament = (filament + i) % filamentCount;
+        if (loaded(nextFilament)) return nextFilament;
+    }
+    return filament;
+}
+
+//! @brief Are all filaments depleted?
+//! @retval true All filaments are depleted.
+//! @retval false All filaments are not depleted.
+bool ad_allDepleted()
+{
+    if (allDepleted(filamentCount) == depleted)
+    {
+        return true;
+    }
+    return false;
+}
--- a/Firmware/AutoDeplete.h
+++ b/Firmware/AutoDeplete.h
@ -0,0 +1,17 @@
+//! @file
+//! @author: Marek Bel
+//! @brief Filament auto deplete engine for multi-material prints with MMUv2 (Now marketed as SpoolJoin)
+//!
+//! Interface for marking MMUv2 filaments as depleted and getting alternative filament for printing.
+
+#ifndef AUTODEPLETE_H
+#define AUTODEPLETE_H
+
+#include <stdint.h>
+
+void ad_markDepleted(uint8_t filament);
+void ad_markLoaded(uint8_t filament);
+uint8_t ad_getAlternative(uint8_t filament);
+bool ad_allDepleted();
+
+#endif /* AUTODEPLETE_H */
--- a/Firmware/BlinkM.cpp
+++ b/Firmware/BlinkM.cpp
@ -15,7 +15,7 @@

 void SendColors(byte red, byte grn, byte blu)
 {
-  Wire.begin();
+  Wire.begin(); 
  Wire.beginTransmission(0x09);
  Wire.write('o');                    //to disable ongoing script, only needs to be used once
  Wire.write('n');
--- a/Firmware/Configuration.cpp
+++ b/Firmware/Configuration.cpp
@ -1,9 +1,10 @@
 #include "Configuration.h"
-#include "Configuration_var.h"
+#include "Configuration_prusa.h"

 const uint16_t _nPrinterType PROGMEM=PRINTER_TYPE;
 const char _sPrinterName[] PROGMEM=PRINTER_NAME;
 const uint16_t _nPrinterMmuType PROGMEM=PRINTER_MMU_TYPE;
 const char _sPrinterMmuName[] PROGMEM=PRINTER_MMU_NAME;

-static_assert(TEMP_HYSTERESIS > 0, "TEMP_HYSTERESIS must be greater than 0");
+uint16_t nPrinterType;
+PGM_P sPrinterName;
--- a/Firmware/Configuration.h
+++ b/Firmware/Configuration.h
@ -1,61 +1,58 @@
 #ifndef CONFIGURATION_H
 #define CONFIGURATION_H

-#include <stdint.h>
 #include "boards.h"

 #define STR_HELPER(x) #x
 #define STR(x) STR_HELPER(x)
-#define _CONCAT(x,y) x##y
-#define CONCAT(x,y) _CONCAT(x,y)

+//-//
 #include <avr/pgmspace.h>
 extern const uint16_t _nPrinterType;
 extern const char _sPrinterName[] PROGMEM;
 extern const uint16_t _nPrinterMmuType;
 extern const char _sPrinterMmuName[] PROGMEM;
+extern uint16_t nPrinterType;
+extern PGM_P sPrinterName;

-// Firmware version.
-// NOTE: These are ONLY used if you are not building via cmake and/or not in a git repository.
-// Otherwise the repository information takes precedence.
-#ifndef CMAKE_CONTROL
-#define FW_MAJOR 3
-#define FW_MINOR 14
-#define FW_REVISION 1
-#define FW_COMMITNR 8225
-#define FW_FLAVOR RC      //uncomment if DEV, ALPHA, BETA or RC
-#define FW_FLAVERSION 1     //uncomment if FW_FLAVOR is defined and versioning is needed. Limited to max 8.
-#endif
-
-#ifndef FW_FLAVOR
-    #define FW_TWEAK (FIRMWARE_REVISION_RELEASED)
-    #define FW_VERSION STR(FW_MAJOR) "." STR(FW_MINOR) "." STR(FW_REVISION)
-    #define FW_VERSION_FULL STR(FW_MAJOR) "." STR(FW_MINOR) "." STR(FW_REVISION) "-" STR(FW_COMMITNR)
-#else
-    // Construct the TWEAK value as it is expected from the enum.
-    #define FW_TWEAK (CONCAT(FIRMWARE_REVISION_,FW_FLAVOR) + FW_FLAVERSION)
-    #define FW_VERSION STR(FW_MAJOR) "." STR(FW_MINOR) "." STR(FW_REVISION) "-" STR(FW_FLAVOR) "" STR(FW_FLAVERSION)
-    #define FW_VERSION_FULL STR(FW_MAJOR) "." STR(FW_MINOR) "." STR(FW_REVISION) "-" STR(FW_FLAVOR) "" STR(FW_FLAVERSION) "+" STR(FW_COMMITNR)
-#endif
-
-// The full version string and repository source are set via cmake
-#ifndef CMAKE_CONTROL
-#define FW_COMMIT_HASH_LENGTH 1
-#define FW_COMMIT_HASH "0"
+// Firmware version
+#define FW_VERSION "3.9.3-RC1"
+#define FW_COMMIT_NR 3555
+// FW_VERSION_UNKNOWN means this is an unofficial build.
+// The firmware should only be checked into github with this symbol.
+#define FW_DEV_VERSION FW_VERSION_UNKNOWN
 #define FW_REPOSITORY "Unknown"
-#endif
+#define FW_VERSION_FULL FW_VERSION "-" STR(FW_COMMIT_NR)

 // G-code language level
 #define GCODE_LEVEL 1

-#ifndef SOURCE_DATE_EPOCH
-#define SOURCE_DATE_EPOCH __DATE__
-#endif
-#ifndef SOURCE_TIME_EPOCH
-#define SOURCE_TIME_EPOCH __TIME__
+// Debug version has debugging enabled (the symbol DEBUG_BUILD is set).
+// The debug build may be a bit slower than the non-debug build, therefore the debug build should
+// not be shipped to a customer.
+#define FW_VERSION_DEBUG    6
+// This is a development build. A development build is either built from an unofficial git repository, 
+// or from an unofficial branch, or it does not have a label set. Only the build server should set this build type.
+#define FW_VERSION_DEVEL    5
+// This is an alpha release. Only the build server should set this build type.
+#define FW_VERSION_ALPHA    4
+// This is a beta release. Only the build server should set this build type.
+#define FW_VERSION_BETA     3
+// This is a release candidate build. Only the build server should set this build type.
+#define FW_VERSION_RC       2
+// This is a final release. Only the build server should set this build type.
+#define FW_VERSION_GOLD     1
+// This is an unofficial build. The firmware should only be checked into github with this symbol,
+// the build server shall never produce builds with this build type.
+#define FW_VERSION_UNKNOWN  0
+
+#if FW_DEV_VERSION == FW_VERSION_DEBUG
+#define DEBUG_BUILD
+#else
+#undef DEBUG_BUILD
 #endif

-#include "Configuration_var.h"
+#include "Configuration_prusa.h"

 #define FW_PRUSA3D_MAGIC "PRUSA3DFW"
 #define FW_PRUSA3D_MAGIC_LEN 10
@ -70,8 +67,10 @@ extern const char _sPrinterMmuName[] PROGMEM;
 // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this
 // build by the user have been successfully uploaded into firmware.

-#define STRING_VERSION_CONFIG_H SOURCE_DATE_EPOCH " " SOURCE_TIME_EPOCH // build date and time
-#define STRING_CONFIG_H_AUTHOR FW_REPOSITORY // Who made the changes.
+//#define STRING_VERSION "1.0.2"
+
+#define STRING_VERSION_CONFIG_H __DATE__ " " __TIME__ // build date and time
+#define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.

 // SERIAL_PORT selects which serial port should be used for communication with the host.
 // This allows the connection of wireless adapters (for instance) to non-default port pins.
@ -81,9 +80,6 @@ extern const char _sPrinterMmuName[] PROGMEM;
 // This determines the communication speed of the printer
 #define BAUDRATE 115200

-// Enable g-code compression (see https://github.com/scottmudge/OctoPrint-MeatPack)
-#define ENABLE_MEATPACK
-
 // This enables the serial port associated to the Bluetooth interface
 //#define BTENABLED              // Enable BT interface on AT90USB devices

@ -122,6 +118,13 @@ extern const char _sPrinterMmuName[] PROGMEM;
 #define TEMP_HYSTERESIS 5       // (degC) range of +/- temperatures considered "close" to the target one
 #define TEMP_WINDOW     1       // (degC) Window around target to start the residency timer x degC early.

+
+
+// If your bed has low resistance e.g. .6 ohm and throws the fuse you can duty cycle it to reduce the
+// average current. The value should be an integer and the heat bed will be turned on for 1 interval of
+// HEATER_BED_DUTY_CYCLE_DIVIDER intervals.
+//#define HEATER_BED_DUTY_CYCLE_DIVIDER 4
+
 // If you want the M105 heater power reported in watts, define the BED_WATTS, and (shared for all extruders) EXTRUDER_WATTS
 //#define EXTRUDER_WATTS (12.0*12.0/6.7) //  P=I^2/R
 //#define BED_WATTS (12.0*12.0/1.1)      // P=I^2/R
@ -141,7 +144,7 @@ extern const char _sPrinterMmuName[] PROGMEM;

 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // Ultimaker
-
+    

 // MakerGear
 //    #define  DEFAULT_Kp 7.0
@ -177,15 +180,15 @@ The issue: If a thermistor come off, it will read a lower temperature than actua
 The system will turn the heater on forever, burning up the filament and anything
 else around.

-After the temperature reaches the target for the first time, this feature will
-start measuring for how long the current temperature stays below the target
+After the temperature reaches the target for the first time, this feature will 
+start measuring for how long the current temperature stays below the target 
 minus _HYSTERESIS (set_temperature - THERMAL_RUNAWAY_PROTECTION_HYSTERESIS).

 If it stays longer than _PERIOD, it means the thermistor temperature
 cannot catch up with the target, so something *may be* wrong. Then, to be on the
 safe side, the system will he halt.

-Bear in mind the count down will just start AFTER the first time the
+Bear in mind the count down will just start AFTER the first time the 
 thermistor temperature is over the target, so you will have no problem if
 your extruder heater takes 2 minutes to hit the target on heating.

@ -259,6 +262,7 @@ your extruder heater takes 2 minutes to hit the target on heating.
 #define DISABLE_Y 0
 #define DISABLE_Z 0
 #define DISABLE_E 0// For all extruders
+#define DISABLE_INACTIVE_EXTRUDER 1 //disable only inactive extruders and keep active extruder enabled


 // ENDSTOP SETTINGS:
@ -277,13 +281,131 @@ your extruder heater takes 2 minutes to hit the target on heating.


 #define X_MAX_LENGTH (X_MAX_POS - X_MIN_POS)
-#define Y_MAX_LENGTH (Y_MAX_POS - Y_MIN_POS)
+#define Y_MAX_LENGTH (Y_MAX_POS - Y_MIN_POS) 
 #define Z_MAX_LENGTH (Z_MAX_POS - Z_MIN_POS)

 #define Z_HEIGHT_HIDE_LIVE_ADJUST_MENU 2.0f

 #define HOME_Z_SEARCH_THRESHOLD 0.15f             // Threshold of the Z height in calibration

+//============================= Bed Auto Leveling ===========================
+
+//#define ENABLE_AUTO_BED_LEVELING // Delete the comment to enable (remove // at the start of the line)
+#define Z_PROBE_REPEATABILITY_TEST  // If not commented out, Z-Probe Repeatability test will be included if Auto Bed Leveling is Enabled.
+
+#ifdef ENABLE_AUTO_BED_LEVELING
+
+// There are 2 different ways to pick the X and Y locations to probe:
+
+//  - "grid" mode
+//    Probe every point in a rectangular grid
+//    You must specify the rectangle, and the density of sample points
+//    This mode is preferred because there are more measurements.
+//    It used to be called ACCURATE_BED_LEVELING but "grid" is more descriptive
+
+//  - "3-point" mode
+//    Probe 3 arbitrary points on the bed (that aren't colinear)
+//    You must specify the X & Y coordinates of all 3 points
+
+  #define AUTO_BED_LEVELING_GRID
+  // with AUTO_BED_LEVELING_GRID, the bed is sampled in a
+  // AUTO_BED_LEVELING_GRID_POINTSxAUTO_BED_LEVELING_GRID_POINTS grid
+  // and least squares solution is calculated
+  // Note: this feature occupies 10'206 byte
+  #ifdef AUTO_BED_LEVELING_GRID
+
+    // set the rectangle in which to probe
+    #define LEFT_PROBE_BED_POSITION 15
+    #define RIGHT_PROBE_BED_POSITION 170
+    #define BACK_PROBE_BED_POSITION 180
+    #define FRONT_PROBE_BED_POSITION 20
+
+     // set the number of grid points per dimension
+     // I wouldn't see a reason to go above 3 (=9 probing points on the bed)
+    #define AUTO_BED_LEVELING_GRID_POINTS 2
+
+
+  #else  // not AUTO_BED_LEVELING_GRID
+    // with no grid, just probe 3 arbitrary points.  A simple cross-product
+    // is used to esimate the plane of the print bed
+
+      #define ABL_PROBE_PT_1_X 15
+      #define ABL_PROBE_PT_1_Y 180
+      #define ABL_PROBE_PT_2_X 15
+      #define ABL_PROBE_PT_2_Y 20
+      #define ABL_PROBE_PT_3_X 170
+      #define ABL_PROBE_PT_3_Y 20
+
+  #endif // AUTO_BED_LEVELING_GRID
+
+
+  // these are the offsets to the probe relative to the extruder tip (Hotend - Probe)
+  // X and Y offsets must be integers
+  #define X_PROBE_OFFSET_FROM_EXTRUDER -25
+  #define Y_PROBE_OFFSET_FROM_EXTRUDER -29
+  #define Z_PROBE_OFFSET_FROM_EXTRUDER -12.35
+
+  #define Z_RAISE_BEFORE_HOMING 5       // (in mm) Raise Z before homing (G28) for Probe Clearance.
+                                        // Be sure you have this distance over your Z_MAX_POS in case
+
+  #define XY_TRAVEL_SPEED 8000         // X and Y axis travel speed between probes, in mm/min
+
+  #define Z_RAISE_BEFORE_PROBING 15    //How much the extruder will be raised before traveling to the first probing point.
+  #define Z_RAISE_BETWEEN_PROBINGS 5  //How much the extruder will be raised when traveling from between next probing points
+
+  //#define Z_PROBE_SLED // turn on if you have a z-probe mounted on a sled like those designed by Charles Bell
+  //#define SLED_DOCKING_OFFSET 5 // the extra distance the X axis must travel to pickup the sled. 0 should be fine but you can push it further if you'd like.
+
+  //If defined, the Probe servo will be turned on only during movement and then turned off to avoid jerk
+  //The value is the delay to turn the servo off after powered on - depends on the servo speed; 300ms is good value, but you can try lower it.
+  // You MUST HAVE the SERVO_ENDSTOPS defined to use here a value higher than zero otherwise your code will not compile.
+
+//  #define PROBE_SERVO_DEACTIVATION_DELAY 300
+
+
+//If you have enabled the Bed Auto Leveling and are using the same Z Probe for Z Homing,
+//it is highly recommended you let this Z_SAFE_HOMING enabled!
+
+  //#define Z_SAFE_HOMING   // This feature is meant to avoid Z homing with probe outside the bed area.
+                          // When defined, it will:
+                          // - Allow Z homing only after X and Y homing AND stepper drivers still enabled
+                          // - If stepper drivers timeout, it will need X and Y homing again before Z homing
+                          // - Position the probe in a defined XY point before Z Homing when homing all axis (G28)
+                          // - Block Z homing only when the probe is outside bed area.
+
+  #ifdef Z_SAFE_HOMING
+
+    #define Z_SAFE_HOMING_X_POINT (X_MAX_LENGTH/2)    // X point for Z homing when homing all axis (G28)
+    #define Z_SAFE_HOMING_Y_POINT (Y_MAX_LENGTH/2)    // Y point for Z homing when homing all axis (G28)
+
+  #endif
+
+  #ifdef AUTO_BED_LEVELING_GRID	// Check if Probe_Offset * Grid Points is greater than Probing Range
+    #if X_PROBE_OFFSET_FROM_EXTRUDER < 0
+      #if (-(X_PROBE_OFFSET_FROM_EXTRUDER * AUTO_BED_LEVELING_GRID_POINTS) >= (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION))
+	     #error "The X axis probing range is not enough to fit all the points defined in AUTO_BED_LEVELING_GRID_POINTS"
+	  #endif
+	#else
+      #if ((X_PROBE_OFFSET_FROM_EXTRUDER * AUTO_BED_LEVELING_GRID_POINTS) >= (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION))
+	     #error "The X axis probing range is not enough to fit all the points defined in AUTO_BED_LEVELING_GRID_POINTS"
+	  #endif
+	#endif
+    #if Y_PROBE_OFFSET_FROM_EXTRUDER < 0
+      #if (-(Y_PROBE_OFFSET_FROM_EXTRUDER * AUTO_BED_LEVELING_GRID_POINTS) >= (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION))
+	     #error "The Y axis probing range is not enough to fit all the points defined in AUTO_BED_LEVELING_GRID_POINTS"
+	  #endif
+	#else
+      #if ((Y_PROBE_OFFSET_FROM_EXTRUDER * AUTO_BED_LEVELING_GRID_POINTS) >= (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION))
+	     #error "The Y axis probing range is not enough to fit all the points defined in AUTO_BED_LEVELING_GRID_POINTS"
+	  #endif
+	#endif
+
+	
+  #endif
+  
+#endif // ENABLE_AUTO_BED_LEVELING
+
+
 // The position of the homing switches
 //#define MANUAL_HOME_POSITIONS  // If defined, MANUAL_*_HOME_POS below will be used
 //#define BED_CENTER_AT_0_0  // If defined, the center of the bed is at (X=0, Y=0)
@ -291,6 +413,13 @@ your extruder heater takes 2 minutes to hit the target on heating.
 //Manual homing switch locations:
 // For deltabots this means top and center of the Cartesian print volume.

+
+// Offset of the extruders (uncomment if using more than one and relying on firmware to position when changing).
+// The offset has to be X=0, Y=0 for the extruder 0 hotend (default extruder).
+// For the other hotends it is their distance from the extruder 0 hotend.
+// #define EXTRUDER_OFFSET_X {0.0, 20.00} // (in mm) for each extruder, offset of the hotend on the X axis
+// #define EXTRUDER_OFFSET_Y {0.0, 5.00}  // (in mm) for each extruder, offset of the hotend on the Y axis
+
 // The speed change that does not require acceleration (i.e. the software might assume it can be done instantaneously)
 #define DEFAULT_XJERK                10       // (mm/sec)
 #define DEFAULT_YJERK                10       // (mm/sec)
@ -301,6 +430,26 @@ your extruder heater takes 2 minutes to hit the target on heating.
 //=============================Additional Features===========================
 //===========================================================================

+// Custom M code points
+#define CUSTOM_M_CODES
+#ifdef CUSTOM_M_CODES
+  #define CUSTOM_M_CODE_SET_Z_PROBE_OFFSET 851
+  #define Z_PROBE_OFFSET_RANGE_MIN -15
+  #define Z_PROBE_OFFSET_RANGE_MAX -5
+#endif
+
+
+// EEPROM
+// The microcontroller can store settings in the EEPROM, e.g. max velocity...
+// M500 - stores parameters in EEPROM
+// M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
+// M502 - reverts to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
+//define this to enable EEPROM support
+//#define EEPROM_SETTINGS
+//to disable EEPROM Serial responses and decrease program space by ~1700 byte: comment this out:
+// please keep turned on if you can.
+//#define EEPROM_CHITCHAT
+
 // Host Keepalive
 //
 // When enabled Marlin will send a busy status message to the host
@ -324,15 +473,16 @@ your extruder heater takes 2 minutes to hit the target on heating.
 #define SDSUPPORT
 #define LCD_WIDTH 20
 #define LCD_HEIGHT 4
-#define LCD_BACKLIGHT_LEVEL_HIGH 130
-#define LCD_BACKLIGHT_LEVEL_LOW 50
-#define LCD_BACKLIGHT_FORCE_ON 30
-#define LCD_BACKLIGHT_TIMEOUT 15


 // Increase the FAN pwm frequency. Removes the PWM noise but increases heating in the FET/Arduino
 //#define FAST_PWM_FAN

+// Temperature status LEDs that display the hotend and bet temperature.
+// If all hotends and bed temperature and temperature setpoint are < 54C then the BLUE led is on.
+// Otherwise the RED led is on. There is 1C hysteresis.
+//#define TEMP_STAT_LEDS
+
 // Use software PWM to drive the fan, as for the heaters. This uses a very low frequency
 // which is not ass annoying as with the hardware PWM. On the other hand, if this frequency
 // is too low, you should also increment SOFT_PWM_SCALE.
@ -358,16 +508,55 @@ your extruder heater takes 2 minutes to hit the target on heating.
 //define BlinkM/CyzRgb Support
 //#define BLINKM

+/*********************************************************************\
+* R/C SERVO support
+* Sponsored by TrinityLabs, Reworked by codexmas
+**********************************************************************/
+
+// Number of servos
+//
+// If you select a configuration below, this will receive a default value and does not need to be set manually
+// set it manually if you have more servos than extruders and wish to manually control some
+// leaving it undefined or defining as 0 will disable the servo subsystem
+// If unsure, leave commented / disabled
+//
+//#define NUM_SERVOS 3 // Servo index starts with 0 for M280 command
+
 #define DEFAULT_NOMINAL_FILAMENT_DIA  1.75  //Enter the diameter (in mm) of the filament generally used (3.0 mm or 1.75 mm). Used by the volumetric extrusion.

+// Calibration status of the machine, to be stored into the EEPROM,
+// (unsigned char*)EEPROM_CALIBRATION_STATUS
+enum CalibrationStatus
+{
+	// Freshly assembled, needs to peform a self-test and the XYZ calibration.
+	CALIBRATION_STATUS_ASSEMBLED = 255,
+
+	// For the wizard: self test has been performed, now the XYZ calibration is needed.
+	CALIBRATION_STATUS_XYZ_CALIBRATION = 250,
+
+	// For the wizard: factory assembled, needs to run Z calibration.
+	CALIBRATION_STATUS_Z_CALIBRATION = 240,
+
+	// The XYZ calibration has been performed, now it remains to run the V2Calibration.gcode.
+	CALIBRATION_STATUS_LIVE_ADJUST = 230,
+
+    // Calibrated, ready to print.
+    CALIBRATION_STATUS_CALIBRATED = 1,
+
+    // Legacy: resetted by issuing a G86 G-code.
+    // This value can only be expected after an upgrade from the initial MK2 firmware releases.
+    // Currently the G86 sets the calibration status to 
+    CALIBRATION_STATUS_UNKNOWN = 0,
+};
+
 // Try to maintain a minimum distance from the bed even when Z is
 // unknown when doing the following operations
-#define MIN_Z_FOR_LOAD    35 // lcd filament loading or autoload (values for load and unload have been unified to prevent movement between unload & load operations!)
-#define MIN_Z_FOR_UNLOAD  35 // lcd filament unloading
-#define MIN_Z_FOR_SWAP    27 // filament change (including M600)
-#define MIN_Z_FOR_PREHEAT 10 // lcd preheat
+#define MIN_Z_FOR_LOAD    50
+#define MIN_Z_FOR_UNLOAD  20
+#define MIN_Z_FOR_PREHEAT 10

 #include "Configuration_adv.h"
 #include "thermistortables.h"

+
 #endif //__CONFIGURATION_H
--- a/Firmware/ConfigurationStore.cpp
+++ b/Firmware/ConfigurationStore.cpp
@ -3,112 +3,172 @@
 #include "Marlin.h"
 #include "planner.h"
 #include "temperature.h"
+#include "ultralcd.h"
 #include "ConfigurationStore.h"
-#include "Configuration_var.h"
+#include "Configuration_prusa.h"
+
+#ifdef MESH_BED_LEVELING
+#include "mesh_bed_leveling.h"
+#endif

 #ifdef TMC2130
 #include "tmc2130.h"
 #endif

+
 M500_conf cs;

+//! @brief Write data to EEPROM
+//! @param pos destination in EEPROM, 0 is start
+//! @param value value to be written
+//! @param size size of type pointed by value
+//! @param name name of variable written, used only for debug input if DEBUG_EEPROM_WRITE defined
+//! @retval true success
+//! @retval false failed
+#ifdef DEBUG_EEPROM_WRITE
+static bool EEPROM_writeData(uint8_t* pos, uint8_t* value, uint8_t size, const char* name)
+#else //DEBUG_EEPROM_WRITE
+static bool EEPROM_writeData(uint8_t* pos, uint8_t* value, uint8_t size, const char*)
+#endif //DEBUG_EEPROM_WRITE
+{
+#ifdef DEBUG_EEPROM_WRITE
+	printf_P(PSTR("EEPROM_WRITE_VAR addr=0x%04x size=0x%02hhx name=%s\n"), pos, size, name);
+#endif //DEBUG_EEPROM_WRITE
+	while (size--)
+	{
+
+        eeprom_update_byte(pos, *value);
+        if (eeprom_read_byte(pos) != *value) {
+            SERIAL_ECHOLNPGM("EEPROM Error");
+            return false;
+        }
+
+		pos++;
+		value++;
+	}
+    return true;
+}
+
+#ifdef DEBUG_EEPROM_READ
+static void EEPROM_readData(uint8_t* pos, uint8_t* value, uint8_t size, const char* name)
+#else //DEBUG_EEPROM_READ
+static void EEPROM_readData(uint8_t* pos, uint8_t* value, uint8_t size, const char*)
+#endif //DEBUG_EEPROM_READ
+{
+#ifdef DEBUG_EEPROM_READ
+	printf_P(PSTR("EEPROM_READ_VAR addr=0x%04x size=0x%02hhx name=%s\n"), pos, size, name);
+#endif //DEBUG_EEPROM_READ
+    while(size--)
+    {
+        *value = eeprom_read_byte(pos);
+        pos++;
+        value++;
+    }
+}
+
 #define EEPROM_VERSION "V2"

+#ifdef EEPROM_SETTINGS
+void Config_StoreSettings()
+{
+  strcpy(cs.version,"000"); //!< invalidate data first @TODO use erase to save one erase cycle
+  
+  if (EEPROM_writeData(reinterpret_cast<uint8_t*>(EEPROM_M500_base),reinterpret_cast<uint8_t*>(&cs),sizeof(cs),0), "cs, invalid version")
+  {
+      strcpy(cs.version,EEPROM_VERSION); //!< validate data if write succeed
+      EEPROM_writeData(reinterpret_cast<uint8_t*>(EEPROM_M500_base->version), reinterpret_cast<uint8_t*>(cs.version), sizeof(cs.version), "cs.version valid");
+  }
+
+  SERIAL_ECHO_START;
+  SERIAL_ECHOLNPGM("Settings Stored");
+}
+#endif //EEPROM_SETTINGS
+

 #ifndef DISABLE_M503
 void Config_PrintSettings(uint8_t level)
 {  // Always have this function, even with EEPROM_SETTINGS disabled, the current values will be shown
 #ifdef TMC2130
-  printf_P(PSTR(
-    "%SSteps per unit:\n%S  M92 X%.2f Y%.2f Z%.2f E%.2f\n"
+	printf_P(PSTR(
+		"%SSteps per unit:\n%S  M92 X%.2f Y%.2f Z%.2f E%.2f\n"
        "%SUStep resolution: \n%S M350 X%d Y%d Z%d E%d\n"
 		"%SMaximum feedrates - normal (mm/s):\n%S  M203 X%.2f Y%.2f Z%.2f E%.2f\n"
 		"%SMaximum feedrates - stealth (mm/s):\n%S  M203 X%.2f Y%.2f Z%.2f E%.2f\n"
 		"%SMaximum acceleration - normal (mm/s2):\n%S  M201 X%lu Y%lu Z%lu E%lu\n"
 		"%SMaximum acceleration - stealth (mm/s2):\n%S  M201 X%lu Y%lu Z%lu E%lu\n"
-		"%SAcceleration: P=print, R=retract, T=travel\n%S  M204 P%.2f R%.2f T%.2f\n"
-		"%SAdvanced variables: S=Min feedrate (mm/s), T=Min travel feedrate (mm/s), B=minimum segment time (us), X=maximum XY jerk (mm/s),  Z=maximum Z jerk (mm/s),  E=maximum E jerk (mm/s)\n%S  M205 S%.2f T%.2f B%lu X%.2f Y%.2f Z%.2f E%.2f\n"
+		"%SAcceleration: S=acceleration, T=retract acceleration\n%S  M204 S%.2f T%.2f\n"
+		"%SAdvanced variables: S=Min feedrate (mm/s), T=Min travel feedrate (mm/s), B=minimum segment time (ms), X=maximum XY jerk (mm/s),  Z=maximum Z jerk (mm/s),  E=maximum E jerk (mm/s)\n%S  M205 S%.2f T%.2f B%.2f X%.2f Y%.2f Z%.2f E%.2f\n"
 		"%SHome offset (mm):\n%S  M206 X%.2f Y%.2f Z%.2f\n"
 		),
-		echomagic, echomagic, cs.axis_steps_per_mm[X_AXIS], cs.axis_steps_per_mm[Y_AXIS], cs.axis_steps_per_mm[Z_AXIS], cs.axis_steps_per_mm[E_AXIS],
+		echomagic, echomagic, cs.axis_steps_per_unit[X_AXIS], cs.axis_steps_per_unit[Y_AXIS], cs.axis_steps_per_unit[Z_AXIS], cs.axis_steps_per_unit[E_AXIS],
 		echomagic, echomagic, cs.axis_ustep_resolution[X_AXIS], cs.axis_ustep_resolution[Y_AXIS], cs.axis_ustep_resolution[Z_AXIS], cs.axis_ustep_resolution[E_AXIS],
 		echomagic, echomagic, cs.max_feedrate_normal[X_AXIS], cs.max_feedrate_normal[Y_AXIS], cs.max_feedrate_normal[Z_AXIS], cs.max_feedrate_normal[E_AXIS],
 		echomagic, echomagic, cs.max_feedrate_silent[X_AXIS], cs.max_feedrate_silent[Y_AXIS], cs.max_feedrate_silent[Z_AXIS], cs.max_feedrate_silent[E_AXIS],
-		echomagic, echomagic, cs.max_acceleration_mm_per_s2_normal[X_AXIS], cs.max_acceleration_mm_per_s2_normal[Y_AXIS], cs.max_acceleration_mm_per_s2_normal[Z_AXIS], cs.max_acceleration_mm_per_s2_normal[E_AXIS],
-		echomagic, echomagic, cs.max_acceleration_mm_per_s2_silent[X_AXIS], cs.max_acceleration_mm_per_s2_silent[Y_AXIS], cs.max_acceleration_mm_per_s2_silent[Z_AXIS], cs.max_acceleration_mm_per_s2_silent[E_AXIS],
-		echomagic, echomagic, cs.acceleration, cs.retract_acceleration, cs.travel_acceleration,
-		echomagic, echomagic, cs.minimumfeedrate, cs.mintravelfeedrate, cs.min_segment_time_us, cs.max_jerk[X_AXIS], cs.max_jerk[Y_AXIS], cs.max_jerk[Z_AXIS], cs.max_jerk[E_AXIS],
+		echomagic, echomagic, cs.max_acceleration_units_per_sq_second_normal[X_AXIS], cs.max_acceleration_units_per_sq_second_normal[Y_AXIS], cs.max_acceleration_units_per_sq_second_normal[Z_AXIS], cs.max_acceleration_units_per_sq_second_normal[E_AXIS],
+		echomagic, echomagic, cs.max_acceleration_units_per_sq_second_silent[X_AXIS], cs.max_acceleration_units_per_sq_second_silent[Y_AXIS], cs.max_acceleration_units_per_sq_second_silent[Z_AXIS], cs.max_acceleration_units_per_sq_second_silent[E_AXIS],
+		echomagic, echomagic, cs.acceleration, cs.retract_acceleration,
+		echomagic, echomagic, cs.minimumfeedrate, cs.mintravelfeedrate, cs.minsegmenttime, cs.max_jerk[X_AXIS], cs.max_jerk[Y_AXIS], cs.max_jerk[Z_AXIS], cs.max_jerk[E_AXIS],
 		echomagic, echomagic, cs.add_homing[X_AXIS], cs.add_homing[Y_AXIS], cs.add_homing[Z_AXIS]
 #else //TMC2130
 	printf_P(PSTR(
 		"%SSteps per unit:\n%S  M92 X%.2f Y%.2f Z%.2f E%.2f\n"
 		"%SMaximum feedrates (mm/s):\n%S  M203 X%.2f Y%.2f Z%.2f E%.2f\n"
 		"%SMaximum acceleration (mm/s2):\n%S  M201 X%lu Y%lu Z%lu E%lu\n"
-		"%SAcceleration: P=print, R=retract, T=travel\n%S  M204 P%.2f R%.2f T%.2f\n"
-		"%SAdvanced variables: S=Min feedrate (mm/s), T=Min travel feedrate (mm/s), B=minimum segment time (us), X=maximum XY jerk (mm/s),  Z=maximum Z jerk (mm/s),  E=maximum E jerk (mm/s)\n%S  M205 S%.2f T%.2f B%lu X%.2f Y%.2f Z%.2f E%.2f\n"
+		"%SAcceleration: S=acceleration, T=retract acceleration\n%S  M204 S%.2f T%.2f\n"
+		"%SAdvanced variables: S=Min feedrate (mm/s), T=Min travel feedrate (mm/s), B=minimum segment time (ms), X=maximum XY jerk (mm/s),  Z=maximum Z jerk (mm/s),  E=maximum E jerk (mm/s)\n%S  M205 S%.2f T%.2f B%.2f X%.2f Y%.2f Z%.2f E%.2f\n"
 		"%SHome offset (mm):\n%S  M206 X%.2f Y%.2f Z%.2f\n"
 		),
-		echomagic, echomagic, cs.axis_steps_per_mm[X_AXIS], cs.axis_steps_per_mm[Y_AXIS], cs.axis_steps_per_mm[Z_AXIS], cs.axis_steps_per_mm[E_AXIS],
+		echomagic, echomagic, cs.axis_steps_per_unit[X_AXIS], cs.axis_steps_per_unit[Y_AXIS], cs.axis_steps_per_unit[Z_AXIS], cs.axis_steps_per_unit[E_AXIS],
 		echomagic, echomagic, max_feedrate[X_AXIS], max_feedrate[Y_AXIS], max_feedrate[Z_AXIS], max_feedrate[E_AXIS],
-		echomagic, echomagic, max_acceleration_mm_per_s2[X_AXIS], max_acceleration_mm_per_s2[Y_AXIS], max_acceleration_mm_per_s2[Z_AXIS], max_acceleration_mm_per_s2[E_AXIS],
-		echomagic, echomagic, cs.acceleration, cs.retract_acceleration, cs.travel_acceleration,
-		echomagic, echomagic, cs.minimumfeedrate, cs.mintravelfeedrate, cs.min_segment_time_us, cs.max_jerk[X_AXIS], cs.max_jerk[Y_AXIS], cs.max_jerk[Z_AXIS], cs.max_jerk[E_AXIS],
+		echomagic, echomagic, max_acceleration_units_per_sq_second[X_AXIS], max_acceleration_units_per_sq_second[Y_AXIS], max_acceleration_units_per_sq_second[Z_AXIS], max_acceleration_units_per_sq_second[E_AXIS],
+		echomagic, echomagic, cs.acceleration, cs.retract_acceleration,
+		echomagic, echomagic, cs.minimumfeedrate, cs.mintravelfeedrate, cs.minsegmenttime, cs.max_jerk[X_AXIS], cs.max_jerk[Y_AXIS], cs.max_jerk[Z_AXIS], cs.max_jerk[E_AXIS],
 		echomagic, echomagic, cs.add_homing[X_AXIS], cs.add_homing[Y_AXIS], cs.add_homing[Z_AXIS]
 #endif //TMC2130
-  );
+	);
 #ifdef PIDTEMP
-  printf_P(PSTR("%SPID settings:\n%S   M301 P%.2f I%.2f D%.2f\n"),
-    echomagic, echomagic, cs.Kp, unscalePID_i(cs.Ki), unscalePID_d(cs.Kd));
+	printf_P(PSTR("%SPID settings:\n%S   M301 P%.2f I%.2f D%.2f\n"),
+		echomagic, echomagic, cs.Kp, unscalePID_i(cs.Ki), unscalePID_d(cs.Kd));
 #endif
 #ifdef PIDTEMPBED
-  printf_P(PSTR("%SPID heatbed settings:\n%S   M304 P%.2f I%.2f D%.2f\n"),
-    echomagic, echomagic, cs.bedKp, unscalePID_i(cs.bedKi), unscalePID_d(cs.bedKd));
+	printf_P(PSTR("%SPID heatbed settings:\n%S   M304 P%.2f I%.2f D%.2f\n"),
+		echomagic, echomagic, cs.bedKp, unscalePID_i(cs.bedKi), unscalePID_d(cs.bedKd));
 #endif
 #ifdef FWRETRACT
-  printf_P(PSTR(
-    "%SRetract: S=Length (mm) F:Speed (mm/m) Z: ZLift (mm)\n%S   M207 S%.2f F%.2f Z%.2f\n"
-    "%SRecover: S=Extra length (mm) F:Speed (mm/m)\n%S   M208 S%.2f F%.2f\n"
-    "%SAuto-Retract: S=0 to disable, 1 to interpret extrude-only moves as retracts or recoveries\n%S   M209 S%d\n"
-    ),
-    echomagic, echomagic, cs.retract_length, cs.retract_feedrate*60, cs.retract_zlift,
-    echomagic, echomagic, cs.retract_recover_length, cs.retract_recover_feedrate*60,
-    echomagic, echomagic, (cs.autoretract_enabled ? 1 : 0)
-  );
+	printf_P(PSTR(
+		"%SRetract: S=Length (mm) F:Speed (mm/m) Z: ZLift (mm)\n%S   M207 S%.2f F%.2f Z%.2f\n"
+		"%SRecover: S=Extra length (mm) F:Speed (mm/m)\n%S   M208 S%.2f F%.2f\n"
+		"%SAuto-Retract: S=0 to disable, 1 to interpret extrude-only moves as retracts or recoveries\n%S   M209 S%d\n"
+		),
+		echomagic, echomagic, cs.retract_length, cs.retract_feedrate*60, cs.retract_zlift,
+		echomagic, echomagic, cs.retract_recover_length, cs.retract_recover_feedrate*60,
+		echomagic, echomagic, (cs.autoretract_enabled ? 1 : 0)
+	);
 #if EXTRUDERS > 1
-  printf_P(PSTR("%SMulti-extruder settings:\n%S   Swap retract length (mm):    %.2f\n%S   Swap rec. addl. length (mm): %.2f\n"),
-    echomagic, echomagic, retract_length_swap, echomagic, retract_recover_length_swap);
+	printf_P(PSTR("%SMulti-extruder settings:\n%S   Swap retract length (mm):    %.2f\n%S   Swap rec. addl. length (mm): %.2f\n"),
+		echomagic, echomagic, retract_length_swap, echomagic, retract_recover_length_swap);
 #endif
-  if (cs.volumetric_enabled) {
-    printf_P(PSTR("%SFilament settings:\n%S   M200 D%.2f\n"),
-      echomagic, echomagic, cs.filament_size[0]);
+	if (cs.volumetric_enabled) {
+		printf_P(PSTR("%SFilament settings:\n%S   M200 D%.2f\n"),
+			echomagic, echomagic, cs.filament_size[0]);
 #if EXTRUDERS > 1
-    printf_P(PSTR("%S   M200 T1 D%.2f\n"),
-      echomagic, echomagic, cs.filament_size[1]);
+		printf_P(PSTR("%S   M200 T1 D%.2f\n"),
+			echomagic, echomagic, cs.filament_size[1]);
 #if EXTRUDERS > 2
-    printf_P(PSTR("%S   M200 T1 D%.2f\n"),
-      echomagic, echomagic, cs.filament_size[2]);
+		printf_P(PSTR("%S   M200 T1 D%.2f\n"),
+			echomagic, echomagic, cs.filament_size[2]);
 #endif
 #endif
    } else {
        printf_P(PSTR("%SFilament settings: Disabled\n"), echomagic);
    }
 #endif
-  if (level >= 10) {
+	if (level >= 10) {
 #ifdef LIN_ADVANCE
-    printf_P(PSTR("%SLinear advance settings:%S   M900 K%.2f\n"),
+		printf_P(PSTR("%SLinear advance settings:%S   M900 K%.2f\n"),
                 echomagic, echomagic, extruder_advance_K);
 #endif //LIN_ADVANCE
-  }
-    // Arc Interpolation Settings
-    printf_P(PSTR(
-        "%SArc Settings: P:Max length(mm) S:Min length (mm) N:Corrections R:Min segments F:Segments/sec.\n%S  M214 P%.2f S%.2f N%d R%d F%d\n"),
-        echomagic, echomagic, cs.mm_per_arc_segment, cs.min_mm_per_arc_segment, cs.n_arc_correction, cs.min_arc_segments, cs.arc_segments_per_sec);
-#ifdef THERMAL_MODEL
-    thermal_model_report_settings();
-#endif
-		printf_P(PSTR(
-        "%SStatistics:\n%S  M78 S%lu T%lu\n"),
-        echomagic, echomagic, eeprom_read_dword((uint32_t *)EEPROM_FILAMENTUSED), eeprom_read_dword((uint32_t *)EEPROM_TOTALTIME));
+	}
 }
 #endif

@ -117,10 +177,14 @@ void Config_PrintSettings(uint8_t level)

 static_assert (EXTRUDERS == 1, "ConfigurationStore M500_conf not implemented for more extruders, fix filament_size array size.");
 static_assert (NUM_AXIS == 4, "ConfigurationStore M500_conf not implemented for more axis."
-        "Fix axis_steps_per_mm max_feedrate_normal max_acceleration_mm_per_s2_normal max_jerk max_feedrate_silent"
-        " max_acceleration_mm_per_s2_silent array size.");
+        "Fix axis_steps_per_unit max_feedrate_normal max_acceleration_units_per_sq_second_normal max_jerk max_feedrate_silent"
+        " max_acceleration_units_per_sq_second_silent array size.");
+#ifdef ENABLE_AUTO_BED_LEVELING
+static_assert (false, "zprobe_zoffset was not initialized in printers in field to -(Z_PROBE_OFFSET_FROM_EXTRUDER), so it contains"
+        "0.0, if this is not acceptable, increment EEPROM_VERSION to force use default_conf");
+#endif

-static_assert (sizeof(M500_conf) == 209, "sizeof(M500_conf) has changed, ensure that EEPROM_VERSION has been incremented, "
+static_assert (sizeof(M500_conf) == 192, "sizeof(M500_conf) has changed, ensure that EEPROM_VERSION has been incremented, "
        "or if you added members in the end of struct, ensure that historically uninitialized values will be initialized."
        "If this is caused by change to more then 8bit processor, decide whether make this struct packed to save EEPROM,"
        "leave as it is to keep fast code, or reorder struct members to pack more tightly.");
@ -168,99 +232,90 @@ static const M500_conf default_conf PROGMEM =
 #else // TMC2130
    {16,16,16,16},
 #endif
-    DEFAULT_TRAVEL_ACCELERATION,
-    DEFAULT_MM_PER_ARC_SEGMENT,
-    DEFAULT_MIN_MM_PER_ARC_SEGMENT,
-    DEFAULT_N_ARC_CORRECTION,
-    DEFAULT_MIN_ARC_SEGMENTS,
-    DEFAULT_ARC_SEGMENTS_PER_SEC
 };

-
-void Config_StoreSettings()
-{
-  strcpy_P(cs.version, default_conf.version);
-  eeprom_update_block_notify(reinterpret_cast<uint8_t*>(&cs), reinterpret_cast<uint8_t*>(EEPROM_M500_base), sizeof(cs));
-#ifdef THERMAL_MODEL
-  thermal_model_save_settings();
-#endif
-
-  SERIAL_ECHO_START;
-  SERIAL_ECHOLNPGM("Settings Stored");
-}
-
-
 //! @brief Read M500 configuration
-//! @retval true Succeeded. Stored settings retrieved or default settings retrieved in case EEPROM cs was empty.
-//! @retval false Failed. Default settings has been retrieved, because of version mismatch
+//! @retval true Succeeded. Stored settings retrieved or default settings retrieved in case EEPROM has been erased.
+//! @retval false Failed. Default settings has been retrieved, because of older version or corrupted data.
 bool Config_RetrieveSettings()
 {
-    eeprom_read_block(reinterpret_cast<uint8_t*>(cs.version), reinterpret_cast<uint8_t*>(EEPROM_M500_base->version), sizeof(cs.version));
+	bool previous_settings_retrieved = true;
+    char ver[4]=EEPROM_VERSION;
+    EEPROM_readData(reinterpret_cast<uint8_t*>(EEPROM_M500_base->version), reinterpret_cast<uint8_t*>(cs.version), sizeof(cs.version), "cs.version"); //read stored version
    //  SERIAL_ECHOLN("Version: [" << ver << "] Stored version: [" << cs.version << "]");
-    if (strncmp_P(cs.version, default_conf.version, sizeof(EEPROM_VERSION)) == 0)  // version number match
+    if (strncmp(ver,cs.version,3) == 0)  // version number match
    {
-        // Initialize arc interpolation settings in eeprom if they are not already
-        eeprom_init_default_float(&EEPROM_M500_base->mm_per_arc_segment, pgm_read_float(&default_conf.mm_per_arc_segment));
-        eeprom_init_default_float(&EEPROM_M500_base->min_mm_per_arc_segment, pgm_read_float(&default_conf.min_mm_per_arc_segment));
-        eeprom_init_default_byte(&EEPROM_M500_base->n_arc_correction, pgm_read_byte(&default_conf.n_arc_correction));
-        eeprom_init_default_word(&EEPROM_M500_base->min_arc_segments, pgm_read_word(&default_conf.min_arc_segments));
-        eeprom_init_default_word(&EEPROM_M500_base->arc_segments_per_sec, pgm_read_word(&default_conf.arc_segments_per_sec));

-        // Initialize the travel_acceleration in eeprom if not already
-        eeprom_init_default_float(&EEPROM_M500_base->travel_acceleration, pgm_read_float(&default_conf.travel_acceleration));
+        EEPROM_readData(reinterpret_cast<uint8_t*>(EEPROM_M500_base), reinterpret_cast<uint8_t*>(&cs), sizeof(cs), "cs");

-        // Initialize the max_feedrate_silent and max_acceleration_mm_per_s2_silent in eeprom if not already
-        eeprom_init_default_block(&EEPROM_M500_base->max_feedrate_silent, sizeof(EEPROM_M500_base->max_feedrate_silent), default_conf.max_feedrate_silent);
-        eeprom_init_default_block(&EEPROM_M500_base->max_acceleration_mm_per_s2_silent, sizeof(EEPROM_M500_base->max_acceleration_mm_per_s2_silent), default_conf.max_acceleration_mm_per_s2_silent);
-
-#ifdef TMC2130
-        eeprom_init_default_block(&EEPROM_M500_base->axis_ustep_resolution, sizeof(EEPROM_M500_base->axis_ustep_resolution), default_conf.axis_ustep_resolution);
-#endif // TMC2130
-
-        // load the CS to RAM
-        eeprom_read_block(reinterpret_cast<uint8_t*>(&cs), reinterpret_cast<uint8_t*>(EEPROM_M500_base), sizeof(cs));
+        
+		if (cs.max_jerk[X_AXIS] > DEFAULT_XJERK) cs.max_jerk[X_AXIS] = DEFAULT_XJERK;
+		if (cs.max_jerk[Y_AXIS] > DEFAULT_YJERK) cs.max_jerk[Y_AXIS] = DEFAULT_YJERK;
        calculate_extruder_multipliers();

-#ifdef TMC2130
-    for (uint8_t j = X_AXIS; j <= Y_AXIS; j++)
-    {
-      if (cs.max_feedrate_normal[j] > NORMAL_MAX_FEEDRATE_XY)
-        cs.max_feedrate_normal[j] = NORMAL_MAX_FEEDRATE_XY;
-      if (cs.max_feedrate_silent[j] > SILENT_MAX_FEEDRATE_XY)
-        cs.max_feedrate_silent[j] = SILENT_MAX_FEEDRATE_XY;
-      if (cs.max_acceleration_mm_per_s2_normal[j] > NORMAL_MAX_ACCEL_XY)
-        cs.max_acceleration_mm_per_s2_normal[j] = NORMAL_MAX_ACCEL_XY;
-      if (cs.max_acceleration_mm_per_s2_silent[j] > SILENT_MAX_ACCEL_XY)
-        cs.max_acceleration_mm_per_s2_silent[j] = SILENT_MAX_ACCEL_XY;
-    }
+		//if max_feedrate_silent and max_acceleration_units_per_sq_second_silent were never stored to eeprom, use default values:
+        for (uint8_t i = 0; i < (sizeof(cs.max_feedrate_silent)/sizeof(cs.max_feedrate_silent[0])); ++i)
+        {
+            const uint32_t erased = 0xffffffff;
+            bool initialized = false;

-    tmc2130_set_res(X_AXIS, cs.axis_ustep_resolution[X_AXIS]);
-    tmc2130_set_res(Y_AXIS, cs.axis_ustep_resolution[Y_AXIS]);
-    tmc2130_set_res(Z_AXIS, cs.axis_ustep_resolution[Z_AXIS]);
-    tmc2130_set_res(E_AXIS, cs.axis_ustep_resolution[E_AXIS]);
+            for(uint8_t j = 0; j < sizeof(float); ++j)
+            {
+                if(0xff != reinterpret_cast<uint8_t*>(&(cs.max_feedrate_silent[i]))[j]) initialized = true;
+            }
+            if (!initialized) memcpy_P(&cs.max_feedrate_silent[i],&default_conf.max_feedrate_silent[i], sizeof(cs.max_feedrate_silent[i]));
+            if (erased == cs.max_acceleration_units_per_sq_second_silent[i]) {
+                memcpy_P(&cs.max_acceleration_units_per_sq_second_silent[i],&default_conf.max_acceleration_units_per_sq_second_silent[i],sizeof(cs.max_acceleration_units_per_sq_second_silent[i]));
+            }
+        }
+
+#ifdef TMC2130
+		for (uint8_t j = X_AXIS; j <= Y_AXIS; j++)
+		{
+			if (cs.max_feedrate_normal[j] > NORMAL_MAX_FEEDRATE_XY)
+				cs.max_feedrate_normal[j] = NORMAL_MAX_FEEDRATE_XY;
+			if (cs.max_feedrate_silent[j] > SILENT_MAX_FEEDRATE_XY)
+				cs.max_feedrate_silent[j] = SILENT_MAX_FEEDRATE_XY;
+			if (cs.max_acceleration_units_per_sq_second_normal[j] > NORMAL_MAX_ACCEL_XY)
+				cs.max_acceleration_units_per_sq_second_normal[j] = NORMAL_MAX_ACCEL_XY;
+			if (cs.max_acceleration_units_per_sq_second_silent[j] > SILENT_MAX_ACCEL_XY)
+				cs.max_acceleration_units_per_sq_second_silent[j] = SILENT_MAX_ACCEL_XY;
+		}
+        
+		if(cs.axis_ustep_resolution[X_AXIS] == 0xff){ cs.axis_ustep_resolution[X_AXIS] = TMC2130_USTEPS_XY; }
+		if(cs.axis_ustep_resolution[Y_AXIS] == 0xff){ cs.axis_ustep_resolution[Y_AXIS] = TMC2130_USTEPS_XY; }
+		if(cs.axis_ustep_resolution[Z_AXIS] == 0xff){ cs.axis_ustep_resolution[Z_AXIS] = TMC2130_USTEPS_Z; }
+		if(cs.axis_ustep_resolution[E_AXIS] == 0xff){ cs.axis_ustep_resolution[E_AXIS] = TMC2130_USTEPS_E; }
+
+		tmc2130_set_res(X_AXIS, cs.axis_ustep_resolution[X_AXIS]);
+		tmc2130_set_res(Y_AXIS, cs.axis_ustep_resolution[Y_AXIS]);
+		tmc2130_set_res(Z_AXIS, cs.axis_ustep_resolution[Z_AXIS]);
+		tmc2130_set_res(E_AXIS, cs.axis_ustep_resolution[E_AXIS]);
 #endif //TMC2130

 		reset_acceleration_rates();

-    // Call updatePID (similar to when we have processed M301)
-    updatePID();
-#ifdef THERMAL_MODEL
-    thermal_model_load_settings();
-#endif
-
+		// Call updatePID (similar to when we have processed M301)
+		updatePID();
        SERIAL_ECHO_START;
        SERIAL_ECHOLNPGM("Stored settings retrieved");
    }
    else
    {
        Config_ResetDefault();
-        //Return false to inform user that eeprom version was changed and firmware is using default hardcoded settings now.
-        //In case that storing to eeprom was not used yet, do not inform user that hardcoded settings are used.
-        if (eeprom_is_initialized_block(EEPROM_M500_base->version, sizeof(EEPROM_M500_base->version))) {
-            return false;
-        }
+		//Return false to inform user that eeprom version was changed and firmware is using default hardcoded settings now.
+		//In case that storing to eeprom was not used yet, do not inform user that hardcoded settings are used.
+		if (eeprom_read_byte(reinterpret_cast<uint8_t*>(&(EEPROM_M500_base->version[0]))) != 0xFF ||
+			eeprom_read_byte(reinterpret_cast<uint8_t*>(&(EEPROM_M500_base->version[1]))) != 0xFF ||
+			eeprom_read_byte(reinterpret_cast<uint8_t*>(&(EEPROM_M500_base->version[2]))) != 0xFF)
+		{
+			previous_settings_retrieved = false;
+		}
    }
-    return true;
+    #ifdef EEPROM_CHITCHAT
+      Config_PrintSettings();
+    #endif
+	return previous_settings_retrieved;
 }
 #endif

@ -268,20 +323,19 @@ void Config_ResetDefault()
 {
    memcpy_P(&cs,&default_conf, sizeof(cs));

-  // steps per sq second need to be updated to agree with the units per sq second
+	// steps per sq second need to be updated to agree with the units per sq second
    reset_acceleration_rates();
-
+    
 #ifdef PIDTEMP
    updatePID();
+#ifdef PID_ADD_EXTRUSION_RATE
+    Kc = DEFAULT_Kc; //this is not stored by Config_StoreSettings
+#endif//PID_ADD_EXTRUSION_RATE
 #endif//PIDTEMP
-#ifdef THERMAL_MODEL
-    thermal_model_reset_settings();
-#endif

-  calculate_extruder_multipliers();
+	calculate_extruder_multipliers();

 SERIAL_ECHO_START;
 SERIAL_ECHOLNPGM("Hardcoded Default Settings Loaded");

 }
-
--- a/Firmware/ConfigurationStore.h
+++ b/Firmware/ConfigurationStore.h
@ -9,17 +9,17 @@
 typedef struct
 {
    char version[4];
-    float axis_steps_per_mm[4];
+    float axis_steps_per_unit[4];
    float max_feedrate_normal[4];
-    uint32_t max_acceleration_mm_per_s2_normal[4];
+    unsigned long max_acceleration_units_per_sq_second_normal[4];
    float acceleration; //!< Normal acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX
    float retract_acceleration; //!< mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX
    float minimumfeedrate;
    float mintravelfeedrate;
-    uint32_t min_segment_time_us; //!< (µs) M205 B
+    unsigned long minsegmenttime;
    float max_jerk[4]; //!< Jerk is a maximum immediate velocity change.
    float add_homing[3];
-    float zprobe_zoffset; //!< unused
+    float zprobe_zoffset;
    float Kp;
    float Ki;
    float Kd;
@ -36,15 +36,8 @@ typedef struct
    bool volumetric_enabled;
    float filament_size[1]; //!< cross-sectional area of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder.
    float max_feedrate_silent[4]; //!< max speeds for silent mode
-    uint32_t max_acceleration_mm_per_s2_silent[4];
+    unsigned long max_acceleration_units_per_sq_second_silent[4];
    unsigned char axis_ustep_resolution[4];
-    float travel_acceleration; //!< travel acceleration mm/s^2
-    // Arc Interpolation Settings, configurable via M214
-    float mm_per_arc_segment;
-    float min_mm_per_arc_segment;
-    uint8_t n_arc_correction; // If equal to zero, this is disabled
-    uint16_t min_arc_segments; // If equal to zero, this is disabled
-    uint16_t arc_segments_per_sec; // If equal to zero, this is disabled
 } M500_conf;

 extern M500_conf cs;
@ -65,4 +58,8 @@ FORCE_INLINE void Config_StoreSettings() {}
 FORCE_INLINE void Config_RetrieveSettings() { Config_ResetDefault(); Config_PrintSettings(); }
 #endif

+inline uint8_t calibration_status() { return eeprom_read_byte((uint8_t*)EEPROM_CALIBRATION_STATUS); }
+inline void calibration_status_store(uint8_t status) { eeprom_update_byte((uint8_t*)EEPROM_CALIBRATION_STATUS, status); }
+inline bool calibration_status_pinda() { return eeprom_read_byte((uint8_t*)EEPROM_CALIBRATION_STATUS_PINDA); }
+
 #endif//CONFIG_STORE_H
--- a/Firmware/Configuration_adv.h
+++ b/Firmware/Configuration_adv.h
@ -10,6 +10,16 @@
 #endif
 #define BED_CHECK_INTERVAL 5000 //ms between checks in bang-bang control

+#ifdef PIDTEMP
+  // this adds an experimental additional term to the heating power, proportional to the extrusion speed.
+  // if Kc is chosen well, the additional required power due to increased melting should be compensated.
+  #define PID_ADD_EXTRUSION_RATE
+  #ifdef PID_ADD_EXTRUSION_RATE
+    #define  DEFAULT_Kc (1) //heating power=Kc*(e_speed)
+  #endif
+#endif
+
+
 //automatic temperature: The hot end target temperature is calculated by all the buffered lines of gcode.
 //The maximum buffered steps/sec of the extruder motor are called "se".
 //You enter the autotemp mode by a M109 S<mintemp> B<maxtemp> F<factor>
@ -52,25 +62,8 @@
 // before setting a PWM value. (Does not work with software PWM for fan on Sanguinololu)
 #define FAN_KICKSTART_TIME 800

-/**
- * Auto-report all at once with M155 S<seconds> C[bitmask] with single timer
- *
- * bit 0 = Auto-report temperatures
- * bit 1 = Auto-report fans
- * bit 2 = Auto-report position
- * bit 3 = free
- * bit 4 = free
- * bit 5 = free
- * bit 6 = free
- * bit 7 = free
-*/
-#define AUTO_REPORT

-// Keepalive period which is restarted with M79
-#define M79_TIMEOUT 30 * 1000 // ms

-// A timer which is restarted everytime a G-command is added to the command queue.
-#define USB_TIMER_TIMEOUT 10 * 1000 // ms

 //===========================================================================
 //=============================Mechanical Settings===========================
@ -125,6 +118,34 @@
 #endif //End auto min/max positions
 //END AUTOSET LOCATIONS OF LIMIT SWITCHES -ZP

+
+// A single Z stepper driver is usually used to drive 2 stepper motors.
+// Uncomment this define to utilize a separate stepper driver for each Z axis motor.
+// Only a few motherboards support this, like RAMPS, which have dual extruder support (the 2nd, often unused, extruder driver is used
+// to control the 2nd Z axis stepper motor). The pins are currently only defined for a RAMPS motherboards.
+// On a RAMPS (or other 5 driver) motherboard, using this feature will limit you to using 1 extruder.
+//#define Z_DUAL_STEPPER_DRIVERS
+
+#ifdef Z_DUAL_STEPPER_DRIVERS
+  #undef EXTRUDERS
+  #define EXTRUDERS 1
+#endif
+
+// Same again but for Y Axis.
+//#define Y_DUAL_STEPPER_DRIVERS
+
+// Define if the two Y drives need to rotate in opposite directions
+#define INVERT_Y2_VS_Y_DIR 1
+
+#ifdef Y_DUAL_STEPPER_DRIVERS
+  #undef EXTRUDERS
+  #define EXTRUDERS 1
+#endif
+
+#if defined (Z_DUAL_STEPPER_DRIVERS) && defined (Y_DUAL_STEPPER_DRIVERS)
+  #error "You cannot have dual drivers for both Y and Z"
+#endif
+
 //homing hits the endstop, then retracts by this distance, before it tries to slowly bump again:
 #define X_HOME_RETRACT_MM 5
 #define Y_HOME_RETRACT_MM 5
@ -199,29 +220,35 @@
 * SD sorting uses static allocation (as set by SDSORT_LIMIT), allowing the
 * compiler to calculate the worst-case usage and throw an error if the SRAM
 * limit is exceeded.
+*
+*  - SDSORT_USES_RAM provides faster sorting via a static directory buffer.
+*  - SDSORT_USES_STACK does the same, but uses a local stack-based buffer.
+*  - SDSORT_CACHE_NAMES will retain the sorted file listing in RAM. (Expensive!)
+*  - SDSORT_DYNAMIC_RAM only uses RAM when the SD menu is visible. (Use with caution!)
 */
 	#define SDCARD_SORT_ALPHA //Alphabetical sorting of SD files menu
-
+	
 	// SD Card Sorting options
+	// In current firmware Prusa Firmware version,
+	// SDSORT_CACHE_NAMES and SDSORT_DYNAMIC_RAM is not supported and must be set to 0.
 	#ifdef SDCARD_SORT_ALPHA
 	  #define SD_SORT_TIME 0
 	  #define SD_SORT_ALPHA 1
 	  #define SD_SORT_NONE 2
-	  #define INSERTSORT
-	  // #define SORTING_DUMP
-	  // #define SORTING_SPEEDTEST
-
+	
 	  #define SDSORT_LIMIT       100    // Maximum number of sorted items (10-256).
 	  #define FOLDER_SORTING     -1     // -1=above  0=none  1=below
+	  #define SDSORT_GCODE       0  // Allow turning sorting on/off with LCD and M34 g-code.
+	  #define SDSORT_USES_RAM    0  // Pre-allocate a static array for faster pre-sorting.
+	  #define SDSORT_USES_STACK  0  // Prefer the stack for pre-sorting to give back some SRAM. (Negated by next 2 options.)
+	  #define SDSORT_CACHE_NAMES 0  // Keep sorted items in RAM longer for speedy performance. Most expensive option.
+	  #define SDSORT_DYNAMIC_RAM 0  // Use dynamic allocation (within SD menus). Least expensive option. Set SDSORT_LIMIT before use!
 	#endif
-
+	
 	#if defined(SDCARD_SORT_ALPHA)
-	  #define HAS_FOLDER_SORTING (FOLDER_SORTING)
+	  #define HAS_FOLDER_SORTING (FOLDER_SORTING || SDSORT_GCODE)
 	#endif

-// Enabe this option to get a pretty message whenever the endstop gets hit (as in the position at which the endstop got triggered)
-//#define VERBOSE_CHECK_HIT_ENDSTOPS
-
 // Enable the option to stop SD printing when hitting and endstops, needs to be enabled from the LCD menu when this option is enabled.
 //#define ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED

@ -268,7 +295,9 @@
  //#define LA_DEBUG_LOGIC     // @wavexx: setup logic channels for isr debugging
 #endif

-// Arc interpretation settings : Moved to the variant files.
+// Arc interpretation settings:
+#define MM_PER_ARC_SEGMENT 1
+#define N_ARC_CORRECTION 25

 const unsigned int dropsegments=5; //everything with less than this number of steps will be ignored as move and joined with the next movement

@ -298,11 +327,6 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 // Control heater 0 and heater 1 in parallel.
 //#define HEATERS_PARALLEL

-//LCD status clock interval timer to switch between
-// remaining print time
-// and time to change/pause/interaction
-#define CLOCK_INTERVAL_TIME 5
-
 //===========================================================================
 //=============================Buffers           ============================
 //===========================================================================
@ -324,11 +348,6 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 // 2nd and 3rd byte (LSB first) contains a 16bit length of a command including its preceding comments.
 #define CMDHDRSIZE 3

-#define FILAMENT_CHANGE_UNLOAD_FEEDRATE         10.f  // (mm/s) Unload filament feedrate. This can be pretty fast.
-#define FILAMENT_UNLOAD_FAST_RETRACT_FEEDRATE   86.67f  // (mm/s) Unload fast retract feedrate.
-#define FILAMENT_UNLOAD_SLOW_RETRACT_FEEDRATE   16.67f  // (mm/s) Unload slow retract feedrate.
-#define FILAMENT_UNLOAD_FAST_RETRACT_LENGTH     45.f  // (mm) Unload fast retract length.
-#define FILAMENT_UNLOAD_SLOW_RETRACT_LENGTH     35.f  // (mm) Unload slow retract length.

 // Firmware based and LCD controlled retract
 // M207 and M208 can be used to define parameters for the retraction.
@ -357,17 +376,6 @@ const unsigned int dropsegments=5; //everything with less than this number of st
  #endif
 #endif

-/**
- * Include capabilities in M115 output
- */
-#define EXTENDED_CAPABILITIES_REPORT
-
-/**
- * Enable M120/M121 G-code commands
- *
- */
-//#define M120_M121_ENABLED  //Be careful enabling and using these G-code commands.
-
 //===========================================================================
 //=============================  Define Defines  ============================
 //===========================================================================
@ -380,6 +388,14 @@ const unsigned int dropsegments=5; //everything with less than this number of st
  #define THERMISTORHEATER_0 TEMP_SENSOR_0
  #define HEATER_0_USES_THERMISTOR
 #endif
+#if TEMP_SENSOR_1 > 0
+  #define THERMISTORHEATER_1 TEMP_SENSOR_1
+  #define HEATER_1_USES_THERMISTOR
+#endif
+#if TEMP_SENSOR_2 > 0
+  #define THERMISTORHEATER_2 TEMP_SENSOR_2
+  #define HEATER_2_USES_THERMISTOR
+#endif
 #if TEMP_SENSOR_BED > 0
  #define THERMISTORBED TEMP_SENSOR_BED
  #define BED_USES_THERMISTOR
@ -393,6 +409,12 @@ const unsigned int dropsegments=5; //everything with less than this number of st
 #if TEMP_SENSOR_0 == -1
  #define HEATER_0_USES_AD595
 #endif
+#if TEMP_SENSOR_1 == -1
+  #define HEATER_1_USES_AD595
+#endif
+#if TEMP_SENSOR_2 == -1
+  #define HEATER_2_USES_AD595
+#endif
 #if TEMP_SENSOR_BED == -1
  #define BED_USES_AD595
 #endif
@ -403,6 +425,14 @@ const unsigned int dropsegments=5; //everything with less than this number of st
  #undef HEATER_0_MINTEMP
  #undef HEATER_0_MAXTEMP
 #endif
+#if TEMP_SENSOR_1 == 0
+  #undef HEATER_1_MINTEMP
+  #undef HEATER_1_MAXTEMP
+#endif
+#if TEMP_SENSOR_2 == 0
+  #undef HEATER_2_MINTEMP
+  #undef HEATER_2_MAXTEMP
+#endif
 #if TEMP_SENSOR_BED == 0
  #undef BED_MINTEMP
  #undef BED_MAXTEMP
--- a/Firmware/Configuration_var.h
+++ b/Firmware/Configuration_var.h
@ -1,10 +0,0 @@
-// Include the printer's variant configuration header
-#pragma once
-
-// This is set by the cmake build to be able to take control of
-// the variant header without breaking existing build mechanisms.
-#ifndef CMAKE_CONTROL
-#include "Configuration_prusa.h"
-#else
-#include FW_VARIANT
-#endif
--- a/Firmware/Dcodes.cpp
+++ b/Firmware/Dcodes.cpp
@ -1,9 +1,8 @@
-#include "Marlin.h"
 #include "Dcodes.h"
+#include "Marlin.h"
 #include "Configuration.h"
 #include "language.h"
 #include "cmdqueue.h"
-#include "util.h"
 #include <stdio.h>
 #include <avr/pgmspace.h>

@ -24,28 +23,31 @@ void print_hex_byte(uint8_t val)
 	print_hex_nibble(val & 15);
 }

-// debug range address type (fits all SRAM/PROGMEM/XFLASH memory ranges)
-#if defined(DEBUG_DCODE6) || defined(DEBUG_DCODES) || defined(XFLASH_DUMP)
-#include "xflash.h"
-#include "xflash_layout.h"
-
-#define DADDR_SIZE 32
-typedef uint32_t daddr_t; // XFLASH requires 24 bits
-#else
-#define DADDR_SIZE 16
-typedef uint16_t daddr_t;
-#endif
-
-void print_hex_word(daddr_t val)
+void print_hex_word(uint16_t val)
 {
-#if DADDR_SIZE > 16
-    print_hex_byte((val >> 16) & 0xFF);
-#endif
-    print_hex_byte((val >> 8) & 0xFF);
-    print_hex_byte(val & 0xFF);
+	print_hex_byte(val >> 8);
+	print_hex_byte(val & 255);
 }

-int parse_hex(const char* hex, uint8_t* data, int count)
+void print_eeprom(uint16_t address, uint16_t count, uint8_t countperline = 16)
+{
+	while (count)
+	{
+		print_hex_word(address);
+		putchar(' ');
+		uint8_t count_line = countperline;
+		while (count && count_line)
+		{
+			putchar(' ');
+			print_hex_byte(eeprom_read_byte((uint8_t*)address++));
+			count_line--;
+			count--;
+		}
+		putchar('\n');
+	}
+}
+
+int parse_hex(char* hex, uint8_t* data, int count)
 {
 	int parsed = 0;
 	while (*hex)
@ -69,16 +71,12 @@ int parse_hex(const char* hex, uint8_t* data, int count)
 }


-enum class dcode_mem_t:uint8_t { sram, eeprom, progmem, xflash };
-
-void print_mem(daddr_t address, daddr_t count, dcode_mem_t type, uint8_t countperline = 16)
+void print_mem(uint32_t address, uint16_t count, uint8_t type, uint8_t countperline = 16)
 {
-#if defined(DEBUG_DCODE6) || defined(DEBUG_DCODES) || defined(XFLASH_DUMP)
-    if(type == dcode_mem_t::xflash)
-        XFLASH_SPI_ENTER();
-#endif
 	while (count)
 	{
+		if (type == 2)
+			print_hex_nibble(address >> 16);
 		print_hex_word(address);
 		putchar(' ');
 		uint8_t count_line = countperline;
@ -87,98 +85,81 @@ void print_mem(daddr_t address, daddr_t count, dcode_mem_t type, uint8_t countpe
 			uint8_t data = 0;
 			switch (type)
 			{
-			case dcode_mem_t::sram: data = *((uint8_t*)address); break;
-			case dcode_mem_t::eeprom: data = eeprom_read_byte((uint8_t*)address); break;
-			case dcode_mem_t::progmem: break;
-#if defined(DEBUG_DCODE6) || defined(DEBUG_DCODES) || defined(XFLASH_DUMP)
-            case dcode_mem_t::xflash: xflash_rd_data(address, &data, 1); break;
-#else
-            case dcode_mem_t::xflash: break;
-#endif
+			case 0: data = *((uint8_t*)address++); break;
+			case 1: data = eeprom_read_byte((uint8_t*)address++); break;
+			case 2: data = pgm_read_byte_far((uint8_t*)address++); break;
 			}
-            ++address;
 			putchar(' ');
 			print_hex_byte(data);
 			count_line--;
 			count--;
-
-            // sporadically call manage_heater, but only when interrupts are enabled (meaning
-            // print_mem is called by D2). Don't do anything otherwise: we are inside a crash
-            // handler where memory & stack needs to be preserved!
-            if((SREG & (1 << SREG_I)) && !((uint16_t)count % 8192))
-                manage_heater();
 		}
 		putchar('\n');
 	}
 }

-// TODO: this only handles SRAM/EEPROM 16bit addresses
-void write_mem(uint16_t address, uint16_t count, const uint8_t* data, const dcode_mem_t type)
-{
-    for (uint16_t i = 0; i < count; i++)
-    {
-        switch (type)
-        {
-        case dcode_mem_t::sram: *((uint8_t*)address) = data[i]; break;
-        case dcode_mem_t::eeprom: eeprom_write_byte_notify((uint8_t*)address, data[i]); break;
-        case dcode_mem_t::progmem: break;
-        case dcode_mem_t::xflash: break;
-        }
-        ++address;
-    }
-}
-
-void dcode_core(daddr_t addr_start, const daddr_t addr_end, const dcode_mem_t type,
-                uint8_t dcode, const char* type_desc)
-{
-    KEEPALIVE_STATE(NOT_BUSY);
-    DBG(_N("D%d - Read/Write %S\n"), dcode, type_desc);
-    daddr_t count = -1; // RW the entire space by default
-    if (code_seen('A'))
-        addr_start = (strchr_pointer[1] == 'x')?strtol(strchr_pointer + 2, 0, 16):(int)code_value();
-    if (code_seen('C'))
-        count = code_value_long();
-    if (addr_start > addr_end)
-        addr_start = addr_end;
-    if ((addr_start + count) > addr_end || (addr_start + count) < addr_start)
-        count = addr_end - addr_start;
-    if (code_seen('X'))
-    {
-        uint8_t data[16];
-        count = parse_hex(strchr_pointer + 1, data, 16);
-        write_mem(addr_start, count, data, type);
-#if DADDR_SIZE > 16
-        DBG(_N("%lu bytes written to %S at address 0x%04lx\n"), count, type_desc, addr_start);
-#else
-        DBG(_N("%u bytes written to %S at address 0x%08x\n"), count, type_desc, addr_start);
-#endif
-    }
-    print_mem(addr_start, count, type);
-}
-
 #if defined DEBUG_DCODE3 || defined DEBUG_DCODES
 #define EEPROM_SIZE 0x1000
    /*!
    ### D3 - Read/Write EEPROM <a href="https://reprap.org/wiki/G-code#D3:_Read.2FWrite_EEPROM">D3: Read/Write EEPROM</a>
    This command can be used without any additional parameters. It will read the entire eeprom.
    #### Usage
-
+    
        D3 [ A | C | X ]
-
+    
    #### Parameters
    - `A` - Address (x0000-x0fff)
    - `C` - Count (1-4096)
    - `X` - Data (hex)
-
+	
 	#### Notes
 	- The hex address needs to be lowercase without the 0 before the x
-	- Count is decimal
+	- Count is decimal 
 	- The hex data needs to be lowercase
-
+	
    */
 void dcode_3()
 {
-    dcode_core(0, EEPROM_SIZE, dcode_mem_t::eeprom, 3, _N("EEPROM"));
+	DBG(_N("D3 - Read/Write EEPROM\n"));
+	uint16_t address = 0x0000; //default 0x0000
+	uint16_t count = EEPROM_SIZE; //default 0x1000 (entire eeprom)
+	if (code_seen('A')) // Address (0x0000-0x0fff)
+		address = (strchr_pointer[1] == 'x')?strtol(strchr_pointer + 2, 0, 16):(int)code_value();
+	if (code_seen('C')) // Count (0x0001-0x1000)
+		count = (int)code_value();
+	address &= 0x1fff;
+	if (count > EEPROM_SIZE) count = EEPROM_SIZE;
+	if ((address + count) > EEPROM_SIZE) count = EEPROM_SIZE - address;
+	if (code_seen('X')) // Data
+	{
+		uint8_t data[16];
+		count = parse_hex(strchr_pointer + 1, data, 16);
+		if (count > 0)
+		{
+			for (uint16_t i = 0; i < count; i++)
+				eeprom_write_byte((uint8_t*)(address + i), data[i]);
+			printf_P(_N("%d bytes written to EEPROM at address 0x%04x"), count, address);
+			putchar('\n');
+		}
+		else
+			count = 0;
+	}
+	print_mem(address, count, 1);
+/*	while (count)
+	{
+		print_hex_word(address);
+		putchar(' ');
+		uint8_t countperline = 16;
+		while (count && countperline)
+		{
+			uint8_t data = eeprom_read_byte((uint8_t*)address++);
+			putchar(' ');
+			print_hex_byte(data);
+			countperline--;
+			count--;
+		}
+		putchar('\n');
+	}*/
 }
 #endif //DEBUG_DCODE3

@ -191,19 +172,39 @@ void dcode_3()
 #include <avr/wdt.h>
 #include "bootapp.h"

+#if 0
+#define FLASHSIZE     0x40000
+
+#define RAMSIZE        0x2000
+#define boot_src_addr  (*((uint32_t*)(RAMSIZE - 16)))
+#define boot_dst_addr  (*((uint32_t*)(RAMSIZE - 12)))
+#define boot_copy_size (*((uint16_t*)(RAMSIZE - 8)))
+#define boot_reserved  (*((uint8_t*)(RAMSIZE - 6)))
+#define boot_app_flags (*((uint8_t*)(RAMSIZE - 5)))
+#define boot_app_magic (*((uint32_t*)(RAMSIZE - 4)))
+#define BOOT_APP_FLG_ERASE 0x01
+#define BOOT_APP_FLG_COPY  0x02
+#define BOOT_APP_FLG_FLASH 0x04
+
+extern float current_temperature_pinda;
+extern float axis_steps_per_unit[NUM_AXIS];
+
+
+#define LOG(args...) printf(args)
+#endif //0
 #define LOG(args...)

    /*!
    *
    ### D-1 - Endless Loop <a href="https://reprap.org/wiki/G-code#G28:_Move_to_Origin_.28Home.29">D-1: Endless Loop</a>
-
+      
          D-1
-
+      
    *
    */
 void dcode__1()
 {
-	DBG(_N("D-1 - Endless loop\n"));
+	printf_P(PSTR("D-1 - Endless loop\n"));
 //	cli();
 	while (1);
 }
@ -213,9 +214,9 @@ void dcode__1()
    /*!
    ### D0 - Reset <a href="https://reprap.org/wiki/G-code#D0:_Reset">D0: Reset</a>
    #### Usage
-
+    
        D0 [ B ]
-
+    
    #### Parameters
    - `B` - Bootloader
    */
@ -238,9 +239,9 @@ void dcode_0()
    /*!
    *
    ### D1 - Clear EEPROM and RESET <a href="https://reprap.org/wiki/G-code#D1:_Clear_EEPROM_and_RESET">D1: Clear EEPROM and RESET</a>
-
+      
          D1
-
+      
    *
    */
 void dcode_1()
@ -248,45 +249,79 @@ void dcode_1()
 	LOG("D1 - Clear EEPROM and RESET\n");
 	cli();
 	for (int i = 0; i < 8192; i++)
-		eeprom_write_byte_notify((unsigned char*)i, (unsigned char)0xff);
+		eeprom_write_byte((unsigned char*)i, (unsigned char)0xff);
 	softReset();
 }
-#endif

-#if defined DEBUG_DCODE2 || defined DEBUG_DCODES
    /*!
    ### D2 - Read/Write RAM <a href="https://reprap.org/wiki/G-code#D2:_Read.2FWrite_RAM">D3: Read/Write RAM</a>
    This command can be used without any additional parameters. It will read the entire RAM.
    #### Usage
-
+    
        D2 [ A | C | X ]
-
+    
    #### Parameters
-    - `A` - Address (x0000-x21ff)
-    - `C` - Count (1-8704)
+    - `A` - Address (x0000-x1fff)
+    - `C` - Count (1-8192)
    - `X` - Data

 	#### Notes
 	- The hex address needs to be lowercase without the 0 before the x
-	- Count is decimal
+	- Count is decimal 
 	- The hex data needs to be lowercase
-
+	
    */
 void dcode_2()
 {
-    dcode_core(RAMSTART, RAMEND+1, dcode_mem_t::sram, 2, _N("SRAM"));
+	LOG("D2 - Read/Write RAM\n");
+	uint16_t address = 0x0000; //default 0x0000
+	uint16_t count = 0x2000; //default 0x2000 (entire ram)
+	if (code_seen('A')) // Address (0x0000-0x1fff)
+		address = (strchr_pointer[1] == 'x')?strtol(strchr_pointer + 2, 0, 16):(int)code_value();
+	if (code_seen('C')) // Count (0x0001-0x2000)
+		count = (int)code_value();
+	address &= 0x1fff;
+	if (count > 0x2000) count = 0x2000;
+	if ((address + count) > 0x2000) count = 0x2000 - address;
+	if (code_seen('X')) // Data
+	{
+		uint8_t data[16];
+		count = parse_hex(strchr_pointer + 1, data, 16);
+		if (count > 0)
+		{
+			for (uint16_t i = 0; i < count; i++)
+				*((uint8_t*)(address + i)) =  data[i];
+			LOG("%d bytes written to RAM at address %04x", count, address);
+		}
+		else
+			count = 0;
+	}
+	print_mem(address, count, 0);
+/*	while (count)
+	{
+		print_hex_word(address);
+		putchar(' ');
+		uint8_t countperline = 16;
+		while (count && countperline)
+		{
+			uint8_t data = *((uint8_t*)address++);
+			putchar(' ');
+			print_hex_byte(data);
+			countperline--;
+			count--;
+		}
+		putchar('\n');
+	}*/
 }
-#endif

-#ifdef DEBUG_DCODES
    /*!
-
+    
    ### D4 - Read/Write PIN <a href="https://reprap.org/wiki/G-code#D4:_Read.2FWrite_PIN">D4: Read/Write PIN</a>
    To read the digital value of a pin you need only to define the pin number.
    #### Usage
-
+    
        D4 [ P | F | V ]
-
+    
    #### Parameters
    - `P` - Pin (0-255)
    - `F` - Function in/out (0/1)
@ -328,23 +363,24 @@ void dcode_4()
    ### D5 - Read/Write FLASH <a href="https://reprap.org/wiki/G-code#D5:_Read.2FWrite_FLASH">D5: Read/Write Flash</a>
    This command can be used without any additional parameters. It will read the 1kb FLASH.
    #### Usage
-
+    
        D5 [ A | C | X | E ]
-
+    
    #### Parameters
    - `A` - Address (x00000-x3ffff)
    - `C` - Count (1-8192)
    - `X` - Data (hex)
    - `E` - Erase
-
+ 	
 	#### Notes
 	- The hex address needs to be lowercase without the 0 before the x
-	- Count is decimal
+	- Count is decimal 
 	- The hex data needs to be lowercase
+	
   */
 void dcode_5()
 {
-	puts_P(PSTR("D5 - Read/Write FLASH"));
+	printf_P(PSTR("D5 - Read/Write FLASH\n"));
 	uint32_t address = 0x0000; //default 0x0000
 	uint16_t count = 0x0400; //default 0x0400 (1kb block)
 	if (code_seen('A')) // Address (0x00000-0x3ffff)
@ -402,32 +438,17 @@ void dcode_5()
 }
 #endif //DEBUG_DCODE5

-#if defined(XFLASH) && (defined DEBUG_DCODE6 || defined DEBUG_DCODES)
+#ifdef DEBUG_DCODES
+
    /*!
    ### D6 - Read/Write external FLASH <a href="https://reprap.org/wiki/G-code#D6:_Read.2FWrite_external_FLASH">D6: Read/Write external Flash</a>
-    This command can be used without any additional parameters. It will read the entire XFLASH.
-    #### Usage
-
-        D6 [ A | C | X ]
-
-    #### Parameters
-    - `A` - Address (x0000-x3ffff)
-    - `C` - Count (1-262144)
-    - `X` - Data
-
-	#### Notes
-	- The hex address needs to be lowercase without the 0 before the x
-	- Count is decimal
-	- The hex data needs to be lowercase
-	- Writing is currently not implemented
+    Reserved
    */
 void dcode_6()
 {
-    dcode_core(0x0, XFLASH_SIZE, dcode_mem_t::xflash, 6, _N("XFLASH"));
+	LOG("D6 - Read/Write external FLASH\n");
 }
-#endif

-#ifdef DEBUG_DCODES
    /*!
    ### D7 - Read/Write Bootloader <a href="https://reprap.org/wiki/G-code#D7:_Read.2FWrite_Bootloader">D7: Read/Write Bootloader</a>
    Reserved
@ -449,9 +470,9 @@ void dcode_7()
    /*!
    ### D8 - Read/Write PINDA <a href="https://reprap.org/wiki/G-code#D8:_Read.2FWrite_PINDA">D8: Read/Write PINDA</a>
    #### Usage
-
+    
        D8 [ ? | ! | P | Z ]
-
+    
    #### Parameters
    - `?` - Read PINDA temperature shift values
    - `!` - Reset PINDA temperature shift values to default
@ -460,7 +481,7 @@ void dcode_7()
    */
 void dcode_8()
 {
-	puts_P(PSTR("D8 - Read/Write PINDA"));
+	printf_P(PSTR("D8 - Read/Write PINDA\n"));
 	uint8_t cal_status = calibration_status_pinda();
 	float temp_pinda = current_temperature_pinda;
 	float offset_z = temp_compensation_pinda_thermistor_offset(temp_pinda);
@ -471,7 +492,7 @@ void dcode_8()
 		{
 			uint16_t offs = 0;
 			if (i > 0) offs = eeprom_read_word(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + (i - 1));
-			float foffs = ((float)offs) / cs.axis_steps_per_mm[Z_AXIS];
+			float foffs = ((float)offs) / cs.axis_steps_per_unit[Z_AXIS];
 			offs = 1000 * foffs;
 			printf_P(PSTR("temp_pinda=%dC temp_shift=%dum\n"), 35 + i * 5, offs);
 		}
@ -479,12 +500,12 @@ void dcode_8()
 	else if (strchr_pointer[1+1] == '!')
 	{
 		cal_status = 1;
-		eeprom_write_byte_notify((uint8_t*)EEPROM_CALIBRATION_STATUS_PINDA, cal_status);
-		eeprom_write_word_notify(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + 0,   8); //40C -  20um -   8usteps
-		eeprom_write_word_notify(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + 1,  24); //45C -  60um -  24usteps
-		eeprom_write_word_notify(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + 2,  48); //50C - 120um -  48usteps
-		eeprom_write_word_notify(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + 3,  80); //55C - 200um -  80usteps
-		eeprom_write_word_notify(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + 4, 120); //60C - 300um - 120usteps
+		eeprom_write_byte((uint8_t*)EEPROM_CALIBRATION_STATUS_PINDA, cal_status);
+		eeprom_write_word(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + 0,   8); //40C -  20um -   8usteps
+		eeprom_write_word(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + 1,  24); //45C -  60um -  24usteps
+		eeprom_write_word(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + 2,  48); //50C - 120um -  48usteps
+		eeprom_write_word(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + 3,  80); //55C - 200um -  80usteps
+		eeprom_write_word(((uint16_t*)EEPROM_PROBE_TEMP_SHIFT) + 4, 120); //60C - 300um - 120usteps
 	}
 	else
 	{
@ -502,11 +523,11 @@ void dcode_8()
    /*!
    ### D9 - Read ADC <a href="https://reprap.org/wiki/G-code#D9:_Read.2FWrite_ADC">D9: Read ADC</a>
    #### Usage
-
+    
        D9 [ I | V ]
-
+    
    #### Parameters
-    - `I` - ADC channel index
+    - `I` - ADC channel index 
        - `0` - Heater 0 temperature
        - `1` - Heater 1 temperature
        - `2` - Bed temperature
@ -531,20 +552,26 @@ const char* dcode_9_ADC_name(uint8_t i)
 	return 0;
 }

+#ifdef AMBIENT_THERMISTOR
+extern int current_temperature_raw_ambient;
+#endif //AMBIENT_THERMISTOR
+
+#ifdef VOLT_PWR_PIN
+extern int current_voltage_raw_pwr;
+#endif //VOLT_PWR_PIN
+
+#ifdef VOLT_BED_PIN
+extern int current_voltage_raw_bed;
+#endif //VOLT_BED_PIN
+
 uint16_t dcode_9_ADC_val(uint8_t i)
 {
 	switch (i)
 	{
-#ifdef SHOW_TEMP_ADC_VALUES
 	case 0: return current_temperature_raw[0];
-#endif //SHOW_TEMP_ADC_VALUES
 	case 1: return 0;
-#ifdef SHOW_TEMP_ADC_VALUES
 	case 2: return current_temperature_bed_raw;
-#endif //SHOW_TEMP_ADC_VALUES
-#ifdef PINDA_THERMISTOR
 	case 3: return current_temperature_raw_pinda;
-#endif //PINDA_THERMISTOR
 #ifdef VOLT_PWR_PIN
 	case 4: return current_voltage_raw_pwr;
 #endif //VOLT_PWR_PIN
@ -560,13 +587,12 @@ uint16_t dcode_9_ADC_val(uint8_t i)

 void dcode_9()
 {
-	puts_P(PSTR("D9 - Read/Write ADC"));
+	printf_P(PSTR("D9 - Read/Write ADC\n"));
 	if ((strchr_pointer[1+1] == '?') || (strchr_pointer[1+1] == 0))
 	{
 		for (uint8_t i = 0; i < ADC_CHAN_CNT; i++)
 			printf_P(PSTR("\tADC%d=%4d\t(%S)\n"), i, dcode_9_ADC_val(i) >> 4, dcode_9_ADC_name(i));
 	}
-#if 0
 	else
 	{
 		uint8_t index = 0xff;
@ -577,12 +603,11 @@ void dcode_9()
 			if (code_seen('V')) // value to be written as simulated
 			{
 				adc_sim_mask |= (1 << index);
-				adc_values[index] = ((uint16_t)code_value_short() << 4);
+				adc_values[index] = (((int)code_value()) << 4);
 				printf_P(PSTR("ADC%d=%4d\n"), index, adc_values[index] >> 4);
 			}
 		}
 	}
-#endif
 }

    /*!
@ -591,7 +616,7 @@ void dcode_9()
 void dcode_10()
 {//Tell the printer that XYZ calibration went OK
 	LOG("D10 - XYZ calibration = OK\n");
-	calibration_status_set(CALIBRATION_STATUS_XYZ);
+	calibration_status_store(CALIBRATION_STATUS_LIVE_ADJUST); 
 }

    /*!
@ -610,9 +635,9 @@ void dcode_12()
    ### D80 - Bed check <a href="https://reprap.org/wiki/G-code#D80:_Bed_check">D80: Bed check</a>
    This command will log data to SD card file "mesh.txt".
    #### Usage
-
+    
        D80 [ E | F | G | H | I | J ]
-
+    
    #### Parameters
    - `E` - Dimension X (default 40)
    - `F` - Dimention Y (default 40)
@ -650,9 +675,9 @@ void dcode_80()
    ### D81 - Bed analysis <a href="https://reprap.org/wiki/G-code#D81:_Bed_analysis">D80: Bed analysis</a>
    This command will log data to SD card file "wldsd.txt".
    #### Usage
-
+    
        D81 [ E | F | G | H | I | J ]
-
+    
    #### Parameters
    - `E` - Dimension X (default 40)
    - `F` - Dimention Y (default 40)
@ -676,9 +701,9 @@ void dcode_81()
 	if (code_seen("H")) { strchr_pointer+=1; points_y = code_value(); }
 	if (code_seen("I")) { strchr_pointer+=1; offset_x = code_value(); }
 	if (code_seen("J")) { strchr_pointer+=1; offset_y = code_value(); }
-
+	
 	bed_analysis(dimension_x,dimension_y,points_x,points_y,offset_x,offset_y);
-
+	
 }

 #endif //HEATBED_ANALYSIS
@ -705,11 +730,11 @@ extern void st_synchronize();
    /*!
    ### D2130 - Trinamic stepper controller <a href="https://reprap.org/wiki/G-code#D2130:_Trinamic_stepper_controller">D2130: Trinamic stepper controller</a>
    @todo Please review by owner of the code. RepRap Wiki Gcode needs to be updated after review of owner as well.
-
+    
    #### Usage
-
+    
        D2130 [ Axis | Command | Subcommand | Value ]
-
+    
    #### Parameters
    - Axis
      - `X` - X stepper driver
@ -738,28 +763,28 @@ extern void st_synchronize();
          - `0, 180 --> 250` - Off
          - `0.9 --> 1.25`   - Valid values (recommended is 1.1)
      - `@`   - Home calibrate axis
-
+    
    Examples:
-
+      
          D2130E?wave
-
+      
      Print extruder microstep linearity compensation curve
-
+      
          D2130E!wave0
-
+      
      Disable extruder linearity compensation curve, (sine curve is used)
-
+      
          D2130E!wave220
-
+      
      (sin(x))^1.1 extruder microstep compensation curve used
-
+    
    Notes:
      For more information see https://www.trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC2130_datasheet.pdf
    *
 	*/
 void dcode_2130()
 {
-	puts_P(PSTR("D2130 - TMC2130"));
+	printf_P(PSTR("D2130 - TMC2130\n"));
 	uint8_t axis = 0xff;
 	switch (strchr_pointer[1+4])
 	{
@ -815,7 +840,7 @@ void dcode_2130()
 			}
 			else if (strcmp(strchr_pointer + 7, "wave") == 0)
 			{
-				tmc2130_get_wave(axis, 0);
+				tmc2130_get_wave(axis, 0, stdout);
 			}
 		}
 		else if (strchr_pointer[1+5] == '!')
@ -836,9 +861,9 @@ void dcode_2130()
 					uint16_t res_new = tmc2130_mres2usteps(mres);
 					tmc2130_set_res(axis, res_new);
 					if (res_new > res)
-						cs.axis_steps_per_mm[axis] *= (res_new / res);
+						cs.axis_steps_per_unit[axis] *= (res_new / res);
 					else
-						cs.axis_steps_per_mm[axis] /= (res / res_new);
+						cs.axis_steps_per_unit[axis] /= (res / res_new);
 				}
 			}
 			else if (strncmp(strchr_pointer + 7, "wave", 4) == 0)
@ -858,19 +883,20 @@ void dcode_2130()
 }
 #endif //TMC2130

-#if defined(FILAMENT_SENSOR) && (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
+#ifdef PAT9125
    /*!
    ### D9125 - PAT9125 filament sensor <a href="https://reprap.org/wiki/G-code#D9:_Read.2FWrite_ADC">D9125: PAT9125 filament sensor</a>
    #### Usage
-
+    
        D9125 [ ? | ! | R | X | Y | L ]
-
+    
    #### Parameters
    - `?` - Print values
    - `!` - Print values
    - `R` - Resolution. Not active in code
    - `X` - X values
    - `Y` - Y values
+    - `L` - Activate filament sensor log
    */
 void dcode_9125()
 {
@ -904,102 +930,15 @@ void dcode_9125()
 		pat9125_y = (int)code_value();
 		LOG("pat9125_y=%d\n", pat9125_y);
 	}
+#ifdef DEBUG_FSENSOR_LOG
+	if (code_seen('L'))
+	{
+		fsensor_log = (int)code_value();
+		LOG("fsensor_log=%d\n", fsensor_log);
+	}
+#endif //DEBUG_FSENSOR_LOG
 }
-#endif //defined(FILAMENT_SENSOR) && (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
+#endif //PAT9125
+

 #endif //DEBUG_DCODES
-
-#ifdef XFLASH_DUMP
-#include "xflash_dump.h"
-
-void dcode_20()
-{
-    if(code_seen('E'))
-        xfdump_full_dump_and_reset();
-    else
-    {
-        unsigned long ts = _millis();
-        xfdump_dump();
-        ts = _millis() - ts;
-        DBG(_N("dump completed in %lums\n"), ts);
-    }
-}
-
-void dcode_21()
-{
-    if(!xfdump_check_state())
-        DBG(_N("no dump available\n"));
-    else
-    {
-        KEEPALIVE_STATE(NOT_BUSY);
-        DBG(_N("D21 - read crash dump\n"));
-        print_mem(DUMP_OFFSET, sizeof(dump_t), dcode_mem_t::xflash);
-    }
-}
-
-void dcode_22()
-{
-    if(!xfdump_check_state())
-        DBG(_N("no dump available\n"));
-    else
-    {
-        xfdump_reset();
-        DBG(_N("dump cleared\n"));
-    }
-}
-#endif
-
-#ifdef EMERGENCY_SERIAL_DUMP
-#include "asm.h"
-#include "xflash_dump.h"
-
-bool emergency_serial_dump = false;
-
-void dcode_23()
-{
-    if(code_seen('E'))
-        serial_dump_and_reset(dump_crash_reason::manual);
-    else
-    {
-        emergency_serial_dump = !code_seen('R');
-        SERIAL_ECHOPGM("serial dump ");
-        SERIAL_ECHOLNRPGM(emergency_serial_dump? _N("enabled"): _N("disabled"));
-    }
-}
-
-void __attribute__((noinline)) serial_dump_and_reset(dump_crash_reason reason)
-{
-    uint16_t sp;
-    uint32_t pc;
-
-    // we're being called from a live state, so shut off interrupts ...
-    cli();
-
-    // sample SP/PC
-    sp = SP;
-    pc = GETPC();
-
-    // extend WDT long enough to allow writing the entire stream
-    wdt_enable(WDTO_8S);
-
-    // ... and heaters
-    WRITE(FAN_PIN, HIGH);
-    disable_heater();
-
-    // this function can also be called from within a corrupted state, so not use
-    // printf family of functions that use the heap or grow the stack.
-    SERIAL_ECHOLNPGM("D23 - emergency serial dump");
-    SERIAL_ECHOPGM("error: ");
-    MYSERIAL.print((uint8_t)reason, DEC);
-    SERIAL_ECHOPGM(" 0x");
-    MYSERIAL.print(pc, HEX);
-    SERIAL_ECHOPGM(" 0x");
-    MYSERIAL.println(sp, HEX);
-
-    print_mem(0, RAMEND+1, dcode_mem_t::sram);
-    SERIAL_ECHOLNRPGM(MSG_OK);
-
-    // reset soon
-    softReset();
-}
-#endif
--- a/Firmware/Dcodes.h
+++ b/Firmware/Dcodes.h
@ -4,10 +4,7 @@
 extern void dcode__1(); //D-1 - Endless loop (to simulate deadlock)
 extern void dcode_0(); //D0 - Reset
 extern void dcode_1(); //D1 - Clear EEPROM
-
-#if defined DEBUG_DCODE2 || defined DEBUG_DCODES
 extern void dcode_2(); //D2 - Read/Write RAM
-#endif

 #if defined DEBUG_DCODE3 || defined DEBUG_DCODES
 extern void dcode_3(); //D3 - Read/Write EEPROM
@ -19,29 +16,13 @@ extern void dcode_4(); //D4 - Read/Write PIN
 extern void dcode_5(); //D5 - Read/Write FLASH
 #endif //DEBUG_DCODE5

-#if defined DEBUG_DCODE6 || defined DEBUG_DCODES
 extern void dcode_6(); //D6 - Read/Write external FLASH
-#endif
-
 extern void dcode_7(); //D7 - Read/Write Bootloader
 extern void dcode_8(); //D8 - Read/Write PINDA
 extern void dcode_9(); //D9 - Read/Write ADC (Write=enable simulated, Read=disable simulated)
 extern void dcode_10(); //D10 - XYZ calibration = OK
 extern void dcode_12(); //D12 - Log time. Writes the current time in the log file.

-#ifdef XFLASH_DUMP
-extern void dcode_20(); //D20 - Generate an offline crash dump
-extern void dcode_21(); //D21 - Print crash dump to serial
-extern void dcode_22(); //D22 - Clear crash dump state
-#endif
-
-#ifdef EMERGENCY_SERIAL_DUMP
-#include "xflash_dump.h"
-extern void dcode_23(); //D23 - Request/generate an online serial crash dump
-extern bool emergency_serial_dump; //emergency dump enabled flag
-extern void serial_dump_and_reset(dump_crash_reason);
-#endif
-
 #ifdef HEATBED_ANALYSIS
 extern void dcode_80(); //D80 - Bed check. This command will log data to SD card file "mesh.txt".
 extern void dcode_81(); //D81 - Bed analysis. This command will log data to SD card file "wldsd.txt".
@ -53,9 +34,9 @@ extern void dcode_81(); //D81 - Bed analysis. This command will log data to SD c
 	extern void dcode_2130(); //D2130 - TMC2130
 #endif //TMC2130

-#if defined(FILAMENT_SENSOR) && (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
+#ifdef PAT9125
 	extern void dcode_9125(); //D9125 - PAT9125
-#endif //defined(FILAMENT_SENSOR) && (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
+#endif //PAT9125


 #endif //DCODES_H
--- a/Firmware/Filament_sensor.cpp
+++ b/Firmware/Filament_sensor.cpp
@ -1,509 +0,0 @@
-#include <avr/pgmspace.h>
-#include <stdio.h>
-#include <util/atomic.h>
-
-#include "Filament_sensor.h"
-#include "Timer.h"
-#include "eeprom.h"
-#include "language.h"
-#include "menu.h"
-#include "messages.h"
-#include "mmu2.h"
-#include "planner.h"
-#include "temperature.h"
-#include "ultralcd.h"
-
-#ifdef FILAMENT_SENSOR
-FSensorBlockRunout::FSensorBlockRunout() {
-    fsensor.setRunoutEnabled(false); //suppress filament runouts while loading filament.
-    fsensor.setAutoLoadEnabled(false); //suppress filament autoloads while loading filament.
-#if (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
-    fsensor.setJamDetectionEnabled(false); //suppress filament jam detection while loading filament.
-#endif //(FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
-//    SERIAL_ECHOLNPGM("FSBlockRunout");
-}
-
-FSensorBlockRunout::~FSensorBlockRunout() {
-    fsensor.settings_init(); // restore filament runout state.
-//    SERIAL_ECHOLNPGM("FSUnBlockRunout");
-}
-
-# if FILAMENT_SENSOR_TYPE == FSENSOR_IR
-IR_sensor fsensor;
-# elif FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG
-IR_sensor_analog fsensor;
-# elif FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125
-PAT9125_sensor fsensor;
-# endif
-
-#else // FILAMENT_SENSOR
-FSensorBlockRunout::FSensorBlockRunout() { }
-FSensorBlockRunout::~FSensorBlockRunout() { }
-#endif // FILAMENT_SENSOR
-
-void Filament_sensor::setEnabled(bool enabled) {
-    eeprom_update_byte_notify((uint8_t *)EEPROM_FSENSOR, enabled);
-    if (enabled) {
-        fsensor.init();
-    } else {
-        fsensor.deinit();
-    }
-}
-
-void Filament_sensor::setAutoLoadEnabled(bool state, bool updateEEPROM) {
-    autoLoadEnabled = state;
-    if (updateEEPROM) {
-        eeprom_update_byte_notify((uint8_t *)EEPROM_FSENS_AUTOLOAD_ENABLED, state);
-    }
-}
-
-void Filament_sensor::setRunoutEnabled(bool state, bool updateEEPROM) {
-    runoutEnabled = state;
-    if (updateEEPROM) {
-        eeprom_update_byte_notify((uint8_t *)EEPROM_FSENS_RUNOUT_ENABLED, state);
-    }
-}
-
-void Filament_sensor::setActionOnError(SensorActionOnError state, bool updateEEPROM) {
-    sensorActionOnError = state;
-    if (updateEEPROM) {
-        eeprom_update_byte_notify((uint8_t *)EEPROM_FSENSOR_ACTION_NA, (uint8_t)state);
-    }
-}
-
-void Filament_sensor::settings_init_common() {
-    bool enabled = eeprom_read_byte((uint8_t *)EEPROM_FSENSOR);
-    if ((state != State::disabled) != enabled) {
-        state = enabled ? State::initializing : State::disabled;
-    }
-
-    autoLoadEnabled = eeprom_read_byte((uint8_t *)EEPROM_FSENS_AUTOLOAD_ENABLED);
-    runoutEnabled = eeprom_read_byte((uint8_t *)EEPROM_FSENS_RUNOUT_ENABLED);
-    sensorActionOnError = (SensorActionOnError)eeprom_read_byte((uint8_t *)EEPROM_FSENSOR_ACTION_NA);
-    if (sensorActionOnError == SensorActionOnError::_Undef) {
-        sensorActionOnError = SensorActionOnError::_Continue;
-    }
-}
-
-bool Filament_sensor::checkFilamentEvents() {
-    if (state != State::ready)
-        return false;
-    if (eventBlankingTimer.running() && !eventBlankingTimer.expired(100)) { // event blanking for 100ms
-        return false;
-    }
-
-    bool newFilamentPresent = fsensor.getFilamentPresent();
-    if (oldFilamentPresent != newFilamentPresent) {
-        oldFilamentPresent = newFilamentPresent;
-        eventBlankingTimer.start();
-        if (newFilamentPresent) { // filament insertion
-//            puts_P(PSTR("filament inserted"));
-            triggerFilamentInserted();
-            postponedLoadEvent = true;
-        } else { // filament removal
-//            puts_P(PSTR("filament removed"));
-            triggerFilamentRemoved();
-        }
-        return true;
-    }
-    return false;
-}
-
-void Filament_sensor::triggerFilamentInserted() {
-    if (autoLoadEnabled
-        && (eFilamentAction == FilamentAction::None)
-        && !(
-            MMU2::mmu2.Enabled() // quick and dirty hack to prevent spurious runouts while the MMU is in charge
-            || moves_planned() != 0
-            || printJobOngoing()
-            || (lcd_commands_type == LcdCommands::Layer1Cal)
-            || eeprom_read_byte((uint8_t *)EEPROM_WIZARD_ACTIVE)
-            )
-        ) {
-        menu_submenu(lcd_AutoLoadFilament, true);
-    }
-}
-
-void Filament_sensor::triggerFilamentRemoved() {
-//    SERIAL_ECHOLNPGM("triggerFilamentRemoved");
-    if (runoutEnabled
-        && (eFilamentAction == FilamentAction::None)
-        && (
-            moves_planned() != 0
-            || printJobOngoing()
-        )
-        && !(
-            saved_printing
-            || MMU2::mmu2.Enabled() // quick and dirty hack to prevent spurious runouts just before the toolchange
-            || (lcd_commands_type == LcdCommands::Layer1Cal)
-            || eeprom_read_byte((uint8_t *)EEPROM_WIZARD_ACTIVE)
-        )
-    ){
-//        SERIAL_ECHOPGM("runoutEnabled="); SERIAL_ECHOLN((int)runoutEnabled);
-//        SERIAL_ECHOPGM("eFilamentAction="); SERIAL_ECHOLN((int)eFilamentAction);
-//        SERIAL_ECHOPGM("saved_printing="); SERIAL_ECHOLN((int)saved_printing);
-        filRunout();
-    }
-}
-
-
-void Filament_sensor::filRunout() {
-//    SERIAL_ECHOLNPGM("filRunout");
-    sendHostNotification_P(MSG_FILAMENT_RUNOUT_DETECTED);
-    runoutEnabled = false;
-    autoLoadEnabled = false;
-    stop_and_save_print_to_ram(0, 0);
-    restore_print_from_ram_and_continue(0);
-    eeprom_increment_byte((uint8_t *)EEPROM_FERROR_COUNT);
-    eeprom_increment_word((uint16_t *)EEPROM_FERROR_COUNT_TOT);
-    enquecommand_front_P(MSG_M600);
-}
-
-void Filament_sensor::triggerError() {
-    state = State::error;
-
-    /// some message, idk
-    ; //
-}
-
-#if (FILAMENT_SENSOR_TYPE == FSENSOR_IR) || (FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG)
-void IR_sensor::init() {
-    if (state == State::error) {
-        fsensor.deinit(); // deinit first if there was an error.
-    }
-//    puts_P(PSTR("fsensor::init()"));
-    SET_INPUT(IR_SENSOR_PIN); // input mode
-    WRITE(IR_SENSOR_PIN, 1);  // pullup
-    settings_init();          // also sets the state to State::initializing
-}
-
-void IR_sensor::deinit() {
-//    puts_P(PSTR("fsensor::deinit()"));
-    SET_INPUT(IR_SENSOR_PIN); // input mode
-    WRITE(IR_SENSOR_PIN, 0);  // no pullup
-    state = State::disabled;
-}
-
-bool IR_sensor::update() {
-    switch (state) {
-    case State::initializing:
-        state = State::ready; // the IR sensor gets ready instantly as it's just a gpio read operation.
-        // initialize the current filament state so that we don't create a switching event right after the sensor is ready.
-        oldFilamentPresent = fsensor.getFilamentPresent();
-        [[fallthrough]];
-    case State::ready: {
-        postponedLoadEvent = false;
-        return checkFilamentEvents();
-    } break;
-    case State::disabled:
-    case State::error:
-    default:
-        return false;
-    }
-    return false;
-}
-
-
-
-#ifdef FSENSOR_PROBING
-bool IR_sensor::probeOtherType() { return pat9125_probe(); }
-#endif
-
-void IR_sensor::settings_init() { Filament_sensor::settings_init_common(); }
-
-#if (FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG)
-void IR_sensor_analog::init() {
-    IR_sensor::init();
-    IR_sensor::settings_init();
-    sensorRevision = (SensorRevision)eeprom_read_byte((uint8_t *)EEPROM_FSENSOR_PCB);
-}
-
-bool IR_sensor_analog::update() {
-    bool event = IR_sensor::update();
-    if (state == State::ready) {
-        if (getVoltReady()) {
-            clearVoltReady();
-            uint16_t volt = getVoltRaw();
-//            printf_P(PSTR("newVoltRaw:%u\n"), volt / OVERSAMPLENR);
-
-            // detect min-max, some long term sliding window for filtration may be added
-            // avoiding floating point operations, thus computing in raw
-            if (volt > maxVolt) {
-                maxVolt = volt;
-            } else if (volt < minVolt) {
-                minVolt = volt;
-            }
-            //! The trouble is, I can hold the filament in the hole in such a way, that it creates the exact voltage
-            //! to be detected as the new fsensor
-            //! We can either fake it by extending the detection window to a looooong time
-            //! or do some other countermeasures
-
-            //! what we want to detect:
-            //! if minvolt gets below ~0.3V, it means there is an old fsensor
-            //! if maxvolt gets above 4.6V, it means we either have an old fsensor or broken cables/fsensor
-            //! So I'm waiting for a situation, when minVolt gets to range <0, 1.5> and maxVolt gets into range <3.0, 5>
-            //! If and only if minVolt is in range <0.3, 1.5> and maxVolt is in range <3.0, 4.6>, I'm considering a situation with the new fsensor
-            if (minVolt >= IRsensor_Ldiode_TRESHOLD && minVolt <= IRsensor_Lmax_TRESHOLD && maxVolt >= IRsensor_Hmin_TRESHOLD &&
-                maxVolt <= IRsensor_Hopen_TRESHOLD) {
-                IR_ANALOG_Check(SensorRevision::_Old, SensorRevision::_Rev04);
-            }
-            //! If and only if minVolt is in range <0.0, 0.3> and maxVolt is in range  <4.6, 5.0V>, I'm considering a situation with the old fsensor
-            //! Note, we are not relying on one voltage here - getting just +5V can mean an old fsensor or a broken new sensor - that's why
-            //! we need to have both voltages detected correctly to allow switching back to the old fsensor.
-            else if (minVolt < IRsensor_Ldiode_TRESHOLD && maxVolt > IRsensor_Hopen_TRESHOLD && maxVolt <= IRsensor_VMax_TRESHOLD) {
-                IR_ANALOG_Check(SensorRevision::_Rev04, SensorRevision::_Old);
-            }
-
-            if (!checkVoltage(volt)) {
-                triggerError();
-            }
-        }
-    }
-
-    ; //
-
-    return event;
-}
-
-void IR_sensor_analog::voltUpdate(uint16_t raw) { // to be called from the ADC ISR when a cycle is finished
-    voltRaw = raw;
-    voltReady = true;
-}
-
-uint16_t IR_sensor_analog::getVoltRaw() {
-    uint16_t ret;
-    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { ret = voltRaw; }
-    return ret;
-}
-
-const char *IR_sensor_analog::getIRVersionText() {
-    switch (sensorRevision) {
-    case SensorRevision::_Old:
-        return _T(MSG_IR_03_OR_OLDER);
-    case SensorRevision::_Rev04:
-        return _T(MSG_IR_04_OR_NEWER);
-    default:
-        return _T(MSG_IR_UNKNOWN);
-    }
-}
-
-void IR_sensor_analog::setSensorRevision(SensorRevision rev, bool updateEEPROM) {
-    sensorRevision = rev;
-    if (updateEEPROM) {
-        eeprom_update_byte_notify((uint8_t *)EEPROM_FSENSOR_PCB, (uint8_t)rev);
-    }
-}
-
-bool IR_sensor_analog::checkVoltage(uint16_t raw) {
-    if (IRsensor_Lmax_TRESHOLD <= raw && raw <= IRsensor_Hmin_TRESHOLD) {
-        /// If the voltage is in forbidden range, the fsensor is ok, but the lever is mounted improperly.
-        /// Or the user is so creative so that he can hold a piece of fillament in the hole in such a genius way,
-        /// that the IR fsensor reading is within 1.5 and 3V ... this would have been highly unusual
-        /// and would have been considered more like a sabotage than normal printer operation
-        if (voltageErrorCnt++ > 4) {
-            puts_P(PSTR("fsensor in forbidden range 1.5-3V - check sensor"));
-            return false;
-        }
-    } else {
-        voltageErrorCnt = 0;
-    }
-    if (sensorRevision == SensorRevision::_Rev04) {
-        /// newer IR sensor cannot normally produce 4.6-5V, this is considered a failure/bad mount
-        if (IRsensor_Hopen_TRESHOLD <= raw && raw <= IRsensor_VMax_TRESHOLD) {
-            puts_P(PSTR("fsensor v0.4 in fault range 4.6-5V - unconnected"));
-            return false;
-        }
-    }
-    // otherwise the IR fsensor is considered working correctly
-    return true;
-}
-
-bool IR_sensor_analog::getVoltReady() const {
-    bool ret;
-    ATOMIC_BLOCK(ATOMIC_RESTORESTATE){ ret = voltReady; }
-    return ret;
-}
-
-void IR_sensor_analog::clearVoltReady(){
-    ATOMIC_BLOCK(ATOMIC_RESTORESTATE){ voltReady = false; }
-}
-
-void IR_sensor_analog::IR_ANALOG_Check(SensorRevision isVersion, SensorRevision switchTo) {
-    bool bTemp = (!CHECK_ALL_HEATERS);
-    bTemp = bTemp && (menu_menu == lcd_status_screen);
-    bTemp = bTemp && ((sensorRevision == isVersion) || (sensorRevision == SensorRevision::_Undef));
-    bTemp = bTemp && (state == State::ready);
-    if (bTemp) {
-        nFSCheckCount++;
-        if (nFSCheckCount > FS_CHECK_COUNT) {
-            nFSCheckCount = 0; // not necessary
-            setSensorRevision(switchTo, true);
-            printf_IRSensorAnalogBoardChange();
-            switch (switchTo) {
-            case SensorRevision::_Old:
-                lcd_setstatuspgm(_T(MSG_IR_03_OR_OLDER));
-                break;
-            case SensorRevision::_Rev04:
-                lcd_setstatuspgm(_T(MSG_IR_04_OR_NEWER));
-                break;
-            default:
-                break;
-            }
-        }
-    } else {
-        nFSCheckCount = 0;
-    }
-}
-#endif //(FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG)
-#endif //(FILAMENT_SENSOR_TYPE == FSENSOR_IR) || (FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG)
-
-#if (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
-void PAT9125_sensor::init() {
-    if (state == State::error) {
-        deinit(); // deinit first if there was an error.
-    }
-//    puts_P(PSTR("fsensor::init()"));
-
-    settings_init(); // also sets the state to State::initializing
-
-    calcChunkSteps(cs.axis_steps_per_mm[E_AXIS]); // for jam detection
-
-    if (!pat9125_init()) {
-        deinit();
-        triggerError();
-        ; //
-    }
-#ifdef IR_SENSOR_PIN
-    else if (!READ(IR_SENSOR_PIN)) {
-        ; // MK3 fw on MK3S printer
-    }
-#endif // IR_SENSOR_PIN
-}
-
-void PAT9125_sensor::deinit() {
-//    puts_P(PSTR("fsensor::deinit()"));
-    ; //
-    state = State::disabled;
-    filter = 0;
-}
-
-bool PAT9125_sensor::update() {
-    switch (state) {
-    case State::initializing:
-        if (!updatePAT9125()) {
-            break; // still not stable. Stay in the initialization state.
-        }
-        oldFilamentPresent =
-            getFilamentPresent(); // initialize the current filament state so that we don't create a switching event right after the sensor is ready.
-        oldPos = pat9125_y;
-        state = State::ready;
-        break;
-    case State::ready: {
-        updatePAT9125();
-        postponedLoadEvent = false;
-        bool event = checkFilamentEvents();
-
-        ; //
-
-        return event;
-    } break;
-    case State::disabled:
-    case State::error:
-    default:
-        return false;
-    }
-    return false;
-}
-
-#ifdef FSENSOR_PROBING
-bool PAT9125_sensor::probeOtherType() {
-    SET_INPUT(IR_SENSOR_PIN); // input mode
-    WRITE(IR_SENSOR_PIN, 1);  // pullup
-    _delay_us(100); // wait for the pullup to pull the line high (might be needed, not really sure. The internal pullups are quite weak and there might be a
-                    // long wire attached).
-    bool fsensorDetected = !READ(IR_SENSOR_PIN);
-    WRITE(IR_SENSOR_PIN, 0); // no pullup
-    return fsensorDetected;
-}
-#endif
-
-void PAT9125_sensor::setJamDetectionEnabled(bool state, bool updateEEPROM) {
-    jamDetection = state;
-    oldPos = pat9125_y;
-    resetStepCount();
-    jamErrCnt = 0;
-    if (updateEEPROM) {
-        eeprom_update_byte_notify((uint8_t *)EEPROM_FSENSOR_JAM_DETECTION, state);
-    }
-}
-
-void PAT9125_sensor::settings_init() {
-//    puts_P(PSTR("settings_init"));
-    Filament_sensor::settings_init_common();
-    setJamDetectionEnabled(eeprom_read_byte((uint8_t *)EEPROM_FSENSOR_JAM_DETECTION));
-}
-
-int16_t PAT9125_sensor::getStepCount() {
-    int16_t ret;
-    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { ret = stepCount; }
-    return ret;
-}
-
-void PAT9125_sensor::resetStepCount() {
-    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { stepCount = 0; }
-}
-
-void PAT9125_sensor::filJam() {
-    runoutEnabled = false;
-    autoLoadEnabled = false;
-    jamDetection = false;
-    stop_and_save_print_to_ram(0, 0);
-    restore_print_from_ram_and_continue(0);
-    eeprom_increment_byte((uint8_t *)EEPROM_FERROR_COUNT);
-    eeprom_increment_word((uint16_t *)EEPROM_FERROR_COUNT_TOT);
-    enquecommand_front_P(MSG_M600);
-}
-
-bool PAT9125_sensor::updatePAT9125() {
-    if (jamDetection) {
-        int16_t _stepCount = getStepCount();
-        if (abs(_stepCount) >= chunkSteps) { // end of chunk. Check distance
-            resetStepCount();
-            if (!pat9125_update()) { // get up to date data. reinit on error.
-                init();              // try to reinit.
-            }
-            bool fsDir = (pat9125_y - oldPos) > 0;
-            bool stDir = _stepCount > 0;
-            if (fsDir != stDir) {
-                jamErrCnt++;
-            } else if (jamErrCnt) {
-                jamErrCnt--;
-            }
-            oldPos = pat9125_y;
-        }
-        if (jamErrCnt > 10) {
-            jamErrCnt = 0;
-            filJam();
-        }
-    }
-
-    if (pollingTimer.expired_cont(pollingPeriod)) {
-        pollingTimer.start();
-        if (!pat9125_update()) {
-            init(); // try to reinit.
-        }
-
-        bool present = (pat9125_s < 17) || (pat9125_s >= 17 && pat9125_b >= 50);
-        if (present != filterFilPresent) {
-            filter++;
-        } else if (filter) {
-            filter--;
-        }
-        if (filter >= filterCnt) {
-            filter = 0;
-            filterFilPresent = present;
-        }
-    }
-    return (filter == 0); // return stability
-}
-#endif // #if (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
--- a/Firmware/Filament_sensor.h
+++ b/Firmware/Filament_sensor.h
@ -1,212 +0,0 @@
-#pragma once
-#include <inttypes.h>
-
-#include "cmdqueue.h"
-#include "pins.h"
-#include "fastio.h"
-#include "adc.h"
-#include "pat9125.h"
-
-#define FSENSOR_IR 1
-#define FSENSOR_IR_ANALOG 2
-#define FSENSOR_PAT9125 3
-
-/// Can be used to block printer's filament sensor handling - to avoid errorneous injecting of M600
-/// while doing a toolchange with the MMU
-/// In case of "no filament sensor" these methods default to an empty implementation
-class FSensorBlockRunout {
-public:
-    FSensorBlockRunout();
-    ~FSensorBlockRunout();
-};
-
-/// Base class Filament sensor
-///
-/// Ideally, there could have been a nice class hierarchy of filament sensor types with common functionality
-/// extracted into this base class.
-/// But:
-/// - virtual methods take more space
-/// - we don't need to switch among different filament sensors at runtime
-/// Therefore the class hierarchy carefully avoids using virtual methods and doesn't look too fancy.
-#ifdef FILAMENT_SENSOR
-class Filament_sensor {
-public:
-    enum class State : uint8_t {
-        disabled = 0,
-        initializing,
-        ready,
-        error,
-    };
-
-    enum class SensorActionOnError : uint8_t {
-        _Continue = 0,
-        _Pause = 1,
-        _Undef = EEPROM_EMPTY_VALUE
-    };
-
-    static void setEnabled(bool enabled);
-
-    void setAutoLoadEnabled(bool state, bool updateEEPROM = false);
-    bool getAutoLoadEnabled() const { return autoLoadEnabled; }
-
-    void setRunoutEnabled(bool state, bool updateEEPROM = false);
-    bool getRunoutEnabled() const { return runoutEnabled; }
-
-    void setActionOnError(SensorActionOnError state, bool updateEEPROM = false);
-    SensorActionOnError getActionOnError() const { return sensorActionOnError; }
-
-    bool getFilamentLoadEvent() const { return postponedLoadEvent; }
-
-    bool isError() const { return state == State::error; }
-    bool isReady() const { return state == State::ready; }
-    bool isEnabled() const { return state != State::disabled; }
-
-protected:
-    void settings_init_common();
-
-    bool checkFilamentEvents();
-
-    void triggerFilamentInserted();
-
-    void triggerFilamentRemoved();
-
-    void filRunout();
-
-    void triggerError();
-
-    State state;
-    bool autoLoadEnabled;
-    bool runoutEnabled;
-    bool oldFilamentPresent; //for creating filament presence switching events.
-    bool postponedLoadEvent; //this event lasts exactly one update cycle. It is long enough to be able to do polling for load event.
-    ShortTimer eventBlankingTimer;
-    SensorActionOnError sensorActionOnError;
-};
-
-#if (FILAMENT_SENSOR_TYPE == FSENSOR_IR) || (FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG)
-class IR_sensor: public Filament_sensor {
-public:
-    void init();
-    void deinit();
-    bool update();
-    bool getFilamentPresent() const { return !READ(IR_SENSOR_PIN); }
-#ifdef FSENSOR_PROBING
-    static bool probeOtherType(); //checks if the wrong fsensor type is detected.
-#endif
-    void settings_init();
-};
-
-#if (FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG)
-constexpr static uint16_t Voltage2Raw(float V) {
-    return (V * 1023 * OVERSAMPLENR / VOLT_DIV_REF ) + 0.5F;
-}
-constexpr static float Raw2Voltage(uint16_t raw) {
-    return VOLT_DIV_REF * (raw / (1023.F * OVERSAMPLENR));
-}
-
-class IR_sensor_analog: public IR_sensor {
-public:
-    void init();
-    bool update();
-    void voltUpdate(uint16_t raw);
-
-    uint16_t __attribute__((noinline)) getVoltRaw();
-
-    enum class SensorRevision : uint8_t {
-        _Old = 0,
-        _Rev04 = 1,
-        _Undef = EEPROM_EMPTY_VALUE
-    };
-
-    SensorRevision getSensorRevision() const { return sensorRevision; }
-
-    const char* __attribute__((noinline)) getIRVersionText();
-
-    void setSensorRevision(SensorRevision rev, bool updateEEPROM = false);
-
-    constexpr static uint16_t IRsensor_Ldiode_TRESHOLD = Voltage2Raw(0.3F); // ~0.3V, raw value=982
-    constexpr static uint16_t IRsensor_Lmax_TRESHOLD = Voltage2Raw(1.5F); // ~1.5V (0.3*Vcc), raw value=4910
-    constexpr static uint16_t IRsensor_Hmin_TRESHOLD = Voltage2Raw(3.0F); // ~3.0V (0.6*Vcc), raw value=9821
-    constexpr static uint16_t IRsensor_Hopen_TRESHOLD = Voltage2Raw(4.6F); // ~4.6V (N.C. @ Ru~20-50k, Rd'=56k, Ru'=10k), raw value=15059
-    constexpr static uint16_t IRsensor_VMax_TRESHOLD = Voltage2Raw(5.F); // ~5V, raw value=16368
-
-private:
-    SensorRevision sensorRevision;
-
-    bool voltReady; // set by the adc ISR, therefore avoid accessing the variable directly but use getVoltReady()
-    bool getVoltReady()const;
-    void clearVoltReady();
-
-    uint16_t voltRaw; // set by the adc ISR, therefore avoid accessing the variable directly but use getVoltRaw()
-    bool checkVoltage(uint16_t raw);
-
-    uint16_t minVolt = Voltage2Raw(6.F);
-    uint16_t maxVolt = 0;
-    uint16_t nFSCheckCount;
-    uint8_t voltageErrorCnt;
-
-    static constexpr uint16_t FS_CHECK_COUNT = 4;
-    /// Switching mechanism of the fsensor type.
-    /// Called from 2 spots which have a very similar behavior
-    /// 1: SensorRevision::_Old -> SensorRevision::_Rev04 and print _i("FS v0.4 or newer")
-    /// 2: SensorRevision::_Rev04 -> sensorRevision=SensorRevision::_Old and print _i("FS v0.3 or older")
-    void IR_ANALOG_Check(SensorRevision isVersion, SensorRevision switchTo);
-};
-#endif //(FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG)
-#endif //(FILAMENT_SENSOR_TYPE == FSENSOR_IR) || (FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG)
-
-#if (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
-class PAT9125_sensor: public Filament_sensor {
-public:
-    void init();
-    void deinit();
-    bool update();
-    bool getFilamentPresent() const { return filterFilPresent; }
-#ifdef FSENSOR_PROBING
-    bool probeOtherType(); //checks if the wrong fsensor type is detected.
-#endif
-
-    void setJamDetectionEnabled(bool state, bool updateEEPROM = false);
-    bool getJamDetectionEnabled() const { return jamDetection; }
-
-    void stStep(bool rev) { //from stepper isr
-        stepCount += rev ? -1 : 1;
-    }
-
-    void settings_init();
-private:
-    static constexpr uint16_t pollingPeriod = 10; //[ms]
-    static constexpr uint8_t filterCnt = 5; //how many checks need to be done in order to determine the filament presence precisely.
-    ShortTimer pollingTimer;
-    uint8_t filter;
-    uint8_t filterFilPresent;
-
-    bool jamDetection;
-    int16_t oldPos;
-    int16_t stepCount;
-    int16_t chunkSteps;
-    uint8_t jamErrCnt;
-
-    constexpr void calcChunkSteps(float u) {
-        chunkSteps = (int16_t)(1.25 * u); //[mm]
-    }
-
-    int16_t getStepCount();
-
-    void resetStepCount();
-
-    void filJam();
-
-    bool updatePAT9125();
-};
-#endif //(FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
-
-#if FILAMENT_SENSOR_TYPE == FSENSOR_IR
-extern IR_sensor fsensor;
-#elif FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG
-extern IR_sensor_analog fsensor;
-#elif FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125
-extern PAT9125_sensor fsensor;
-#endif
-
-#endif //FILAMENT_SENSOR
--- a/Firmware/FontTable.h
+++ b/Firmware/FontTable.h
@ -1,74 +0,0 @@
-{0x3E, {0x0F, 0xA8, 0xAF, 0x00, }, 'H'}, // index=0x80, utf8='🄷'
-{0x00, {0x69, 0x96, 0x00, 0x00, }, '\xdf'}, // index=0x81, utf8='°'
-{0x60, {0x25, 0x55, 0x58, 0x87, }, 'h'}, // index=0x82, utf8='🌡'
-{0x04, {0x27, 0xF2, 0xE0, 0x00, }, '^'}, // index=0x83, utf8='⬏'
-{0x34, {0x03, 0xCC, 0x19, 0x60, }, '\xf3'}, // index=0x84, utf8='🔃'
-{0x3C, {0x0E, 0xF8, 0x8F, 0x00, }, '\xdb'}, // index=0x85, utf8='🗀'
-{0x08, {0x02, 0x94, 0x92, 0x00, }, '>'}, // index=0x86, utf8='»'
-{0x1C, {0x07, 0x9A, 0x87, 0x00, }, '\xe5'}, // index=0x87, utf8='🕑'
-{0x24, {0x00, 0x85, 0x28, 0x52, }, '\x7e'}, // index=0x88, utf8='⏬'
-{0x06, {0x00, 0x1B, 0xE4, 0x00, }, '\x7e'}, // index=0x89, utf8='✔'
-{0x78, {0x12, 0x70, 0x78, 0x70, }, 'a'}, // index=0x8A, utf8='á'
-{0xF8, {0x12, 0x78, 0x8F, 0x88, }, 'A'}, // index=0x8B, utf8='Á'
-{0x78, {0x42, 0x70, 0x78, 0x70, }, 'a'}, // index=0x8C, utf8='à'
-{0x78, {0x25, 0x70, 0x78, 0x70, }, 'a'}, // index=0x8D, utf8='â'
-{0xF8, {0x50, 0x78, 0x8F, 0x88, }, '\xd1'}, // index=0x8E, utf8='Ä'
-{0xF0, {0x25, 0x27, 0x07, 0x87, }, 'a'}, // index=0x8F, utf8='å'
-{0xF0, {0x25, 0x27, 0x8F, 0x88, }, 'A'}, // index=0x90, utf8='Å'
-{0x2C, {0x0D, 0x27, 0xAA, 0x50, }, '\xd1'}, // index=0x91, utf8='æ'
-{0xBC, {0x07, 0x07, 0x87, 0x11, }, 'a'}, // index=0x92, utf8='ą'
-{0xBE, {0x78, 0x8F, 0x88, 0x11, }, 'A'}, // index=0x93, utf8='Ą'
-{0x78, {0x52, 0x70, 0x78, 0x70, }, 'a'}, // index=0x94, utf8='ă'
-{0xF8, {0x52, 0x78, 0x8F, 0x88, }, 'A'}, // index=0x95, utf8='Ă'
-{0x20, {0x12, 0x78, 0x88, 0x70, }, 'c'}, // index=0x96, utf8='ć'
-{0x20, {0x52, 0x78, 0x88, 0x70, }, 'c'}, // index=0x97, utf8='č'
-{0x48, {0x52, 0x78, 0x88, 0x87, }, 'C'}, // index=0x98, utf8='Č'
-{0x03, {0x11, 0x17, 0x99, 0x70, }, 'd'}, // index=0x99, utf8='ď'
-{0x18, {0x12, 0x78, 0xF8, 0x70, }, 'e'}, // index=0x9A, utf8='é'
-{0x44, {0x12, 0xF8, 0xF8, 0xF0, }, 'E'}, // index=0x9B, utf8='É'
-{0x18, {0x42, 0x78, 0xF8, 0x70, }, 'e'}, // index=0x9C, utf8='è'
-{0x18, {0x25, 0x78, 0xF8, 0x70, }, 'e'}, // index=0x9D, utf8='ê'
-{0x18, {0x52, 0x78, 0xF8, 0x70, }, 'e'}, // index=0x9E, utf8='ě'
-{0x18, {0x50, 0x78, 0xF8, 0x70, }, 'e'}, // index=0x9F, utf8='ë'
-{0x0C, {0x07, 0x8F, 0x87, 0x23, }, 'e'}, // index=0xA0, utf8='ę'
-{0xA2, {0x0F, 0x8F, 0x8F, 0x11, }, 'E'}, // index=0xA1, utf8='Ę'
-{0x00, {0x12, 0x06, 0x22, 0x70, }, 'i'}, // index=0xA2, utf8='í'
-{0x00, {0x12, 0x07, 0x22, 0x27, }, 'I'}, // index=0xA3, utf8='Í'
-{0x00, {0x25, 0x06, 0x22, 0x70, }, 'i'}, // index=0xA4, utf8='î'
-{0x00, {0x25, 0x07, 0x22, 0x27, }, 'I'}, // index=0xA5, utf8='Î'
-{0x00, {0x12, 0x06, 0x22, 0x27, }, 'l'}, // index=0xA6, utf8='ĺ'
-{0x02, {0x62, 0x36, 0xA2, 0x70, }, 'l'}, // index=0xA7, utf8='ł'
-{0x40, {0x45, 0x6C, 0x44, 0x70, }, 'L'}, // index=0xA8, utf8='Ł'
-{0x03, {0x62, 0x22, 0x22, 0x70, }, 'l'}, // index=0xA9, utf8='ľ'
-{0x43, {0x88, 0x88, 0x88, 0xF0, }, 'L'}, // index=0xAA, utf8='Ľ'
-{0x78, {0x12, 0xBC, 0x88, 0x80, }, 'n'}, // index=0xAB, utf8='ń'
-{0x78, {0x52, 0xBC, 0x88, 0x80, }, 'n'}, // index=0xAC, utf8='ň'
-{0x38, {0x12, 0x78, 0x88, 0x70, }, 'o'}, // index=0xAD, utf8='ó'
-{0x78, {0x12, 0x78, 0x88, 0x87, }, 'O'}, // index=0xAE, utf8='Ó'
-{0x38, {0x42, 0x78, 0x88, 0x70, }, 'o'}, // index=0xAF, utf8='ò'
-{0x38, {0x25, 0x78, 0x88, 0x70, }, 'o'}, // index=0xB0, utf8='ô'
-{0x3A, {0x00, 0x79, 0xAC, 0x78, }, 'o'}, // index=0xB1, utf8='ø'
-{0x3D, {0x07, 0x9A, 0xAC, 0x78, }, 'o'}, // index=0xB2, utf8='Ø'
-{0x78, {0x50, 0x78, 0x88, 0x87, }, '\xef'}, // index=0xB3, utf8='Ö'
-{0x39, {0x49, 0x78, 0x88, 0x70, }, 'o'}, // index=0xB4, utf8='ő'
-{0x08, {0x52, 0xBC, 0x88, 0x80, }, 'r'}, // index=0xB5, utf8='ř'
-{0x98, {0x52, 0xF8, 0x8F, 0x98, }, 'R'}, // index=0xB6, utf8='Ř'
-{0x20, {0x12, 0x78, 0x70, 0xF0, }, 's'}, // index=0xB7, utf8='ś'
-{0x48, {0x12, 0x78, 0x63, 0x87, }, 'S'}, // index=0xB8, utf8='Ś'
-{0x20, {0x52, 0x78, 0x70, 0xF0, }, 's'}, // index=0xB9, utf8='š'
-{0x48, {0x52, 0x78, 0x63, 0x87, }, 'S'}, // index=0xBA, utf8='Š'
-{0x10, {0x07, 0x87, 0x0F, 0x24, }, 's'}, // index=0xBB, utf8='ș'
-{0x12, {0x78, 0x63, 0x87, 0x24, }, 'S'}, // index=0xBC, utf8='Ș'
-{0x60, {0x69, 0x9A, 0x98, 0x8B, }, '\xe2'}, // index=0xBD, utf8='ß'
-{0x23, {0x04, 0xE4, 0x44, 0x30, }, 't'}, // index=0xBE, utf8='ť'
-{0x10, {0x4E, 0x44, 0x43, 0x24, }, 't'}, // index=0xBF, utf8='ț'
-{0x7C, {0x12, 0x88, 0x89, 0x60, }, 'u'}, // index=0xC0, utf8='ú'
-{0x3C, {0x50, 0x88, 0x88, 0x70, }, 'U'}, // index=0xC1, utf8='Ü'
-{0x78, {0x25, 0x28, 0x89, 0x60, }, 'u'}, // index=0xC2, utf8='ů'
-{0x79, {0x49, 0x08, 0x89, 0x60, }, 'u'}, // index=0xC3, utf8='ű'
-{0x3C, {0x12, 0x88, 0x70, 0x70, }, 'y'}, // index=0xC4, utf8='ý'
-{0x0C, {0x12, 0x88, 0x52, 0x22, }, 'Y'}, // index=0xC5, utf8='Ý'
-{0x44, {0x12, 0xF1, 0x24, 0xF0, }, 'z'}, // index=0xC6, utf8='ź'
-{0x44, {0x52, 0xF1, 0x24, 0xF0, }, 'z'}, // index=0xC7, utf8='ž'
-{0x8C, {0x52, 0xF0, 0x36, 0x8F, }, 'Z'}, // index=0xC8, utf8='Ž'
-{0x44, {0x20, 0xF1, 0x24, 0xF0, }, 'z'}, // index=0xC9, utf8='ż'
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@ -4,7 +4,7 @@
 #ifndef MARLIN_H
 #define MARLIN_H

-#include "macros.h"
+#define  FORCE_INLINE __attribute__((always_inline)) inline

 #include <math.h>
 #include <stdio.h>
@ -21,7 +21,7 @@
 #include "Configuration.h"
 #include "pins.h"
 #include "Timer.h"
-#include "printer_state.h"
+extern uint8_t mbl_z_probe_nr;

 #ifndef AT90USB
 #define  HardwareSerial_h // trick to disable the standard HWserial
@ -63,13 +63,25 @@
  #define MYSERIAL MSerial
 #endif

+#include "lcd.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+extern FILE _uartout;
+#ifdef __cplusplus
+}
+#endif
+
+#define uartout (&_uartout)
+
 #define SERIAL_PROTOCOL(x) (MYSERIAL.print(x))
 #define SERIAL_PROTOCOL_F(x,y) (MYSERIAL.print(x,y))
 #define SERIAL_PROTOCOLPGM(x) (serialprintPGM(PSTR(x)))
 #define SERIAL_PROTOCOLRPGM(x) (serialprintPGM((x)))
-#define SERIAL_PROTOCOLLN(x) (MYSERIAL.println(x))
-#define SERIAL_PROTOCOLLNPGM(x) (serialprintlnPGM(PSTR(x)))
-#define SERIAL_PROTOCOLLNRPGM(x) (serialprintlnPGM((x)))
+#define SERIAL_PROTOCOLLN(x) (MYSERIAL.println(x)/*,MYSERIAL.write('\n')*/)
+#define SERIAL_PROTOCOLLNPGM(x) (serialprintPGM(PSTR(x)),MYSERIAL.println()/*write('\n')*/)
+#define SERIAL_PROTOCOLLNRPGM(x) (serialprintPGM((x)),MYSERIAL.println()/*write('\n')*/)


 extern const char errormagic[] PROGMEM;
@ -103,10 +115,8 @@ void serial_echopair_P(const char *s_P, unsigned long v);
 // I'd rather skip a few CPU ticks than 5.5KB (!!) of FLASH
 void serialprintPGM(const char *str);

-//The "ln" variant of the function above.
-void serialprintlnPGM(const char *str);
-
 bool is_buffer_empty();
+void get_command();
 void process_commands();
 void ramming();

@ -121,20 +131,35 @@ void manage_inactivity(bool ignore_stepper_queue=false);
 #endif

 #if defined(Y_ENABLE_PIN) && Y_ENABLE_PIN > -1
-  #define  enable_y() WRITE(Y_ENABLE_PIN, Y_ENABLE_ON)
-  #define disable_y() { WRITE(Y_ENABLE_PIN,!Y_ENABLE_ON); axis_known_position[Y_AXIS] = false; }
+  #ifdef Y_DUAL_STEPPER_DRIVERS
+    #define  enable_y() { WRITE(Y_ENABLE_PIN, Y_ENABLE_ON); WRITE(Y2_ENABLE_PIN,  Y_ENABLE_ON); }
+    #define disable_y() { WRITE(Y_ENABLE_PIN,!Y_ENABLE_ON); WRITE(Y2_ENABLE_PIN, !Y_ENABLE_ON); axis_known_position[Y_AXIS] = false; }
+  #else
+    #define  enable_y() WRITE(Y_ENABLE_PIN, Y_ENABLE_ON)
+    #define disable_y() { WRITE(Y_ENABLE_PIN,!Y_ENABLE_ON); axis_known_position[Y_AXIS] = false; }
+  #endif
 #else
  #define enable_y() ;
  #define disable_y() ;
 #endif

-#if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1
+#if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1 
 	#if defined(Z_AXIS_ALWAYS_ON)
+		  #ifdef Z_DUAL_STEPPER_DRIVERS
+			#define  poweron_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
+			#define poweroff_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+		  #else
 			#define  poweron_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
 			#define poweroff_z() {}
+		  #endif
 	#else
+		#ifdef Z_DUAL_STEPPER_DRIVERS
+			#define  poweron_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
+			#define poweroff_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+		#else
 			#define  poweron_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
 			#define poweroff_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+		#endif
 	#endif
 #else
    #define  poweron_z() {}
@ -145,7 +170,6 @@ void manage_inactivity(bool ignore_stepper_queue=false);
    #define  enable_z()  poweron_z()
    #define disable_z() poweroff_z()
 #else
-    extern bool bEnableForce_z; // Used by ultralcd stealth toggle
    void init_force_z();
    void check_force_z();
    void enable_force_z();
@ -154,6 +178,21 @@ void manage_inactivity(bool ignore_stepper_queue=false);
    #define disable_z() disable_force_z()
 #endif // PSU_Delta

+
+//#if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1
+//#ifdef Z_DUAL_STEPPER_DRIVERS
+//#define  enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
+//#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+//#else
+//#define  enable_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
+//#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+//#endif
+//#else
+//#define enable_z() ;
+//#define disable_z() ;
+//#endif
+
+
 #if defined(E0_ENABLE_PIN) && (E0_ENABLE_PIN > -1)
  #define enable_e0() WRITE(E0_ENABLE_PIN, E_ENABLE_ON)
  #define disable_e0() WRITE(E0_ENABLE_PIN,!E_ENABLE_ON)
@ -162,6 +201,26 @@ void manage_inactivity(bool ignore_stepper_queue=false);
  #define disable_e0() /* nothing */
 #endif

+#if (EXTRUDERS > 1) && defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
+  #define enable_e1() WRITE(E1_ENABLE_PIN, E_ENABLE_ON)
+  #define disable_e1() WRITE(E1_ENABLE_PIN,!E_ENABLE_ON)
+#else
+  #define enable_e1()  /* nothing */
+  #define disable_e1() /* nothing */
+#endif
+
+#if (EXTRUDERS > 2) && defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
+  #define enable_e2() WRITE(E2_ENABLE_PIN, E_ENABLE_ON)
+  #define disable_e2() WRITE(E2_ENABLE_PIN,!E_ENABLE_ON)
+#else
+  #define enable_e2()  /* nothing */
+  #define disable_e2() /* nothing */
+#endif
+
+
+#define FARM_FILAMENT_COLOR_NONE 99;
+
+
 enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
 #define X_AXIS_MASK  1
 #define Y_AXIS_MASK  2
@ -175,137 +234,168 @@ void FlushSerialRequestResend();
 void ClearToSend();
 void update_currents();

-void kill(const char *full_screen_message = NULL);
+void get_coordinates();
+void prepare_move();
+void kill(const char *full_screen_message = NULL, unsigned char id = 0);
+void Stop();
+bool IsStopped();
 void finishAndDisableSteppers();

-void UnconditionalStop();                   // Stop heaters, motion and clear current print status
-void ConditionalStop();                     // Similar to UnconditionalStop, but doesn't disable heaters
-void ThermalStop(bool allow_pause = false); // Emergency stop used by overtemp functions which allows
-                                            // recovery (with pause=true)
-bool IsStopped();                           // Returns true if the print has been stopped
+//put an ASCII command at the end of the current buffer.
+void enquecommand(const char *cmd, bool from_progmem = false);

 //put an ASCII command at the end of the current buffer, read from flash
 #define enquecommand_P(cmd) enquecommand(cmd, true)

+//put an ASCII command at the begin of the current buffer
+void enquecommand_front(const char *cmd, bool from_progmem = false);
+
 //put an ASCII command at the begin of the current buffer, read from flash
 #define enquecommand_front_P(cmd) enquecommand_front(cmd, true)

+void repeatcommand_front();
+
+// Remove all lines from the command queue.
+void cmdqueue_reset();
+
+void prepare_arc_move(char isclockwise);
 void clamp_to_software_endstops(float target[3]);
 void refresh_cmd_timeout(void);

+// Timer counter, incremented by the 1ms Arduino timer.
+// The standard Arduino timer() function returns this value atomically
+// by disabling / enabling interrupts. This is costly, if the interrupts are known
+// to be disabled.
+#ifdef SYSTEM_TIMER_2
+extern volatile unsigned long timer2_millis;
+#else //SYSTEM_TIMER_2
+extern volatile unsigned long timer0_millis;
+#endif //SYSTEM_TIMER_2
+
+// An unsynchronized equivalent to a standard Arduino _millis() function.
+// To be used inside an interrupt routine.
+
+FORCE_INLINE unsigned long millis_nc() { 
+#ifdef SYSTEM_TIMER_2
+	return timer2_millis;
+#else //SYSTEM_TIMER_2
+	return timer0_millis;
+#endif //SYSTEM_TIMER_2
+}
+
 #ifdef FAST_PWM_FAN
 void setPwmFrequency(uint8_t pin, int val);
 #endif

-enum class HeatingStatus : uint8_t
-{
-    NO_HEATING = 0,
-    EXTRUDER_HEATING = 1,
-    EXTRUDER_HEATING_COMPLETE = 2,
-    BED_HEATING = 3,
-    BED_HEATING_COMPLETE = 4,
-};
-
-extern HeatingStatus heating_status;
+#ifndef CRITICAL_SECTION_START
+  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
+  #define CRITICAL_SECTION_END    SREG = _sreg;
+#endif //CRITICAL_SECTION_START

 extern bool fans_check_enabled;
-constexpr float homing_feedrate[] = HOMING_FEEDRATE;
+extern float homing_feedrate[];
 extern uint8_t axis_relative_modes;
 extern float feedrate;
 extern int feedmultiply;
 extern int extrudemultiply; // Sets extrude multiply factor (in percent) for all extruders
+extern int extruder_multiply[EXTRUDERS]; // sets extrude multiply factor (in percent) for each extruder individually
 extern float extruder_multiplier[EXTRUDERS]; // reciprocal of cross-sectional area of filament (in square millimeters), stored this way to reduce computational burden in planner
 extern float current_position[NUM_AXIS] ;
 extern float destination[NUM_AXIS] ;
 extern float min_pos[3];
 extern float max_pos[3];
 extern bool axis_known_position[3];
-extern uint8_t fanSpeed; //!< Print fan speed, ranges from 0 to 255
-extern uint8_t newFanSpeed;
+extern int fanSpeed;
+extern int8_t lcd_change_fil_state;
 extern float default_retraction;

-void get_coordinates();
-void prepare_move(uint16_t start_segment_idx = 0);
-void prepare_arc_move(bool isclockwise, uint16_t start_segment_idx = 0);
-uint16_t restore_interrupted_gcode();
-
-///@brief Helper function to reduce code size, cheaper to call function than to inline division
-///@param feedrate_mm_min feedrate with unit mm per minute
-///@returns feedrate with unit mm per second
-float __attribute__((noinline)) get_feedrate_mm_s(const float feedrate_mm_min);
-
 #ifdef TMC2130
-void check_Z_crash(void);
-void homeaxis(uint8_t axis, uint8_t cnt = 1, uint8_t* pstep = 0);
+void homeaxis(int axis, uint8_t cnt = 1, uint8_t* pstep = 0);
 #else
-void homeaxis(uint8_t axis, uint8_t cnt = 1);
+void homeaxis(int axis, uint8_t cnt = 1);
 #endif //TMC2130

+
+#ifdef FAN_SOFT_PWM
+extern unsigned char fanSpeedSoftPwm;
+#endif
+
 #ifdef FWRETRACT
 extern bool retracted[EXTRUDERS];
 extern float retract_length_swap;
 extern float retract_recover_length_swap;
 #endif

-extern ShortTimer usb_timer;
-extern bool processing_tcode;
+
+extern uint8_t host_keepalive_interval;
+
+extern unsigned long starttime;
+extern unsigned long stoptime;
+extern int bowden_length[4];
+extern bool is_usb_printing;
 extern bool homing_flag;
-extern uint32_t total_filament_used; // mm/100 or 10um
+extern bool loading_flag;
+extern unsigned int usb_printing_counter;

-/// @brief Save print statistics to EEPROM
-void save_statistics();
+extern unsigned long kicktime;

+extern unsigned long total_filament_used;
+void save_statistics(unsigned long _total_filament_used, unsigned long _total_print_time);
+extern unsigned int heating_status;
+extern unsigned int status_number;
+extern unsigned int heating_status_counter;
+extern char snmm_filaments_used;
+extern unsigned long PingTime;
+extern unsigned long NcTime;
+extern bool no_response;
+extern uint8_t important_status;
+extern uint8_t saved_filament_type;
+
+extern bool fan_state[2];
 extern int fan_edge_counter[2];
 extern int fan_speed[2];

-// Active extruder becomes a #define to make the whole firmware compilable.
-// We may even remove the references to it wherever possible in the future
-#define active_extruder 0
+// Handling multiple extruders pins
+extern uint8_t active_extruder;
+
+//Long pause
+extern unsigned long pause_time;
+extern unsigned long start_pause_print;
+extern unsigned long t_fan_rising_edge;

 extern bool mesh_bed_leveling_flag;
-extern bool did_pause_print;
+extern bool mesh_bed_run_from_menu;

+extern bool sortAlpha;
+
+extern char dir_names[3][9];
+
+extern int8_t lcd_change_fil_state;
 // save/restore printing
 extern bool saved_printing;
-extern uint32_t saved_sdpos;
 extern uint8_t saved_printing_type;
+#define PRINTING_TYPE_SD 0
+#define PRINTING_TYPE_USB 1
+#define PRINTING_TYPE_NONE 2

-extern uint16_t saved_extruder_temperature; //!< Active extruder temperature
-extern uint8_t saved_bed_temperature; //!< Bed temperature
-extern bool saved_extruder_relative_mode;
-extern uint8_t saved_fan_speed; //!< Print fan speed, ranges from 0 to 255
-extern float saved_pos[NUM_AXIS];
-extern uint16_t saved_feedrate2;
+//save/restore printing in case that mmu is not responding
+extern bool mmu_print_saved;

 //estimated time to end of the print
 extern uint8_t print_percent_done_normal;
-extern uint8_t print_percent_done_silent;
 extern uint16_t print_time_remaining_normal;
+extern uint8_t print_percent_done_silent;
 extern uint16_t print_time_remaining_silent;
-extern uint16_t print_time_to_change_normal;
-extern uint16_t print_time_to_change_silent;

 #define PRINT_TIME_REMAINING_INIT 0xffff

+extern uint16_t mcode_in_progress;
+extern uint16_t gcode_in_progress;
+
 extern LongTimer safetyTimer;

-#define PRINT_PERCENT_DONE_INIT 0xff
-
-// Returns true if there is a print running. It does not matter if
-// the print is paused, that still counts as a "running" print.
-bool printJobOngoing();
-
-// Make debug_printer_states available everywhere
-#ifdef DEBUG_PRINTER_STATES
-void debug_printer_states();
-#endif //DEBUG_PRINTER_STATES
-
-// Printing is paused according to SD or host indicators
-bool printingIsPaused();
-
-bool printer_active();
-
-bool printer_recovering();
+#define PRINT_PERCENT_DONE_INIT   0xff
+#define PRINTER_ACTIVE (IS_SD_PRINTING || is_usb_printing || isPrintPaused || (custom_message_type == CustomMsg::TempCal) || saved_printing || (lcd_commands_type == LcdCommands::Layer1Cal) || mmu_print_saved)

 //! Beware - mcode_in_progress is set as soon as the command gets really processed,
 //! which is not the same as posting the M600 command into the command queue
@ -314,26 +404,11 @@ bool printer_recovering();
 //! Instead, the fsensor uses another state variable :( , which is set to true, when the M600 command is enqued
 //! and is reset to false when the fsensor returns into its filament runout finished handler
 //! I'd normally change this macro, but who knows what would happen in the MMU :)
-bool check_fsensor();
-
-//! Condition where Babystepping is allowed:
-//! 1) Not allowed during Homing (printer busy)
-//! 2) Not allowed during Mesh Bed Leveling (printer busy)
-//! 3) Not allowed when a print job is paused
-//! 4) Allowed if:
-//!         - First Layer Calibration is running (the event when heaters are turned off is used to dismiss the menu)
-//!         - A print job is running
-//!         - If the printer is idle with not planned moves
-bool babystep_allowed();
-
-//! Same as babystep_allowed() but additionally adds a requirement
-//! where the Z-axis position must be less than 2.0mm (only allowed
-//! during the first couple of layers)
-bool babystep_allowed_strict();
+#define CHECK_FSENSOR ((IS_SD_PRINTING || is_usb_printing) && (mcode_in_progress != 600) && !saved_printing && e_active())

 extern void calculate_extruder_multipliers();

-// Similar to the default Arduino delay function,
+// Similar to the default Arduino delay function, 
 // but it keeps the background tasks running.
 extern void delay_keep_alive(unsigned int ms);

@ -349,6 +424,7 @@ void bed_analysis(float x_dimension, float y_dimension, int x_points_num, int y_
 void bed_check(float x_dimension, float y_dimension, int x_points_num, int y_points_num, float shift_x, float shift_y);
 #endif //HEATBED_ANALYSIS
 float temp_comp_interpolation(float temperature);
+void show_fw_version_warnings();
 uint8_t check_printer_version();

 #ifdef PINDA_THERMISTOR
@ -358,35 +434,30 @@ float temp_compensation_pinda_thermistor_offset(float temperature_pinda);
 void serialecho_temperatures();
 bool check_commands();

+void uvlo_();
+void uvlo_tiny();
+void recover_print(uint8_t automatic); 
+void setup_uvlo_interrupt();
+
+#if defined(TACH_1) && TACH_1 >-1
+void setup_fan_interrupt();
+#endif
+
+extern bool recover_machine_state_after_power_panic();
+extern void restore_print_from_eeprom(bool mbl_was_active);
+extern void position_menu();
+
 extern void print_world_coordinates();
 extern void print_physical_coordinates();
 extern void print_mesh_bed_leveling_table();

-void save_print_file_state();
-void restore_print_file_state();
-void save_planner_global_state();
-void refresh_print_state_in_ram();
-
-/// Updates the feedrate multiplier when a print is saved such that
-/// it is not overwritten when the print is later resumed
-void refresh_saved_feedrate_multiplier_in_ram();
-void clear_print_state_in_ram();
 extern void stop_and_save_print_to_ram(float z_move, float e_move);
-void restore_file_from_sd();
-void restore_extruder_temperature_from_ram();
 extern void restore_print_from_ram_and_continue(float e_move);
 extern void cancel_saved_printing();

-// Define some coordinates outside the clamp limits (making them invalid past the parsing stage) so
-// that they can be used later for various logical checks
-#define X_COORD_INVALID (X_MIN_POS-1)
-#define SAVED_START_POSITION_UNSET X_COORD_INVALID
-extern float saved_start_position[NUM_AXIS];
-extern uint16_t saved_segment_idx;
-extern bool isPartialBackupAvailable;
-

 //estimated time to end of the print
+extern uint16_t print_time_remaining();
 extern uint8_t calc_percent_done();


@ -396,7 +467,7 @@ extern uint8_t calc_percent_done();
 /*enum MarlinBusyState {
 	NOT_BUSY,           // Not in a handler
 	IN_HANDLER,         // Processing a GCode
-	IN_PROCESS,         // Known to be blocking command input
+	IN_PROCESS,         // Known to be blocking command input (as in G29)
 	PAUSED_FOR_USER,    // Blocking pending any input
 	PAUSED_FOR_INPUT    // Blocking pending text input (concept)
 };*/
@ -409,7 +480,6 @@ extern uint8_t calc_percent_done();

 #define KEEPALIVE_STATE(n) do { busy_state = n;} while (0)
 extern void host_keepalive();
-extern void host_autoreport();
 //extern MarlinBusyState busy_state;
 extern int8_t busy_state;

@ -420,7 +490,6 @@ extern int8_t busy_state;
 #define FORCE_HIGH_POWER_END	force_high_power_mode(false)

 void force_high_power_mode(bool start_high_power_section);
-void change_power_mode_live(uint8_t mode);

 #endif //TMC2130

@ -428,25 +497,21 @@ void change_power_mode_live(uint8_t mode);

 bool gcode_M45(bool onlyZ, int8_t verbosity_level);
 void gcode_M114();
-#if (defined(FANCHECK) && (((defined(TACH_0) && (TACH_0 >-1)) || (defined(TACH_1) && (TACH_1 > -1)))))
-void gcode_M123();
-#endif //FANCHECK and TACH_0 and TACH_1
-void gcode_M701(float fastLoadLength, uint8_t mmuSlotIndex);
+void gcode_M701();

 #define UVLO !(PINE & (1<<4))

+void proc_commands();

-void M600_load_filament(const char* filament_name);
-void M600_load_filament_movements(const char* filament_name);
-void M600_wait_for_user();
-bool M600_check_state_and_repeat(const char* filament_name);
+
+void M600_load_filament();
+void M600_load_filament_movements();
+void M600_wait_for_user(float HotendTempBckp);
+void M600_check_state(float nozzle_temp);
 void load_filament_final_feed();
 void marlin_wait_for_click();
-float raise_z(float delta);
-void raise_z_above(float target);
+void raise_z_above(float target, bool plan=true);

 extern "C" void softReset();
-void stack_error();

-extern uint32_t IP_address;
 #endif
--- a/Firmware/MarlinSerial.cpp
+++ b/Firmware/MarlinSerial.cpp
@ -15,7 +15,7 @@
  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
-
+  
  Modified 23 November 2006 by David A. Mellis
  Modified 28 September 2010 by Mark Sproul
 */
@ -26,11 +26,11 @@
 uint8_t selectedSerialPort = 0;

 #ifndef AT90USB
-// this next line disables the entire HardwareSerial.cpp,
+// this next line disables the entire HardwareSerial.cpp, 
 // this is so I can support Attiny series and any other chip without a UART
 #if defined(UBRRH) || defined(UBRR0H) || defined(UBRR1H) || defined(UBRR2H) || defined(UBRR3H)

-#ifdef HAS_UART
+#if UART_PRESENT(SERIAL_PORT)
  ring_buffer rx_buffer  =  { { 0 }, 0, 0 };
 #endif

@ -75,7 +75,7 @@ ISR(M_USARTx_RX_vect)
 #endif //DEBUG_DUMP_TO_2ND_SERIAL
 	}
 }
-
+#ifndef SNMM
 ISR(USART1_RX_vect)
 {
 	// Test for a framing error.
@ -97,6 +97,7 @@ ISR(USART1_RX_vect)
 	}
 }
 #endif
+#endif

 // Public Methods //////////////////////////////////////////////////////////////

@ -129,8 +130,10 @@ void MarlinSerial::begin(long baud)
  sbi(M_UCSRxB, M_RXENx);
  sbi(M_UCSRxB, M_TXENx);
  sbi(M_UCSRxB, M_RXCIEx);
+  
+#ifndef SNMM

-  if (selectedSerialPort == 1) { //set up also the second serial port
+  if (selectedSerialPort == 1) { //set up also the second serial port 
 	  if (useU2X) {
 		  UCSR1A = 1 << U2X1;
 		  baud_setting = (F_CPU / 4 / baud - 1) / 2;
@ -142,11 +145,12 @@ void MarlinSerial::begin(long baud)
 	  // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register)
 	  UBRR1H = baud_setting >> 8;
 	  UBRR1L = baud_setting;
-
+	  
 	  sbi(UCSR1B, RXEN1);
 	  sbi(UCSR1B, TXEN1);
-	  sbi(UCSR1B, RXCIE1);
+	  sbi(UCSR1B, RXCIE1);	  
  }
+#endif
 }

 void MarlinSerial::end()
@ -155,9 +159,11 @@ void MarlinSerial::end()
  cbi(M_UCSRxB, M_TXENx);
  cbi(M_UCSRxB, M_RXCIEx);

+#ifndef SNMM
  cbi(UCSR1B, RXEN1);
  cbi(UCSR1B, TXEN1);
  cbi(UCSR1B, RXCIE1);
+#endif
 }


@ -249,8 +255,8 @@ void MarlinSerial::print(double n, int digits)

 void MarlinSerial::println(void)
 {
-  // print('\r');
-  print('\n');
+  print('\r');
+  print('\n');  
 }

 /*void MarlinSerial::println(const String &s)
@ -311,13 +317,13 @@ void MarlinSerial::println(double n, int digits)

 void MarlinSerial::printNumber(unsigned long n, uint8_t base)
 {
-  unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars.
-  uint8_t i = 0;
+  unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. 
+  unsigned long i = 0;

  if (n == 0) {
    print('0');
    return;
-  }
+  } 

  while (n > 0) {
    buf[i++] = n % base;
@ -330,8 +336,8 @@ void MarlinSerial::printNumber(unsigned long n, uint8_t base)
      'A' + buf[i - 1] - 10));
 }

-void MarlinSerial::printFloat(double number, uint8_t digits)
-{
+void MarlinSerial::printFloat(double number, uint8_t digits) 
+{ 
  // Handle negative numbers
  if (number < 0.0)
  {
@ -343,7 +349,7 @@ void MarlinSerial::printFloat(double number, uint8_t digits)
  double rounding = 0.5;
  for (uint8_t i=0; i<digits; ++i)
    rounding /= 10.0;
-
+  
  number += rounding;

  // Extract the integer part of the number and print it
@ -353,7 +359,7 @@ void MarlinSerial::printFloat(double number, uint8_t digits)

  // Print the decimal point, but only if there are digits beyond
  if (digits > 0)
-    print('.');
+    print("."); 

  // Extract digits from the remainder one at a time
  while (digits-- > 0)
@ -361,8 +367,8 @@ void MarlinSerial::printFloat(double number, uint8_t digits)
    remainder *= 10.0;
    int toPrint = int(remainder);
    print(toPrint);
-    remainder -= toPrint;
-  }
+    remainder -= toPrint; 
+  } 
 }
 // Preinstantiate Objects //////////////////////////////////////////////////////

--- a/Firmware/MarlinSerial.h
+++ b/Firmware/MarlinSerial.h
@ -23,35 +23,33 @@
 #define MarlinSerial_h
 #include "Marlin.h"

-#if !defined(SERIAL_PORT)
+#if !defined(SERIAL_PORT) 
 #define SERIAL_PORT 0
 #endif

 // The presence of the UBRRH register is used to detect a UART.
-#if ((SERIAL_PORT == 0 && (defined(UBRRH) || defined(UBRR0H))) || \
-	(SERIAL_PORT == 1 && defined(UBRR1H)) || \
-	(SERIAL_PORT == 2 && defined(UBRR2H)) || \
-	(SERIAL_PORT == 3 && defined(UBRR3H)))
-#define HAS_UART
-#endif
-
+#define UART_PRESENT(port) ((port == 0 && (defined(UBRRH) || defined(UBRR0H))) || \
+						(port == 1 && defined(UBRR1H)) || (port == 2 && defined(UBRR2H)) || \
+						(port == 3 && defined(UBRR3H)))				
+						
 // These are macros to build serial port register names for the selected SERIAL_PORT (C preprocessor
 // requires two levels of indirection to expand macro values properly)
+#define SERIAL_REGNAME(registerbase,number,suffix) SERIAL_REGNAME_INTERNAL(registerbase,number,suffix)
 #if SERIAL_PORT == 0 && (!defined(UBRR0H) || !defined(UDR0)) // use un-numbered registers if necessary
-#define SERIAL_REGNAME(registerbase,number,suffix) _REGNAME_SHORT(registerbase, suffix)
+#define SERIAL_REGNAME_INTERNAL(registerbase,number,suffix) registerbase##suffix
 #else
-#define SERIAL_REGNAME(registerbase,number,suffix) _REGNAME(registerbase, number, suffix)
+#define SERIAL_REGNAME_INTERNAL(registerbase,number,suffix) registerbase##number##suffix
 #endif

-// Registers used by MarlinSerial class (these are expanded
+// Registers used by MarlinSerial class (these are expanded 
 // depending on selected serial port
 #define M_UCSRxA SERIAL_REGNAME(UCSR,SERIAL_PORT,A) // defines M_UCSRxA to be UCSRnA where n is the serial port number
-#define M_UCSRxB SERIAL_REGNAME(UCSR,SERIAL_PORT,B)
-#define M_RXENx SERIAL_REGNAME(RXEN,SERIAL_PORT,)
-#define M_TXENx SERIAL_REGNAME(TXEN,SERIAL_PORT,)
-#define M_RXCIEx SERIAL_REGNAME(RXCIE,SERIAL_PORT,)
-#define M_UDREx SERIAL_REGNAME(UDRE,SERIAL_PORT,)
-#define M_UDRx SERIAL_REGNAME(UDR,SERIAL_PORT,)
+#define M_UCSRxB SERIAL_REGNAME(UCSR,SERIAL_PORT,B) 
+#define M_RXENx SERIAL_REGNAME(RXEN,SERIAL_PORT,)    
+#define M_TXENx SERIAL_REGNAME(TXEN,SERIAL_PORT,)    
+#define M_RXCIEx SERIAL_REGNAME(RXCIE,SERIAL_PORT,)    
+#define M_UDREx SERIAL_REGNAME(UDRE,SERIAL_PORT,)    
+#define M_UDRx SERIAL_REGNAME(UDR,SERIAL_PORT,)  
 #define M_UBRRxH SERIAL_REGNAME(UBRR,SERIAL_PORT,H)
 #define M_UBRRxL SERIAL_REGNAME(UBRR,SERIAL_PORT,L)
 #define M_RXCx SERIAL_REGNAME(RXC,SERIAL_PORT,)
@ -84,7 +82,7 @@ struct ring_buffer
  int tail;
 };

-#ifdef HAS_UART
+#if UART_PRESENT(SERIAL_PORT)
  extern ring_buffer rx_buffer;
 #endif

@ -97,7 +95,7 @@ class MarlinSerial //: public Stream
    static int peek(void);
    static int read(void);
    static void flush(void);
-
+    
    static /*FORCE_INLINE*/ int available(void)
    {
      return (unsigned int)(RX_BUFFER_SIZE + rx_buffer.head - rx_buffer.tail) % RX_BUFFER_SIZE;
@ -124,7 +122,7 @@ class MarlinSerial //: public Stream
 			UDR1 = c;
 		}
 	}
-
+    
    static void checkRx(void)
    {
        if (selectedSerialPort == 0) {
@ -177,15 +175,15 @@ class MarlinSerial //: public Stream
            }
        }
    }
-
-
+    
+    
    private:
    static void printNumber(unsigned long, uint8_t);
    static void printFloat(double, uint8_t);
-
-
+    
+    
  public:
-
+    
    static /*FORCE_INLINE*/ void write(const char *str)
    {
      while (*str)
@ -205,7 +203,7 @@ class MarlinSerial //: public Stream
        write(s[i]);
      }
    }*/
-
+    
    static FORCE_INLINE void print(const char *str)
    {
      write(str);
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
--- a/Firmware/Prusa_farm.cpp
+++ b/Firmware/Prusa_farm.cpp
@ -1,496 +0,0 @@
-#include "Prusa_farm.h"
-#include "macros.h"
-#include "Marlin.h"
-#include "cmdqueue.h"
-#include "temperature.h"
-#include "cardreader.h"
-#include "util.h"
-#include "ultralcd.h"
-#include "Filament_sensor.h"
-#include "language.h"
-#include "lcd.h"
-#include "stopwatch.h"
-
-#ifdef PRUSA_FARM
-uint8_t farm_mode = 0;
-
-static ShortTimer NcTime;
-static uint8_t farm_timer = 8;
-static uint8_t status_number = 0;
-static bool no_response = false;
-#ifdef PRUSA_M28
-#define CHUNK_SIZE 64 // bytes
-#define SAFETY_MARGIN 1
-bool prusa_sd_card_upload = false;
-char chunk[CHUNK_SIZE+SAFETY_MARGIN];
-#endif
-
-
-static void prusa_statistics_err(char c);
-static void prusa_stat_printerstatus(uint8_t _status);
-static void prusa_stat_farm_number();
-static void prusa_stat_diameter();
-static void prusa_stat_temperatures();
-static void prusa_stat_printinfo();
-static void lcd_send_status();
-#ifdef FARM_CONNECT_MESSAGE
-static void proc_commands();
-static void lcd_connect_printer();
-#endif //FARM_CONNECT_MESSAGE
-#ifdef PRUSA_M28
-static void trace();
-#endif
-
-
-static void prusa_statistics_err(char c) {
-    SERIAL_ECHOPGM("{[ERR:");
-    SERIAL_ECHO(c);
-    SERIAL_ECHO(']');
-    prusa_stat_farm_number();
-}
-
-static void prusa_statistics_case0(uint8_t statnr) {
-    SERIAL_ECHO('{');
-    prusa_stat_printerstatus(statnr);
-    prusa_stat_farm_number();
-    prusa_stat_printinfo();
-}
-
-static void prusa_stat_printerstatus(uint8_t _status) {
-    SERIAL_ECHOPGM("[PRN:");
-    SERIAL_ECHO(_status);
-    SERIAL_ECHO(']');
-}
-
-static void prusa_stat_farm_number() {
-    SERIAL_ECHOPGM("[PFN:0]");
-}
-
-static void prusa_stat_diameter() {
-    SERIAL_ECHOPGM("[DIA:");
-    SERIAL_ECHO(eeprom_read_word((uint16_t*)EEPROM_NOZZLE_DIAMETER_uM));
-    SERIAL_ECHO(']');
-}
-
-static void prusa_stat_temperatures() {
-    SERIAL_ECHOPGM("[ST0:");
-    SERIAL_ECHO(target_temperature[0]);
-    SERIAL_ECHOPGM("][STB:");
-    SERIAL_ECHO(target_temperature_bed);
-    SERIAL_ECHOPGM("][AT0:");
-    SERIAL_ECHO(current_temperature[0]);
-    SERIAL_ECHOPGM("][ATB:");
-    SERIAL_ECHO(current_temperature_bed);
-    SERIAL_ECHO(']');
-}
-
-static void prusa_stat_printinfo() {
-    SERIAL_ECHOPGM("[TFU:");
-    SERIAL_ECHO(total_filament_used);
-    SERIAL_ECHOPGM("][PCD:");
-    SERIAL_ECHO((int)card.percentDone());
-    SERIAL_ECHOPGM("][FEM:");
-    SERIAL_ECHO(feedmultiply);
-    SERIAL_ECHOPGM("][FNM:");
-    SERIAL_ECHO(card.longFilename[0] ? card.longFilename : card.filename);
-    SERIAL_ECHOPGM("][TIM:");
-    if (print_job_timer.isRunning()) {
-        SERIAL_ECHO(print_job_timer.duration());
-    }
-    else {
-        SERIAL_ECHO(0);
-    }
-    SERIAL_ECHOPGM("][FWR:");
-    SERIAL_ECHORPGM(FW_VERSION_STR_P());
-    SERIAL_ECHO(']');
-    prusa_stat_diameter();
-}
-
-static void lcd_send_status() {
-    if (farm_mode && no_response && (NcTime.expired(NC_TIME * 1000))) {
-        //send important status messages periodicaly
-        prusa_statistics(8);
-        NcTime.start();
-#ifdef FARM_CONNECT_MESSAGE
-        lcd_connect_printer();
-#endif //FARM_CONNECT_MESSAGE
-    }
-}
-
-#ifdef FARM_CONNECT_MESSAGE
-static void proc_commands() {
-    if (buflen) {
-        process_commands();
-        if (!cmdbuffer_front_already_processed)
-            cmdqueue_pop_front();
-        cmdbuffer_front_already_processed = false;
-    }
-}
-
-static void lcd_connect_printer() {
-    lcd_update_enable(false);
-    lcd_clear();
-
-    int i = 0;
-    int t = 0;
-    lcd_puts_at_P(0, 0, PSTR("Connect printer to"));
-    lcd_puts_at_P(0, 1, PSTR("monitoring or hold"));
-    lcd_puts_at_P(0, 2, PSTR("the knob to continue"));
-    while (no_response) {
-        i++;
-        t++;
-        delay_keep_alive(100);
-        proc_commands();
-        if (t == 10) {
-            prusa_statistics(8);
-            t = 0;
-        }
-        if (READ(BTN_ENC)) { //if button is not pressed
-            i = 0;
-            lcd_puts_at_P(0, 3, PSTR("                    "));
-        }
-        if (i != 0)
-            lcd_putc_at((i * 20) / (NC_BUTTON_LONG_PRESS * 10), 3, LCD_STR_SOLID_BLOCK[0]);
-        if (i == NC_BUTTON_LONG_PRESS * 10)
-            no_response = false;
-    }
-    lcd_update_enable(true);
-    lcd_update(2);
-}
-#endif //FARM_CONNECT_MESSAGE
-
-#ifdef PRUSA_M28
-static void trace() {
-    Sound_MakeCustom(25,440,true);
-}
-
-void serial_read_stream() {
-    disable_heater();
-
-    lcd_clear();
-    lcd_puts_P(PSTR(" Upload in progress"));
-
-    // first wait for how many bytes we will receive
-    uint32_t bytesToReceive;
-
-    // receive the four bytes
-    char bytesToReceiveBuffer[4];
-    for (int i=0; i<4; i++) {
-        int data;
-        while ((data = MYSERIAL.read()) == -1) {};
-        bytesToReceiveBuffer[i] = data;
-
-    }
-
-    // make it a uint32
-    memcpy(&bytesToReceive, &bytesToReceiveBuffer, 4);
-
-    // we're ready, notify the sender
-    MYSERIAL.write('+');
-
-    // lock in the routine
-    uint32_t receivedBytes = 0;
-    while (prusa_sd_card_upload) {
-        int i;
-        for (i=0; i<CHUNK_SIZE; i++) {
-            int data;
-
-            // check if we're not done
-            if (receivedBytes == bytesToReceive) {
-                break;
-            }
-
-            // read the next byte
-            while ((data = MYSERIAL.read()) == -1) {};
-            receivedBytes++;
-
-            // save it to the chunk
-            chunk[i] = data;
-        }
-
-        // write the chunk to SD
-        card.write_command_no_newline(&chunk[0]);
-
-        // notify the sender we're ready for more data
-        MYSERIAL.write('+');
-
-        // for safety
-        manage_heater();
-
-        // check if we're done
-        if(receivedBytes == bytesToReceive) {
-            trace(); // beep
-            card.closefile();
-            prusa_sd_card_upload = false;
-            SERIAL_PROTOCOLLNRPGM(MSG_FILE_SAVED);
-        }
-    }
-}
-#endif //PRUSA_M28
-
-
-void prusa_statistics(uint8_t _message) {
-    const uint8_t _fil_nr = 0;
-    if (!farm_mode)
-        return;
-
-    switch (_message) {
-    case 0: // default message
-        if (busy_state == PAUSED_FOR_USER) {
-            prusa_statistics_case0(15);
-        }
-        else if (printingIsPaused()) {
-            prusa_statistics_case0(14);
-        }
-        else if (IS_SD_PRINTING || (eFilamentAction != FilamentAction::None)) {
-            prusa_statistics_case0(4);
-        }
-        else {
-            SERIAL_ECHO('{');
-            prusa_stat_printerstatus(1);
-            prusa_stat_farm_number();
-            prusa_stat_diameter();
-            status_number = 1;
-        }
-        break;
-
-    case 1: // 1 heating
-        SERIAL_ECHO('{');
-        prusa_stat_printerstatus(2);
-        prusa_stat_farm_number();
-        status_number = 2;
-        farm_timer = 1;
-        break;
-
-    case 2: // heating done
-        SERIAL_ECHO('{');
-        prusa_stat_printerstatus(3);
-        prusa_stat_farm_number();
-        SERIAL_ECHOLN('}');
-        status_number = 3;
-        farm_timer = 1;
-
-        if (IS_SD_PRINTING || (eFilamentAction != FilamentAction::None)) {
-            SERIAL_ECHO('{');
-            prusa_stat_printerstatus(4);
-            prusa_stat_farm_number();
-            status_number = 4;
-        }
-        else {
-            SERIAL_ECHO('{');
-            prusa_stat_printerstatus(3);
-            prusa_stat_farm_number();
-            status_number = 3;
-        }
-        farm_timer = 1;
-        break;
-
-    case 3: // filament change
-        // must do a return here to prevent doing SERIAL_ECHOLN("}") at the very end of this function
-        // saved a considerable amount of FLASH
-        return;
-        break;
-    case 4: // print succesfull
-        SERIAL_ECHOPGM("{[RES:1][FIL:");
-        MYSERIAL.print(int(_fil_nr));
-        SERIAL_ECHO(']');
-        prusa_stat_printerstatus(status_number);
-        prusa_stat_farm_number();
-        farm_timer = 2;
-        break;
-    case 5: // print not succesfull
-        SERIAL_ECHOPGM("{[RES:0][FIL:");
-        MYSERIAL.print(int(_fil_nr));
-        SERIAL_ECHO(']');
-        prusa_stat_printerstatus(status_number);
-        prusa_stat_farm_number();
-        farm_timer = 2;
-        break;
-    case 6: // print done
-        SERIAL_ECHOPGM("{[PRN:8]");
-        prusa_stat_farm_number();
-        status_number = 8;
-        farm_timer = 2;
-        break;
-    case 7:        // print done - stopped
-        SERIAL_ECHOPGM("{[PRN:9]");
-        prusa_stat_farm_number();
-        status_number = 9;
-        farm_timer = 2;
-        break;
-    case 8: // printer started
-        SERIAL_ECHOPGM("{[PRN:0]");
-        prusa_stat_farm_number();
-        status_number = 0;
-        farm_timer = 2;
-        break;
-    case 20: // echo farm no
-        SERIAL_ECHO('{');
-        prusa_stat_printerstatus(status_number);
-        prusa_stat_farm_number();
-        farm_timer = 4;
-        break;
-    case 21: // temperatures
-        SERIAL_ECHO('{');
-        prusa_stat_temperatures();
-        prusa_stat_farm_number();
-        prusa_stat_printerstatus(status_number);
-        break;
-    case 22: // waiting for filament change
-        SERIAL_ECHOPGM("{[PRN:5]");
-        prusa_stat_farm_number();
-        status_number = 5;
-        break;
-
-    case 90: // Error - Thermal Runaway
-        prusa_statistics_err('1');
-        break;
-    case 91: // Error - Thermal Runaway Preheat
-        prusa_statistics_err('2');
-        break;
-    case 92: // Error - Min temp
-        prusa_statistics_err('3');
-        break;
-    case 93: // Error - Max temp
-        prusa_statistics_err('4');
-        break;
-
-    case 99: // heartbeat
-        SERIAL_ECHOPGM("{[PRN:99]");
-        prusa_stat_temperatures();
-        prusa_stat_farm_number();
-        break;
-    }
-    SERIAL_ECHOLN('}');
-}
-
-void prusa_statistics_update_from_status_screen() {
-    if (farm_mode) {
-        farm_timer--;
-        if (farm_timer < 1) {
-            farm_timer = 10;
-            prusa_statistics(0);
-        }
-        switch (farm_timer) {
-        case 8:
-            prusa_statistics(21);
-            if(eFilamentAction != FilamentAction::None)
-                prusa_statistics(22);
-            break;
-        case 5:
-            if (IS_SD_PRINTING)
-                prusa_statistics(20);
-            break;
-        }
-    }
-}
-
-void prusa_statistics_update_from_lcd_update() {
-    lcd_send_status();
-}
-
-void farm_mode_init() {
-    farm_mode = eeprom_init_default_byte((uint8_t*)EEPROM_FARM_MODE, 0);
-    if (farm_mode) {
-        no_response = true; //we need confirmation by recieving PRUSA thx
-        prusa_statistics(8);
-#ifdef HAS_SECOND_SERIAL_PORT
-        selectedSerialPort = 1;
-#endif //HAS_SECOND_SERIAL_PORT
-        MYSERIAL.begin(BAUDRATE);
-#ifdef FILAMENT_SENSOR
-        //to be converted to Filament_sensor.h...
-        //disabled filament autoload (PFW360)
-        fsensor.setAutoLoadEnabled(false);
-#endif //FILAMENT_SENSOR
-        // ~ FanCheck -> on
-        eeprom_update_byte_notify((uint8_t*)EEPROM_FAN_CHECK_ENABLED, true);
-    }
-}
-
-bool farm_prusa_code_seen() {
-    if (!farm_mode)
-        return false;
-
-    if (code_seen_P(PSTR("PRN"))) { // PRUSA PRN
-        printf_P(_N("%u"), status_number);
-    }
-    else if (code_seen_P(PSTR("thx"))) { // PRUSA thx
-        no_response = false;
-    }
-#ifdef PRUSA_M28
-    else if (code_seen_P(PSTR("M28"))) { // PRUSA M28
-        trace();
-        prusa_sd_card_upload = true;
-        card.openFileWrite(strchr_pointer+4);
-    }
-#endif //PRUSA_M28
-    else if (code_seen_P(PSTR("fv"))) { // PRUSA fv
-        // get file version
-#ifdef SDSUPPORT
-        card.openFileReadFilteredGcode(strchr_pointer + 3, true);
-        while (true) {
-            uint16_t readByte = card.getFilteredGcodeChar();
-            MYSERIAL.write(readByte);
-            if (readByte == '\n') {
-                break;
-            }
-        }
-        card.closefile();
-#endif // SDSUPPORT
-    }
-    else {
-        return false;
-    }
-
-    return true;
-}
-
-void farm_gcode_g98() {
-    farm_mode = 1;
-    eeprom_update_byte_notify((unsigned char *)EEPROM_FARM_MODE, farm_mode);
-    SilentModeMenu = SILENT_MODE_OFF;
-    eeprom_update_byte_notify((unsigned char *)EEPROM_SILENT, SilentModeMenu);
-    fCheckModeInit(); // alternatively invoke printer reset
-}
-
-void farm_gcode_g99() {
-    farm_disable();
-    lcd_update(2);
-    fCheckModeInit(); // alternatively invoke printer reset
-}
-
-void farm_disable() {
-    farm_mode = false;
-    eeprom_update_byte_notify((uint8_t*)EEPROM_FARM_MODE, farm_mode);
-}
-
-#else //PRUSA_FARM
-
-void prusa_statistics(_UNUSED uint8_t message) {
-}
-
-void prusa_statistics_update_from_status_screen() {
-}
-
-void prusa_statistics_update_from_lcd_update() {
-}
-
-void farm_mode_init() {
-}
-
-bool farm_prusa_code_seen() {
-    return false;
-}
-
-void farm_gcode_g98() {
-}
-
-void farm_gcode_g99() {
-}
-
-void farm_disable() {
-}
-
-#endif //PRUSA_FARM
-
--- a/Firmware/Prusa_farm.h
+++ b/Firmware/Prusa_farm.h
@ -1,32 +0,0 @@
-#pragma once
-
-#include <inttypes.h>
-#include "config.h"
-
-#define FARM_PREHEAT_HOTEND_TEMP 250
-#define FARM_PREHEAT_HPB_TEMP 80
-
-#define FARM_DEFAULT_SAFETYTIMER_TIME_ms (45*60*1000ul)
-#define NC_TIME 10 //time in s for periodic important status messages sending which needs reponse from monitoring
-#define NC_BUTTON_LONG_PRESS 15 //time in s
-
-//#define FARM_CONNECT_MESSAGE
-
-#ifdef PRUSA_FARM
-extern uint8_t farm_mode;
-#else
-#define farm_mode 0
-#endif
-
-#ifdef PRUSA_M28
-extern bool prusa_sd_card_upload;
-extern void serial_read_stream();
-#endif
-extern void prusa_statistics(uint8_t _message);
-extern void prusa_statistics_update_from_status_screen();
-extern void prusa_statistics_update_from_lcd_update();
-extern void farm_mode_init();
-extern bool farm_prusa_code_seen();
-extern void farm_gcode_g98();
-extern void farm_gcode_g99();
-extern void farm_disable();
--- a/Firmware/Sd2Card.cpp
+++ b/Firmware/Sd2Card.cpp
@ -205,14 +205,14 @@ uint32_t Sd2Card::cardSize() {
 }
 //------------------------------------------------------------------------------
 void Sd2Card::chipSelectHigh() {
-  WRITE(SDSS, 1);
+  digitalWrite(chipSelectPin_, HIGH);
 }
 //------------------------------------------------------------------------------
 void Sd2Card::chipSelectLow() {
 #ifndef SOFTWARE_SPI
  spiInit(spiRate_);
 #endif  // SOFTWARE_SPI
-  WRITE(SDSS, 0);
+  digitalWrite(chipSelectPin_, LOW);
 }
 //------------------------------------------------------------------------------
 /** Erase a range of blocks.
@ -283,25 +283,26 @@ bool Sd2Card::eraseSingleBlockEnable() {
 * the value zero, false, is returned for failure.  The reason for failure
 * can be determined by calling errorCode() and errorData().
 */
-bool Sd2Card::init(uint8_t sckRateID) {
+bool Sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin) {
  errorCode_ = type_ = 0;
+  chipSelectPin_ = chipSelectPin;
  // 16-bit init start time allows over a minute
  uint16_t t0 = (uint16_t)_millis();
  uint32_t arg;

  // set pin modes
+  pinMode(chipSelectPin_, OUTPUT);
  chipSelectHigh();
-  SET_OUTPUT(SDSS);
-  SET_INPUT(MISO);
-  SET_OUTPUT(MOSI);
-  SET_OUTPUT(SCK);
+  pinMode(SPI_MISO_PIN, INPUT);
+  pinMode(SPI_MOSI_PIN, OUTPUT);
+  pinMode(SPI_SCK_PIN, OUTPUT);

 #ifndef SOFTWARE_SPI
  // SS must be in output mode even it is not chip select
-  SET_OUTPUT(SS);
+  pinMode(SS_PIN, OUTPUT);
  // set SS high - may be chip select for another SPI device
 #if SET_SPI_SS_HIGH
-  WRITE(SS, 1);
+  digitalWrite(SS_PIN, HIGH);
 #endif  // SET_SPI_SS_HIGH
  // set SCK rate for initialization commands
  spiRate_ = SPI_SD_INIT_RATE;
@ -311,16 +312,13 @@ bool Sd2Card::init(uint8_t sckRateID) {
  // must supply min of 74 clock cycles with CS high.
  for (uint8_t i = 0; i < 10; i++) spiSend(0XFF);

-  WRITE(MISO, 1); // temporarily enable the MISO line pullup
  // command to go idle in SPI mode
  while ((status_ = cardCommand(CMD0, 0)) != R1_IDLE_STATE) {
    if (((uint16_t)_millis() - t0) > SD_INIT_TIMEOUT) {
-      WRITE(MISO, 0); // disable the MISO line pullup
      error(SD_CARD_ERROR_CMD0);
      goto fail;
    }
  }
-  WRITE(MISO, 0); // disable the MISO line pullup

  // send 0xFF until 0xFF received to give card some clock cycles
  t0 = (uint16_t)_millis();
@ -767,11 +765,8 @@ uint8_t Sd2Card::waitStartBlock(void) {
  return false;
 }

-// Toshiba FlashAir support, copied from
+// Toshiba FlashAir support, copied from 
 // https://flashair-developers.com/en/documents/tutorials/arduino/
-// However, the official website was closed in September 2019.
-// There is an archived website (written in Japanese).
-// https://flashair-developers.github.io/website/docs/tutorials/arduino/2.html

 //------------------------------------------------------------------------------
 /** Perform Extention Read. */
@ -779,11 +774,11 @@ uint8_t Sd2Card::readExt(uint32_t arg, uint8_t* dst, uint16_t count) {
  uint16_t i;

  // send command and argument.
-  if (cardCommand(CMD48, arg) && cardCommand(CMD17, arg)) { // CMD48 for W-03, CMD17 for W-04
+  if (cardCommand(CMD48, arg)) {
    error(SD_CARD_ERROR_CMD48);
    goto fail;
  }
-
+  
  // wait for start block token.
  if (!waitStartBlock()) {
    goto fail;
@ -793,7 +788,7 @@ uint8_t Sd2Card::readExt(uint32_t arg, uint8_t* dst, uint16_t count) {
  for (i = 0; i < count; ++i) {
    dst[i] = spiRec();
  }
-
+  
  // skip dummy bytes and 16-bit crc.
  for (; i < 514; ++i) {
    spiRec();
@ -815,19 +810,19 @@ uint8_t Sd2Card::readExt(uint32_t arg, uint8_t* dst, uint16_t count) {
 * \return The value one, true, is returned for success and
 * the value zero, false, is returned for failure.
 */
-uint8_t Sd2Card::readExtMemory(uint8_t mio, uint8_t func,
+uint8_t Sd2Card::readExtMemory(uint8_t mio, uint8_t func, 
    uint32_t addr, uint16_t count, uint8_t* dst) {
  uint32_t offset = addr & 0x1FF;
  if (offset + count > 512) count = 512 - offset;
-
+  
  if (count == 0) return true;
-
-  uint32_t arg =
-      (((uint32_t)mio & 0x1) << 31) |
+  
+  uint32_t arg = 
+      (((uint32_t)mio & 0x1) << 31) | 
    (mio ? (((uint32_t)func & 0x7) << 28) : (((uint32_t)func & 0xF) << 27)) |
    ((addr & 0x1FFFF) << 9) |
    ((count - 1) & 0x1FF);
-
+  
  return readExt(arg, dst, count);
 }

--- a/Firmware/Sd2Card.h
+++ b/Firmware/Sd2Card.h
@ -28,6 +28,7 @@
 * \brief Sd2Card class for V2 SD/SDHC cards
 */
 #include "SdFatConfig.h"
+#include "Sd2PinMap.h"
 #include "SdInfo.h"
 //------------------------------------------------------------------------------
 // SPI speed is F_CPU/2^(1 + index), 0 <= index <= 6
@ -132,7 +133,22 @@ uint8_t const SD_CARD_TYPE_SDHC = 3;
 //------------------------------------------------------------------------------
 // SPI pin definitions - do not edit here - change in SdFatConfig.h
 //
-#ifdef SOFTWARE_SPI
+#ifndef SOFTWARE_SPI
+// hardware pin defs
+/** The default chip select pin for the SD card is SS. */
+uint8_t const  SD_CHIP_SELECT_PIN = SS_PIN;
+// The following three pins must not be redefined for hardware SPI.
+/** SPI Master Out Slave In pin */
+uint8_t const  SPI_MOSI_PIN = MOSI_PIN;
+/** SPI Master In Slave Out pin */
+uint8_t const  SPI_MISO_PIN = MISO_PIN;
+/** SPI Clock pin */
+uint8_t const  SPI_SCK_PIN = SCK_PIN;
+
+#else  // SOFTWARE_SPI
+
+/** SPI chip select pin */
+uint8_t const SD_CHIP_SELECT_PIN = SOFT_SPI_CS_PIN;
 /** SPI Master Out Slave In pin */
 uint8_t const SPI_MOSI_PIN = SOFT_SPI_MOSI_PIN;
 /** SPI Master In Slave Out pin */
@ -160,21 +176,22 @@ class Sd2Card {
  /**
   * \return error code for last error. See Sd2Card.h for a list of error codes.
   */
-  uint8_t errorCode() const {return errorCode_;}
+  int errorCode() const {return errorCode_;}
  /** \return error data for last error. */
-  uint8_t errorData() const {return status_;}
+  int errorData() const {return status_;}
  /**
   * Initialize an SD flash memory card with default clock rate and chip
-   * select pin.  See sd2Card::init(uint8_t sckRateID).
+   * select pin.  See sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin).
   *
   * \return true for success or false for failure.
   */
-  bool init(uint8_t sckRateID = SPI_FULL_SPEED);
+  bool init(uint8_t sckRateID = SPI_FULL_SPEED,
+    uint8_t chipSelectPin = SD_CHIP_SELECT_PIN);
  bool readBlock(uint32_t block, uint8_t* dst);
  /**
   * Read a card's CID register. The CID contains card identification
   * information such as Manufacturer ID, Product name, Product serial
-   * number and Manufacturing date.
+   * number and Manufacturing date. 
   *
   * \param[out] cid pointer to area for returned data.
   *
@ -215,6 +232,7 @@ class Sd2Card {

 private:
  //----------------------------------------------------------------------------
+  uint8_t chipSelectPin_;
  uint8_t errorCode_;
  uint8_t spiRate_;
  uint8_t status_;
@ -243,4 +261,4 @@ class Sd2Card {
 #endif  // Sd2Card_h


-#endif
+#endif
--- a/Firmware/Sd2PinMap.h
+++ b/Firmware/Sd2PinMap.h
@ -0,0 +1,368 @@
+/* Arduino SdFat Library
+ * Copyright (C) 2010 by William Greiman
+ *
+ * This file is part of the Arduino SdFat Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This Library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino SdFat Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+// Warning this file was generated by a program.
+#include "Marlin.h"
+#ifdef SDSUPPORT
+
+#ifndef Sd2PinMap_h
+#define Sd2PinMap_h
+#include <avr/io.h>
+//------------------------------------------------------------------------------
+/** struct for mapping digital pins */
+struct pin_map_t {
+  volatile uint8_t* ddr;
+  volatile uint8_t* pin;
+  volatile uint8_t* port;
+  uint8_t bit;
+};
+//------------------------------------------------------------------------------
+#if defined(__AVR_ATmega1280__)\
+|| defined(__AVR_ATmega2560__)
+// Mega
+
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 20;  // D1
+uint8_t const SCL_PIN = 21;  // D0
+
+#undef MOSI_PIN
+#undef MISO_PIN
+// SPI port
+uint8_t const SS_PIN = 53;    // B0
+uint8_t const MOSI_PIN = 51;  // B2
+uint8_t const MISO_PIN = 50;  // B3
+uint8_t const SCK_PIN = 52;   // B1
+
+static const pin_map_t digitalPinMap[] = {
+  {&DDRE, &PINE, &PORTE, 0},  // E0  0
+  {&DDRE, &PINE, &PORTE, 1},  // E1  1
+  {&DDRE, &PINE, &PORTE, 4},  // E4  2
+  {&DDRE, &PINE, &PORTE, 5},  // E5  3
+  {&DDRG, &PING, &PORTG, 5},  // G5  4
+  {&DDRE, &PINE, &PORTE, 3},  // E3  5
+  {&DDRH, &PINH, &PORTH, 3},  // H3  6
+  {&DDRH, &PINH, &PORTH, 4},  // H4  7
+  {&DDRH, &PINH, &PORTH, 5},  // H5  8
+  {&DDRH, &PINH, &PORTH, 6},  // H6  9
+  {&DDRB, &PINB, &PORTB, 4},  // B4 10
+  {&DDRB, &PINB, &PORTB, 5},  // B5 11
+  {&DDRB, &PINB, &PORTB, 6},  // B6 12
+  {&DDRB, &PINB, &PORTB, 7},  // B7 13
+  {&DDRJ, &PINJ, &PORTJ, 1},  // J1 14
+  {&DDRJ, &PINJ, &PORTJ, 0},  // J0 15
+  {&DDRH, &PINH, &PORTH, 1},  // H1 16
+  {&DDRH, &PINH, &PORTH, 0},  // H0 17
+  {&DDRD, &PIND, &PORTD, 3},  // D3 18
+  {&DDRD, &PIND, &PORTD, 2},  // D2 19
+  {&DDRD, &PIND, &PORTD, 1},  // D1 20
+  {&DDRD, &PIND, &PORTD, 0},  // D0 21
+  {&DDRA, &PINA, &PORTA, 0},  // A0 22
+  {&DDRA, &PINA, &PORTA, 1},  // A1 23
+  {&DDRA, &PINA, &PORTA, 2},  // A2 24
+  {&DDRA, &PINA, &PORTA, 3},  // A3 25
+  {&DDRA, &PINA, &PORTA, 4},  // A4 26
+  {&DDRA, &PINA, &PORTA, 5},  // A5 27
+  {&DDRA, &PINA, &PORTA, 6},  // A6 28
+  {&DDRA, &PINA, &PORTA, 7},  // A7 29
+  {&DDRC, &PINC, &PORTC, 7},  // C7 30
+  {&DDRC, &PINC, &PORTC, 6},  // C6 31
+  {&DDRC, &PINC, &PORTC, 5},  // C5 32
+  {&DDRC, &PINC, &PORTC, 4},  // C4 33
+  {&DDRC, &PINC, &PORTC, 3},  // C3 34
+  {&DDRC, &PINC, &PORTC, 2},  // C2 35
+  {&DDRC, &PINC, &PORTC, 1},  // C1 36
+  {&DDRC, &PINC, &PORTC, 0},  // C0 37
+  {&DDRD, &PIND, &PORTD, 7},  // D7 38
+  {&DDRG, &PING, &PORTG, 2},  // G2 39
+  {&DDRG, &PING, &PORTG, 1},  // G1 40
+  {&DDRG, &PING, &PORTG, 0},  // G0 41
+  {&DDRL, &PINL, &PORTL, 7},  // L7 42
+  {&DDRL, &PINL, &PORTL, 6},  // L6 43
+  {&DDRL, &PINL, &PORTL, 5},  // L5 44
+  {&DDRL, &PINL, &PORTL, 4},  // L4 45
+  {&DDRL, &PINL, &PORTL, 3},  // L3 46
+  {&DDRL, &PINL, &PORTL, 2},  // L2 47
+  {&DDRL, &PINL, &PORTL, 1},  // L1 48
+  {&DDRL, &PINL, &PORTL, 0},  // L0 49
+  {&DDRB, &PINB, &PORTB, 3},  // B3 50
+  {&DDRB, &PINB, &PORTB, 2},  // B2 51
+  {&DDRB, &PINB, &PORTB, 1},  // B1 52
+  {&DDRB, &PINB, &PORTB, 0},  // B0 53
+  {&DDRF, &PINF, &PORTF, 0},  // F0 54
+  {&DDRF, &PINF, &PORTF, 1},  // F1 55
+  {&DDRF, &PINF, &PORTF, 2},  // F2 56
+  {&DDRF, &PINF, &PORTF, 3},  // F3 57
+  {&DDRF, &PINF, &PORTF, 4},  // F4 58
+  {&DDRF, &PINF, &PORTF, 5},  // F5 59
+  {&DDRF, &PINF, &PORTF, 6},  // F6 60
+  {&DDRF, &PINF, &PORTF, 7},  // F7 61
+  {&DDRK, &PINK, &PORTK, 0},  // K0 62
+  {&DDRK, &PINK, &PORTK, 1},  // K1 63
+  {&DDRK, &PINK, &PORTK, 2},  // K2 64
+  {&DDRK, &PINK, &PORTK, 3},  // K3 65
+  {&DDRK, &PINK, &PORTK, 4},  // K4 66
+  {&DDRK, &PINK, &PORTK, 5},  // K5 67
+  {&DDRK, &PINK, &PORTK, 6},  // K6 68
+  {&DDRK, &PINK, &PORTK, 7}   // K7 69
+};
+//------------------------------------------------------------------------------
+#elif defined(__AVR_ATmega644P__)\
+|| defined(__AVR_ATmega644__)\
+|| defined(__AVR_ATmega1284P__)
+// Sanguino
+
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 17;  // C1
+uint8_t const SCL_PIN = 18;  // C2
+
+// SPI port
+uint8_t const SS_PIN = 4;    // B4
+uint8_t const MOSI_PIN = 5;  // B5
+uint8_t const MISO_PIN = 6;  // B6
+uint8_t const SCK_PIN = 7;   // B7
+
+static const pin_map_t digitalPinMap[] = {
+  {&DDRB, &PINB, &PORTB, 0},  // B0  0
+  {&DDRB, &PINB, &PORTB, 1},  // B1  1
+  {&DDRB, &PINB, &PORTB, 2},  // B2  2
+  {&DDRB, &PINB, &PORTB, 3},  // B3  3
+  {&DDRB, &PINB, &PORTB, 4},  // B4  4
+  {&DDRB, &PINB, &PORTB, 5},  // B5  5
+  {&DDRB, &PINB, &PORTB, 6},  // B6  6
+  {&DDRB, &PINB, &PORTB, 7},  // B7  7
+  {&DDRD, &PIND, &PORTD, 0},  // D0  8
+  {&DDRD, &PIND, &PORTD, 1},  // D1  9
+  {&DDRD, &PIND, &PORTD, 2},  // D2 10
+  {&DDRD, &PIND, &PORTD, 3},  // D3 11
+  {&DDRD, &PIND, &PORTD, 4},  // D4 12
+  {&DDRD, &PIND, &PORTD, 5},  // D5 13
+  {&DDRD, &PIND, &PORTD, 6},  // D6 14
+  {&DDRD, &PIND, &PORTD, 7},  // D7 15
+  {&DDRC, &PINC, &PORTC, 0},  // C0 16
+  {&DDRC, &PINC, &PORTC, 1},  // C1 17
+  {&DDRC, &PINC, &PORTC, 2},  // C2 18
+  {&DDRC, &PINC, &PORTC, 3},  // C3 19
+  {&DDRC, &PINC, &PORTC, 4},  // C4 20
+  {&DDRC, &PINC, &PORTC, 5},  // C5 21
+  {&DDRC, &PINC, &PORTC, 6},  // C6 22
+  {&DDRC, &PINC, &PORTC, 7},  // C7 23
+  {&DDRA, &PINA, &PORTA, 7},  // A7 24
+  {&DDRA, &PINA, &PORTA, 6},  // A6 25
+  {&DDRA, &PINA, &PORTA, 5},  // A5 26
+  {&DDRA, &PINA, &PORTA, 4},  // A4 27
+  {&DDRA, &PINA, &PORTA, 3},  // A3 28
+  {&DDRA, &PINA, &PORTA, 2},  // A2 29
+  {&DDRA, &PINA, &PORTA, 1},  // A1 30
+  {&DDRA, &PINA, &PORTA, 0}   // A0 31
+};
+//------------------------------------------------------------------------------
+#elif defined(__AVR_ATmega32U4__)
+// Teensy 2.0
+
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 6;  // D1
+uint8_t const SCL_PIN = 5;  // D0
+
+// SPI port
+uint8_t const SS_PIN = 0;    // B0
+uint8_t const MOSI_PIN = 2;  // B2
+uint8_t const MISO_PIN = 3;  // B3
+uint8_t const SCK_PIN = 1;   // B1
+
+static const pin_map_t digitalPinMap[] = {
+  {&DDRB, &PINB, &PORTB, 0},  // B0  0
+  {&DDRB, &PINB, &PORTB, 1},  // B1  1
+  {&DDRB, &PINB, &PORTB, 2},  // B2  2
+  {&DDRB, &PINB, &PORTB, 3},  // B3  3
+  {&DDRB, &PINB, &PORTB, 7},  // B7  4
+  {&DDRD, &PIND, &PORTD, 0},  // D0  5
+  {&DDRD, &PIND, &PORTD, 1},  // D1  6
+  {&DDRD, &PIND, &PORTD, 2},  // D2  7
+  {&DDRD, &PIND, &PORTD, 3},  // D3  8
+  {&DDRC, &PINC, &PORTC, 6},  // C6  9
+  {&DDRC, &PINC, &PORTC, 7},  // C7 10
+  {&DDRD, &PIND, &PORTD, 6},  // D6 11
+  {&DDRD, &PIND, &PORTD, 7},  // D7 12
+  {&DDRB, &PINB, &PORTB, 4},  // B4 13
+  {&DDRB, &PINB, &PORTB, 5},  // B5 14
+  {&DDRB, &PINB, &PORTB, 6},  // B6 15
+  {&DDRF, &PINF, &PORTF, 7},  // F7 16
+  {&DDRF, &PINF, &PORTF, 6},  // F6 17
+  {&DDRF, &PINF, &PORTF, 5},  // F5 18
+  {&DDRF, &PINF, &PORTF, 4},  // F4 19
+  {&DDRF, &PINF, &PORTF, 1},  // F1 20
+  {&DDRF, &PINF, &PORTF, 0},  // F0 21
+  {&DDRD, &PIND, &PORTD, 4},  // D4 22
+  {&DDRD, &PIND, &PORTD, 5},  // D5 23
+  {&DDRE, &PINE, &PORTE, 6}   // E6 24
+};
+//------------------------------------------------------------------------------
+#elif defined(__AVR_AT90USB646__)\
+|| defined(__AVR_AT90USB1286__)
+// Teensy++ 1.0 & 2.0
+
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 1;  // D1
+uint8_t const SCL_PIN = 0;  // D0
+
+// SPI port
+uint8_t const SS_PIN    = 20;    // B0
+uint8_t const MOSI_PIN  = 22;    // B2
+uint8_t const MISO_PIN  = 23;    // B3
+uint8_t const SCK_PIN   = 21;    // B1
+
+static const pin_map_t digitalPinMap[] = {
+  {&DDRD, &PIND, &PORTD, 0},  // D0  0
+  {&DDRD, &PIND, &PORTD, 1},  // D1  1
+  {&DDRD, &PIND, &PORTD, 2},  // D2  2
+  {&DDRD, &PIND, &PORTD, 3},  // D3  3
+  {&DDRD, &PIND, &PORTD, 4},  // D4  4
+  {&DDRD, &PIND, &PORTD, 5},  // D5  5
+  {&DDRD, &PIND, &PORTD, 6},  // D6  6
+  {&DDRD, &PIND, &PORTD, 7},  // D7  7
+  {&DDRE, &PINE, &PORTE, 0},  // E0  8
+  {&DDRE, &PINE, &PORTE, 1},  // E1  9
+  {&DDRC, &PINC, &PORTC, 0},  // C0 10
+  {&DDRC, &PINC, &PORTC, 1},  // C1 11
+  {&DDRC, &PINC, &PORTC, 2},  // C2 12
+  {&DDRC, &PINC, &PORTC, 3},  // C3 13
+  {&DDRC, &PINC, &PORTC, 4},  // C4 14
+  {&DDRC, &PINC, &PORTC, 5},  // C5 15
+  {&DDRC, &PINC, &PORTC, 6},  // C6 16
+  {&DDRC, &PINC, &PORTC, 7},  // C7 17
+  {&DDRE, &PINE, &PORTE, 6},  // E6 18
+  {&DDRE, &PINE, &PORTE, 7},  // E7 19
+  {&DDRB, &PINB, &PORTB, 0},  // B0 20
+  {&DDRB, &PINB, &PORTB, 1},  // B1 21
+  {&DDRB, &PINB, &PORTB, 2},  // B2 22
+  {&DDRB, &PINB, &PORTB, 3},  // B3 23
+  {&DDRB, &PINB, &PORTB, 4},  // B4 24
+  {&DDRB, &PINB, &PORTB, 5},  // B5 25
+  {&DDRB, &PINB, &PORTB, 6},  // B6 26
+  {&DDRB, &PINB, &PORTB, 7},  // B7 27
+  {&DDRA, &PINA, &PORTA, 0},  // A0 28
+  {&DDRA, &PINA, &PORTA, 1},  // A1 29
+  {&DDRA, &PINA, &PORTA, 2},  // A2 30
+  {&DDRA, &PINA, &PORTA, 3},  // A3 31
+  {&DDRA, &PINA, &PORTA, 4},  // A4 32
+  {&DDRA, &PINA, &PORTA, 5},  // A5 33
+  {&DDRA, &PINA, &PORTA, 6},  // A6 34
+  {&DDRA, &PINA, &PORTA, 7},  // A7 35
+  {&DDRE, &PINE, &PORTE, 4},  // E4 36
+  {&DDRE, &PINE, &PORTE, 5},  // E5 37
+  {&DDRF, &PINF, &PORTF, 0},  // F0 38
+  {&DDRF, &PINF, &PORTF, 1},  // F1 39
+  {&DDRF, &PINF, &PORTF, 2},  // F2 40
+  {&DDRF, &PINF, &PORTF, 3},  // F3 41
+  {&DDRF, &PINF, &PORTF, 4},  // F4 42
+  {&DDRF, &PINF, &PORTF, 5},  // F5 43
+  {&DDRF, &PINF, &PORTF, 6},  // F6 44
+  {&DDRF, &PINF, &PORTF, 7}   // F7 45
+};
+//------------------------------------------------------------------------------
+#elif defined(__AVR_ATmega168__)\
+||defined(__AVR_ATmega168P__)\
+||defined(__AVR_ATmega328P__)
+// 168 and 328 Arduinos
+
+// Two Wire (aka I2C) ports
+uint8_t const SDA_PIN = 18;  // C4
+uint8_t const SCL_PIN = 19;  // C5
+
+// SPI port
+uint8_t const SS_PIN = 10;    // B2
+uint8_t const MOSI_PIN = 11;  // B3
+uint8_t const MISO_PIN = 12;  // B4
+uint8_t const SCK_PIN = 13;   // B5
+
+static const pin_map_t digitalPinMap[] = {
+  {&DDRD, &PIND, &PORTD, 0},  // D0  0
+  {&DDRD, &PIND, &PORTD, 1},  // D1  1
+  {&DDRD, &PIND, &PORTD, 2},  // D2  2
+  {&DDRD, &PIND, &PORTD, 3},  // D3  3
+  {&DDRD, &PIND, &PORTD, 4},  // D4  4
+  {&DDRD, &PIND, &PORTD, 5},  // D5  5
+  {&DDRD, &PIND, &PORTD, 6},  // D6  6
+  {&DDRD, &PIND, &PORTD, 7},  // D7  7
+  {&DDRB, &PINB, &PORTB, 0},  // B0  8
+  {&DDRB, &PINB, &PORTB, 1},  // B1  9
+  {&DDRB, &PINB, &PORTB, 2},  // B2 10
+  {&DDRB, &PINB, &PORTB, 3},  // B3 11
+  {&DDRB, &PINB, &PORTB, 4},  // B4 12
+  {&DDRB, &PINB, &PORTB, 5},  // B5 13
+  {&DDRC, &PINC, &PORTC, 0},  // C0 14
+  {&DDRC, &PINC, &PORTC, 1},  // C1 15
+  {&DDRC, &PINC, &PORTC, 2},  // C2 16
+  {&DDRC, &PINC, &PORTC, 3},  // C3 17
+  {&DDRC, &PINC, &PORTC, 4},  // C4 18
+  {&DDRC, &PINC, &PORTC, 5}   // C5 19
+};
+#else  // defined(__AVR_ATmega1280__)
+#error unknown chip
+#endif  // defined(__AVR_ATmega1280__)
+//------------------------------------------------------------------------------
+static const uint8_t digitalPinCount = sizeof(digitalPinMap)/sizeof(pin_map_t);
+
+uint8_t badPinNumber(void)
+  __attribute__((error("Pin number is too large or not a constant")));
+
+static inline __attribute__((always_inline))
+  bool getPinMode(uint8_t pin) {
+  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
+    return (*digitalPinMap[pin].ddr >> digitalPinMap[pin].bit) & 1;
+  } else {
+    return badPinNumber();
+  }
+}
+static inline __attribute__((always_inline))
+  void setPinMode(uint8_t pin, uint8_t mode) {
+  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
+    if (mode) {
+      *digitalPinMap[pin].ddr |= 1 << digitalPinMap[pin].bit;
+    } else {
+      *digitalPinMap[pin].ddr &= ~(1 << digitalPinMap[pin].bit);
+    }
+  } else {
+    badPinNumber();
+  }
+}
+static inline __attribute__((always_inline))
+  bool fastDigitalRead(uint8_t pin) {
+  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
+    return (*digitalPinMap[pin].pin >> digitalPinMap[pin].bit) & 1;
+  } else {
+    return badPinNumber();
+  }
+}
+static inline __attribute__((always_inline))
+  void fastDigitalWrite(uint8_t pin, uint8_t value) {
+  if (__builtin_constant_p(pin) && pin < digitalPinCount) {
+    if (value) {
+      *digitalPinMap[pin].port |= 1 << digitalPinMap[pin].bit;
+    } else {
+      *digitalPinMap[pin].port &= ~(1 << digitalPinMap[pin].bit);
+    }
+  } else {
+    badPinNumber();
+  }
+}
+#endif  // Sd2PinMap_h
+
+
+#endif
--- a/Firmware/SdBaseFile.cpp
+++ b/Firmware/SdBaseFile.cpp
@ -530,9 +530,9 @@ bool SdBaseFile::mkdir(SdBaseFile* parent, const uint8_t dname[11]) {
  * \return The value one, true, is returned for success and
  * the value zero, false, is returned for failure.
  */
-bool SdBaseFile::open(const char* path, uint8_t oflag) {
-  return open(cwd_, path, oflag);
-}
+  bool SdBaseFile::open(const char* path, uint8_t oflag) {
+    return open(cwd_, path, oflag);
+  }
 //------------------------------------------------------------------------------
 /** Open a file or directory by name.
 *
@ -1015,7 +1015,7 @@ void SdBaseFile::printFatTime( uint16_t fatTime) {
 * the value zero, false, is returned for failure.
 */
 bool SdBaseFile::printName() {
-  char name[FILENAME_LENGTH];
+  char name[13];
  if (!getFilename(name)) return false;
  MYSERIAL.print(name);
  return true;
@ -1115,7 +1115,7 @@ int8_t SdBaseFile::readDir(dir_t* dir, char* longFilename) {
  int16_t n;
  // if not a directory file or miss-positioned return an error
  if (!isDir() || (0X1F & curPosition_)) return -1;
-
+  
  //If we have a longFilename buffer, mark it as invalid. If we find a long filename it will be filled automaticly.
  if (longFilename != NULL)
  {
--- a/Firmware/SdBaseFile.h
+++ b/Firmware/SdBaseFile.h
@ -281,10 +281,8 @@ class SdBaseFile {
  static void printFatDate(uint16_t fatDate);
  static void printFatTime( uint16_t fatTime);
  bool printName();
-protected:
  int16_t read();
  int16_t read(void* buf, uint16_t nbyte);
-public:
  int8_t readDir(dir_t* dir, char* longFilename);
  static bool remove(SdBaseFile* dirFile, const char* path);
  bool remove();
@ -323,7 +321,7 @@ public:
  SdVolume* volume() const {return vol_;}
  int16_t write(const void* buf, uint16_t nbyte);
 //------------------------------------------------------------------------------
- protected:
+ private:
  // allow SdFat to set cwd_
  friend class SdFat;
  // global pointer to cwd dir
--- a/Firmware/SdFatStructs.h
+++ b/Firmware/SdFatStructs.h
@ -468,29 +468,29 @@ uint32_t const FAT32MASK = 0X0FFFFFFF;
 * \brief FAT short directory entry
 *
 * Short means short 8.3 name, not the entry size.
- *
- * Date Format. A FAT directory entry date stamp is a 16-bit field that is
+ *  
+ * Date Format. A FAT directory entry date stamp is a 16-bit field that is 
 * basically a date relative to the MS-DOS epoch of 01/01/1980. Here is the
- * format (bit 0 is the LSB of the 16-bit word, bit 15 is the MSB of the
+ * format (bit 0 is the LSB of the 16-bit word, bit 15 is the MSB of the 
 * 16-bit word):
- *
- * Bits 9-15: Count of years from 1980, valid value range 0-127
+ *   
+ * Bits 9-15: Count of years from 1980, valid value range 0-127 
 * inclusive (1980-2107).
- *
+ *   
 * Bits 5-8: Month of year, 1 = January, valid value range 1-12 inclusive.
 *
 * Bits 0-4: Day of month, valid value range 1-31 inclusive.
 *
 * Time Format. A FAT directory entry time stamp is a 16-bit field that has
- * a granularity of 2 seconds. Here is the format (bit 0 is the LSB of the
+ * a granularity of 2 seconds. Here is the format (bit 0 is the LSB of the 
 * 16-bit word, bit 15 is the MSB of the 16-bit word).
- *
+ *   
 * Bits 11-15: Hours, valid value range 0-23 inclusive.
- *
+ * 
 * Bits 5-10: Minutes, valid value range 0-59 inclusive.
- *
+ *      
 * Bits 0-4: 2-second count, valid value range 0-29 inclusive (0 - 58 seconds).
- *
+ *   
 * The valid time range is from Midnight 00:00:00 to 23:59:58.
 */
 struct directoryEntry {
@ -548,7 +548,7 @@ struct directoryEntry {
 *
 * directoryVFATEntries are found in the same list as normal directoryEntry.
 * But have the attribute field set to DIR_ATT_LONG_NAME.
- *
+ * 
 * Long filenames are saved in multiple directoryVFATEntries.
 * Each entry containing 13 UTF-16 characters.
 */
--- a/Firmware/SdFatUtil.cpp
+++ b/Firmware/SdFatUtil.cpp
@ -45,19 +45,27 @@ int SdFatUtil::FreeRam() {
 #endif  // __arm

 void SdFatUtil::set_stack_guard()
-{
+{	
 	uint32_t *stack_guard;

-	stack_guard = (uint32_t*)(&__bss_end + STACK_GUARD_MARGIN);
+	stack_guard = (uint32_t*)&__bss_end;
    *stack_guard = STACK_GUARD_TEST_VALUE;
 }

 bool SdFatUtil::test_stack_integrity()
 {
-	uint32_t* stack_guard = (uint32_t*)(&__bss_end + STACK_GUARD_MARGIN);
+	uint32_t* stack_guard = (uint32_t*)&__bss_end;
 	return (*stack_guard == STACK_GUARD_TEST_VALUE);
 }

+uint32_t SdFatUtil::get_stack_guard_test_value()
+{
+	uint32_t* stack_guard;
+	uint32_t output;
+	stack_guard = (uint32_t*)&__bss_end;
+	output = *stack_guard;
+	return(output);
+}
 //------------------------------------------------------------------------------
 /** %Print a string in flash memory.
 *
--- a/Firmware/SdFatUtil.h
+++ b/Firmware/SdFatUtil.h
@ -41,10 +41,11 @@ namespace SdFatUtil {
  void SerialPrintln_P(PGM_P str);
  void set_stack_guard();
  bool test_stack_integrity();
+  uint32_t get_stack_guard_test_value();
 }

 using namespace SdFatUtil;  // NOLINT
 #endif  // #define SdFatUtil_h


-#endif
+#endif
--- a/Firmware/SdFile.cpp
+++ b/Firmware/SdFile.cpp
@ -30,194 +30,6 @@
 */
 SdFile::SdFile(const char* path, uint8_t oflag) : SdBaseFile(path, oflag) {
 }
-
-bool SdFile::openFilteredGcode(SdBaseFile* dirFile, const char* path){
-    if( open(dirFile, path, O_READ) ){
-        // compute the block to start with
-        if( ! gfComputeNextFileBlock() )
-            return false;
-        gfReset();
-        return true;
-    } else {
-        return false;
-    }
-}
-
-bool SdFile::seekSetFilteredGcode(uint32_t pos){
-    if(! seekSet(pos) )return false;
-    if(! gfComputeNextFileBlock() )return false;
-    gfReset();
-    return true;
-}
-
-const uint8_t *SdFile::gfBlockBuffBegin() const {
-    return vol_->cache()->data; // this is constant for the whole time, so it should be fast and sleek
-}
-
-void SdFile::gfReset(){
-    // reset cache read ptr to its begin
-    gfReadPtr = gfBlockBuffBegin() + gfOffset;
-}
-
-// think twice before allowing this to inline - manipulating 4B longs is costly
-// moreover - this function has its parameters in registers only, so no heavy stack usage besides the call/ret
-void __attribute__((noinline)) SdFile::gfUpdateCurrentPosition(uint16_t inc){
-    curPosition_ += inc;
-}
-
-#define find_endl(resultP, startP) \
-__asm__ __volatile__ (  \
-"cycle:          \n" \
-"ld  r22, Z+     \n" \
-"cpi r22, 0x0A   \n" \
-"brne cycle      \n" \
-: "=z" (resultP) /* result of the ASM code - in our case the Z register (R30:R31) */ \
-: "z" (startP)   /* input of the ASM code - in our case the Z register as well (R30:R31) */ \
-: "r22"          /* modifying register R22 - so that the compiler knows */ \
-)
-
-// avoid calling the default heavy-weight read() for just one byte
-int16_t SdFile::readFilteredGcode(){
-    if( ! gfEnsureBlock() ){
-        goto eof_or_fail; // this is unfortunate :( ... other calls are using the cache and we can loose the data block of our gcode file
-    }
-    // assume, we have the 512B block cache filled and terminated with a '\n'
-    {
-    const uint8_t *start = gfReadPtr;
-
-    // It may seem unreasonable to copy the variable into a local one and copy it back at the end of this method,
-    // but there is an important point of view: the compiler is unsure whether it can optimize the reads/writes
-    // to gfReadPtr within this method, because it is a class member variable.
-    // The compiler cannot see, if omitting read/write won't have any incorrect side-effects to the rest of the whole FW.
-    // So this trick explicitly states, that rdPtr is a local variable limited to the scope of this method,
-    // therefore the compiler can omit read/write to it (keep it in registers!) as it sees fit.
-    // And it does! Codesize dropped by 68B!
-    const uint8_t *rdPtr = gfReadPtr;
-
-    // the same applies to gfXBegin, codesize dropped another 100B!
-    const uint8_t *blockBuffBegin = gfBlockBuffBegin();
-
-    uint8_t consecutiveCommentLines = 0;
-    while( *rdPtr == ';' ){
-        for(;;){
-
-            //while( *(++gfReadPtr) != '\n' ); // skip until a newline is found - suboptimal code!
-            // Wondering, why this "nice while cycle" is done in such a weird way using a separate find_endl() function?
-            // Have a look at the ASM code GCC produced!
-
-            // At first - a separate find_endl() makes the compiler understand,
-            // that I don't need to store gfReadPtr every time, I'm only interested in the final address where the '\n' was found
-            // - the cycle can run on CPU registers only without touching memory besides reading the character being compared.
-            // Not only makes the code run considerably faster, but is also 40B shorter!
-            // This was the generated code:
-            //FORCE_INLINE const uint8_t * find_endl(const uint8_t *p){
-            //   while( *(++p) != '\n' ); // skip until a newline is found
-            //   return p; }
-            //   11c5e:	movw	r30, r18
-            //   11c60:	subi	r18, 0xFF	; 255
-            //   11c62:	sbci	r19, 0xFF	; 255
-            //   11c64:	ld	r22, Z
-            //   11c66:	cpi	r22, 0x0A	; 10
-            //   11c68:	brne	.-12     	; 0x11c5e <get_command()+0x524>
-
-            // Still, even that was suboptimal as the compiler seems not to understand the usage of ld r22, Z+ (the plus is important)
-            // aka automatic increment of the Z register (R30:R31 pair)
-            // There is no other way than pure ASM!
-            find_endl(rdPtr, rdPtr);
-
-            // found a newline, prepare the next block if block cache end reached
-            if( rdPtr - blockBuffBegin > 512 ){
-                // at the end of block cache, fill new data in
-                gfUpdateCurrentPosition( rdPtr - start - 1 );
-                if( ! gfComputeNextFileBlock() )goto eof_or_fail;
-                if( ! gfEnsureBlock() )goto eof_or_fail; // fetch it into RAM
-                rdPtr = start = blockBuffBegin;
-            } else {
-                if(consecutiveCommentLines >= 250){
-                    --rdPtr; // unget the already consumed newline
-                    goto emit_char;
-                }
-                // peek the next byte - we are inside the block at least at 511th index - still safe
-                if( *rdPtr == ';' ){
-                    // consecutive comment
-                    ++consecutiveCommentLines;
-                } else {
-                    --rdPtr; // unget the already consumed newline
-                    goto emit_char;
-                }
-                break; // found the real end of the line even across many blocks
-            }
-        }
-    }
-emit_char:
-    {
-        gfUpdateCurrentPosition( rdPtr - start + 1 );
-        int16_t rv = *rdPtr++;
-
-        if( curPosition_ >= fileSize_ ){
-            // past the end of file
-            goto eof_or_fail;
-        } else if( rdPtr - blockBuffBegin >= 512 ){
-            // past the end of current bufferred block - prepare the next one...
-            if( ! gfComputeNextFileBlock() )goto eof_or_fail;
-            // don't need to force fetch the block here, it will get loaded on the next call
-            rdPtr = blockBuffBegin;
-        }
-
-        // save the current read ptr for the next run
-        gfReadPtr = rdPtr;
-        return rv;
-    }
-
-}
-
-eof_or_fail:
-    // make the rdptr point to a safe location - end of file
-    gfReadPtr = gfBlockBuffBegin() + 512;
-    return -1;
-}
-
-bool SdFile::gfEnsureBlock(){
-    // this comparison is heavy-weight, especially when there is another one inside cacheRawBlock
-    // but it is necessary to avoid computing of terminateOfs if not needed
-    if( gfBlock != vol_->cacheBlockNumber_ ){
-        if ( ! vol_->cacheRawBlock(gfBlock, SdVolume::CACHE_FOR_READ)){
-            return false;
-        }
-        // terminate with a '\n'
-        const uint32_t terminateOfs = fileSize_ - gfOffset;
-        vol_->cache()->data[ terminateOfs < 512 ? terminateOfs : 512 ] = '\n';
-    }
-    return true;
-}
-
-bool SdFile::gfComputeNextFileBlock() {
-    // error if not open or write only
-    if (!isOpen() || !(flags_ & O_READ)) return false;
-
-    gfOffset = curPosition_ & 0X1FF;  // offset in block
-    if (type_ == FAT_FILE_TYPE_ROOT_FIXED) {
-        // SHR by 9 means skip the last byte and shift just 3 bytes by 1
-        // -> should be 8 instructions... and not the horrible loop shifting 4 bytes at once
-        // still need to get some work on this
-        gfBlock = vol_->rootDirStart() + (curPosition_ >> 9);
-    } else {
-        uint8_t blockOfCluster = vol_->blockOfCluster(curPosition_);
-        if (gfOffset == 0 && blockOfCluster == 0) {
-            // start of new cluster
-            if (curPosition_ == 0) {
-                // use first cluster in file
-                curCluster_ = firstCluster_;
-            } else {
-                // get next cluster from FAT
-                if (!vol_->fatGet(curCluster_, &curCluster_)) return false;
-            }
-        }
-        gfBlock = vol_->clusterStartBlock(curCluster_) + blockOfCluster;
-    }
-    return true;
-}
-
 //------------------------------------------------------------------------------
 /** Write data to an open file.
 *
--- a/Firmware/SdFile.h
+++ b/Firmware/SdFile.h
@ -34,24 +34,7 @@
 * \brief SdBaseFile with Print.
 */
 class SdFile : public SdBaseFile/*, public Print*/ {
-  // GCode filtering vars and methods - due to optimization reasons not wrapped in a separate class
-
-  // beware - this read ptr is manipulated inside just 2 methods - readFilteredGcode and gfReset
-  // If you even want to call gfReset from readFilteredGcode, you must make sure
-  // to update gfReadPtr inside readFilteredGcode from a local copy (see explanation of this trick in readFilteredGcode)
-  const uint8_t *gfReadPtr;
-
-  uint32_t gfBlock; // remember the current file block to be kept in cache - due to reuse of the memory, the block may fall out a must be read back
-  uint16_t gfOffset;
-
-  const uint8_t *gfBlockBuffBegin()const;
-
-  void gfReset();
-
-  bool gfEnsureBlock();
-  bool gfComputeNextFileBlock();
-  void gfUpdateCurrentPosition(uint16_t inc);
-public:
+ public:
  SdFile() {}
  SdFile(const char* name, uint8_t oflag);
  #if ARDUINO >= 100
@ -59,10 +42,7 @@ public:
  #else
   void write(uint8_t b);
  #endif
-
-  bool openFilteredGcode(SdBaseFile* dirFile, const char* path);
-  int16_t readFilteredGcode();
-  bool seekSetFilteredGcode(uint32_t pos);
+  
  int16_t write(const void* buf, uint16_t nbyte);
  void write(const char* str);
  void write_P(PGM_P str);
@ -71,4 +51,4 @@ public:
 #endif  // SdFile_h


-#endif
+#endif
--- a/Firmware/SdVolume.h
+++ b/Firmware/SdVolume.h
@ -36,7 +36,7 @@
 */
 union cache_t {
           /** Used to access cached file data blocks. */
-  uint8_t  data[512 + 1]; // abuse the last byte for saving '\n' - ugly optimization of read_filtered's inner skipping loop
+  uint8_t  data[512];
           /** Used to access cached FAT16 entries. */
  uint16_t fat16[256];
           /** Used to access cached FAT32 entries. */
@ -119,7 +119,6 @@ class SdVolume {
  bool dbgFat(uint32_t n, uint32_t* v) {return fatGet(n, v);}
 //------------------------------------------------------------------------------
 private:
-  friend class SdFile;
  // Allow SdBaseFile access to SdVolume private data.
  friend class SdBaseFile;

@ -212,4 +211,4 @@ class SdVolume {
 #endif  // ALLOW_DEPRECATED_FUNCTIONS
 };
 #endif  // SdVolume
-#endif
+#endif
--- a/Firmware/Servo.cpp
+++ b/Firmware/Servo.cpp
@ -0,0 +1,344 @@
+/*
+ Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+ Copyright (c) 2009 Michael Margolis.  All right reserved.
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with this library; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+/*
+
+ A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
+ The servos are pulsed in the background using the value most recently written using the write() method
+
+ Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
+ Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
+
+ The methods are:
+
+ Servo - Class for manipulating servo motors connected to Arduino pins.
+
+ attach(pin )  - Attaches a servo motor to an i/o pin.
+ attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
+ default min is 544, max is 2400
+
+ write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
+ writeMicroseconds() - Sets the servo pulse width in microseconds
+ read()      - Gets the last written servo pulse width as an angle between 0 and 180.
+ readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
+ attached()  - Returns true if there is a servo attached.
+ detach()    - Stops an attached servos from pulsing its i/o pin.
+
+*/
+#include "Configuration.h" 
+
+#ifdef NUM_SERVOS
+#include <avr/interrupt.h>
+#include <Arduino.h>
+
+#include "Servo.h"
+
+#define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds to tick (assumes prescale of 8)  // 12 Aug 2009
+#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
+
+
+#define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
+
+//#define NBR_TIMERS        (MAX_SERVOS / SERVOS_PER_TIMER)
+
+static servo_t servos[MAX_SERVOS];                          // static array of servo structures
+static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
+
+uint8_t ServoCount = 0;                                     // the total number of attached servos
+
+
+// convenience macros
+#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
+#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this timer
+#define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel
+#define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class by timer and channel
+
+#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
+#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo
+
+/************ static functions common to all instances ***********************/
+
+static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
+{
+  if( Channel[timer] < 0 )
+    *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
+  else{
+    if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
+      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
+  }
+
+  Channel[timer]++;    // increment to the next channel
+  if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
+    *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
+    if(SERVO(timer,Channel[timer]).Pin.isActive == true)     // check if activated
+      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
+  }
+  else {
+    // finished all channels so wait for the refresh period to expire before starting over
+    if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) )  // allow a few ticks to ensure the next OCR1A not missed
+      *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
+    else
+      *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has elapsed
+    Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
+  }
+}
+
+#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
+// Interrupt handlers for Arduino
+#if defined(_useTimer1)
+SIGNAL (TIMER1_COMPA_vect)
+{
+  handle_interrupts(_timer1, &TCNT1, &OCR1A);
+}
+#endif
+
+#if defined(_useTimer3)
+SIGNAL (TIMER3_COMPA_vect)
+{
+  handle_interrupts(_timer3, &TCNT3, &OCR3A);
+}
+#endif
+
+#if defined(_useTimer4)
+SIGNAL (TIMER4_COMPA_vect)
+{
+  handle_interrupts(_timer4, &TCNT4, &OCR4A);
+}
+#endif
+
+#if defined(_useTimer5)
+SIGNAL (TIMER5_COMPA_vect)
+{
+  handle_interrupts(_timer5, &TCNT5, &OCR5A);
+}
+#endif
+
+#elif defined WIRING
+// Interrupt handlers for Wiring
+#if defined(_useTimer1)
+void Timer1Service()
+{
+  handle_interrupts(_timer1, &TCNT1, &OCR1A);
+}
+#endif
+#if defined(_useTimer3)
+void Timer3Service()
+{
+  handle_interrupts(_timer3, &TCNT3, &OCR3A);
+}
+#endif
+#endif
+
+
+static void initISR(timer16_Sequence_t timer)
+{
+#if defined (_useTimer1)
+  if(timer == _timer1) {
+    TCCR1A = 0;             // normal counting mode
+    TCCR1B = _BV(CS11);     // set prescaler of 8
+    TCNT1 = 0;              // clear the timer count
+#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
+    TIFR |= _BV(OCF1A);      // clear any pending interrupts;
+    TIMSK |=  _BV(OCIE1A) ;  // enable the output compare interrupt
+#else
+    // here if not ATmega8 or ATmega128
+    TIFR1 |= _BV(OCF1A);     // clear any pending interrupts;
+    TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt
+#endif
+#if defined(WIRING)
+    timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
+#endif
+  }
+#endif
+
+#if defined (_useTimer3)
+  if(timer == _timer3) {
+    TCCR3A = 0;             // normal counting mode
+    TCCR3B = _BV(CS31);     // set prescaler of 8
+    TCNT3 = 0;              // clear the timer count
+#if defined(__AVR_ATmega128__)
+    TIFR |= _BV(OCF3A);     // clear any pending interrupts;
+	ETIMSK |= _BV(OCIE3A);  // enable the output compare interrupt
+#else
+    TIFR3 = _BV(OCF3A);     // clear any pending interrupts;
+    TIMSK3 =  _BV(OCIE3A) ; // enable the output compare interrupt
+#endif
+#if defined(WIRING)
+    timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service);  // for Wiring platform only
+#endif
+  }
+#endif
+
+#if defined (_useTimer4)
+  if(timer == _timer4) {
+    TCCR4A = 0;             // normal counting mode
+    TCCR4B = _BV(CS41);     // set prescaler of 8
+    TCNT4 = 0;              // clear the timer count
+    TIFR4 = _BV(OCF4A);     // clear any pending interrupts;
+    TIMSK4 =  _BV(OCIE4A) ; // enable the output compare interrupt
+  }
+#endif
+
+#if defined (_useTimer5)
+  if(timer == _timer5) {
+    TCCR5A = 0;             // normal counting mode
+    TCCR5B = _BV(CS51);     // set prescaler of 8
+    TCNT5 = 0;              // clear the timer count
+    TIFR5 = _BV(OCF5A);     // clear any pending interrupts;
+    TIMSK5 =  _BV(OCIE5A) ; // enable the output compare interrupt
+  }
+#endif
+}
+
+static void finISR(timer16_Sequence_t timer)
+{
+    //disable use of the given timer
+#if defined WIRING   // Wiring
+  if(timer == _timer1) {
+    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
+    TIMSK1 &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt
+    #else
+    TIMSK &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt
+    #endif
+    timerDetach(TIMER1OUTCOMPAREA_INT);
+  }
+  else if(timer == _timer3) {
+    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
+    TIMSK3 &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
+    #else
+    ETIMSK &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
+    #endif
+    timerDetach(TIMER3OUTCOMPAREA_INT);
+  }
+#else
+    //For arduino - in future: call here to a currently undefined function to reset the timer
+#endif
+}
+
+static boolean isTimerActive(timer16_Sequence_t timer)
+{
+  // returns true if any servo is active on this timer
+  for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
+    if(SERVO(timer,channel).Pin.isActive == true)
+      return true;
+  }
+  return false;
+}
+
+
+/****************** end of static functions ******************************/
+
+Servo::Servo()
+{
+  if( ServoCount < MAX_SERVOS) {
+    this->servoIndex = ServoCount++;                    // assign a servo index to this instance
+	servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values  - 12 Aug 2009
+  }
+  else
+    this->servoIndex = INVALID_SERVO ;  // too many servos
+}
+
+uint8_t Servo::attach(int pin)
+{
+  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
+}
+
+uint8_t Servo::attach(int pin, int min, int max)
+{
+  if(this->servoIndex < MAX_SERVOS ) {
+#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
+    if (pin > 0) this->pin = pin; else pin = this->pin;
+#endif
+    pinMode( pin, OUTPUT) ;                                   // set servo pin to output
+    servos[this->servoIndex].Pin.nbr = pin;
+    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
+    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
+    this->max  = (MAX_PULSE_WIDTH - max)/4;
+    // initialize the timer if it has not already been initialized
+    timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
+    if(isTimerActive(timer) == false)
+      initISR(timer);
+    servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
+  }
+  return this->servoIndex ;
+}
+
+void Servo::detach()
+{
+  servos[this->servoIndex].Pin.isActive = false;
+  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
+  if(isTimerActive(timer) == false) {
+    finISR(timer);
+  }
+}
+
+void Servo::write(int value)
+{
+  if(value < MIN_PULSE_WIDTH)
+  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
+    if(value < 0) value = 0;
+    if(value > 180) value = 180;
+    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());
+  }
+  this->writeMicroseconds(value);
+}
+
+void Servo::writeMicroseconds(int value)
+{
+  // calculate and store the values for the given channel
+  byte channel = this->servoIndex;
+  if( (channel < MAX_SERVOS) )   // ensure channel is valid
+  {
+    if( value < SERVO_MIN() )          // ensure pulse width is valid
+      value = SERVO_MIN();
+    else if( value > SERVO_MAX() )
+      value = SERVO_MAX();
+
+  	value = value - TRIM_DURATION;
+    value = usToTicks(value);  // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
+
+    uint8_t oldSREG = SREG;
+    cli();
+    servos[channel].ticks = value;
+    SREG = oldSREG;
+  }
+}
+
+int Servo::read() // return the value as degrees
+{
+  return  map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
+}
+
+int Servo::readMicroseconds()
+{
+  unsigned int pulsewidth;
+  if( this->servoIndex != INVALID_SERVO )
+    pulsewidth = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION ;   // 12 aug 2009
+  else
+    pulsewidth  = 0;
+
+  return pulsewidth;
+}
+
+bool Servo::attached()
+{
+  return servos[this->servoIndex].Pin.isActive ;
+}
+
+#endif
--- a/Firmware/Servo.h
+++ b/Firmware/Servo.h
@ -0,0 +1,135 @@
+/*
+  Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+  Copyright (c) 2009 Michael Margolis.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+/*
+
+  A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
+  The servos are pulsed in the background using the value most recently written using the write() method
+
+  Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
+  Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
+  The sequence used to seize timers is defined in timers.h
+
+  The methods are:
+
+   Servo - Class for manipulating servo motors connected to Arduino pins.
+
+   attach(pin )  - Attaches a servo motor to an i/o pin.
+   attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
+   default min is 544, max is 2400
+
+   write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
+   writeMicroseconds() - Sets the servo pulse width in microseconds
+   read()      - Gets the last written servo pulse width as an angle between 0 and 180.
+   readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
+   attached()  - Returns true if there is a servo attached.
+   detach()    - Stops an attached servos from pulsing its i/o pin.
+ */
+
+#ifndef Servo_h
+#define Servo_h
+
+#include <inttypes.h>
+
+/*
+ * Defines for 16 bit timers used with  Servo library
+ *
+ * If _useTimerX is defined then TimerX is a 16 bit timer on the current board
+ * timer16_Sequence_t enumerates the sequence that the timers should be allocated
+ * _Nbr_16timers indicates how many 16 bit timers are available.
+ *
+ */
+
+// Say which 16 bit timers can be used and in what order
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+#define _useTimer5
+//#define _useTimer1
+#define _useTimer3
+#define _useTimer4
+//typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ;
+typedef enum { _timer5, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ;
+
+#elif defined(__AVR_ATmega32U4__)
+//#define _useTimer1
+#define _useTimer3
+//typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ;
+typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ;
+
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
+#define _useTimer3
+//#define _useTimer1
+//typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ;
+typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ;
+
+#elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284P__) ||defined(__AVR_ATmega2561__)
+#define _useTimer3
+//#define _useTimer1
+//typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ;
+typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ;
+
+#else  // everything else
+//#define _useTimer1
+//typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ;
+typedef enum { _Nbr_16timers } timer16_Sequence_t ;
+#endif
+
+#define Servo_VERSION           2     // software version of this library
+
+#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
+#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
+#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
+#define REFRESH_INTERVAL    20000     // minimum time to refresh servos in microseconds
+
+#define SERVOS_PER_TIMER       12     // the maximum number of servos controlled by one timer
+#define MAX_SERVOS   (_Nbr_16timers  * SERVOS_PER_TIMER)
+
+#define INVALID_SERVO         255     // flag indicating an invalid servo index
+
+typedef struct  {
+  uint8_t nbr        :6 ;             // a pin number from 0 to 63
+  uint8_t isActive   :1 ;             // true if this channel is enabled, pin not pulsed if false
+} ServoPin_t   ;
+
+typedef struct {
+  ServoPin_t Pin;
+  unsigned int ticks;
+} servo_t;
+
+class Servo
+{
+public:
+  Servo();
+  uint8_t attach(int pin);           // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
+  uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
+  void detach();
+  void write(int value);             // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
+  void writeMicroseconds(int value); // Write pulse width in microseconds
+  int read();                        // returns current pulse width as an angle between 0 and 180 degrees
+  int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
+  bool attached();                   // return true if this servo is attached, otherwise false
+#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
+  int pin;                           // store the hardware pin of the servo
+#endif
+private:
+   uint8_t servoIndex;               // index into the channel data for this servo
+   int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH
+   int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH
+};
+
+#endif
--- a/Firmware/SpoolJoin.cpp
+++ b/Firmware/SpoolJoin.cpp
@ -1,67 +0,0 @@
-#include "SpoolJoin.h"
-#include "Marlin.h"
-#include "eeprom.h"
-#include "messages.h"
-#include "language.h"
-
-namespace SpoolJoin {
-
-SpoolJoin spooljoin;
-
-SpoolJoin::SpoolJoin()
-    : currentMMUSlot(0)
-{
-}
-
-void SpoolJoin::initSpoolJoinStatus()
-{
-    // Useful information to see during bootup
-    SERIAL_ECHOPGM("SpoolJoin is ");
-    uint8_t status = eeprom_init_default_byte((uint8_t*)EEPROM_SPOOL_JOIN, (uint8_t)EEPROM::Disabled);
-    if (status == (uint8_t)EEPROM::Enabled)
-    {
-        SERIAL_ECHOLNRPGM(_O(MSG_ON));
-    } else {
-        SERIAL_ECHOLNRPGM(_O(MSG_OFF));
-    }
-}
-
-void SpoolJoin::toggleSpoolJoin()
-{
-    if (eeprom_read_byte((uint8_t*)EEPROM_SPOOL_JOIN) == (uint8_t)EEPROM::Disabled)
-    {
-        eeprom_update_byte_notify((uint8_t*)EEPROM_SPOOL_JOIN, (uint8_t)EEPROM::Enabled);
-    } else {
-        eeprom_update_byte_notify((uint8_t*)EEPROM_SPOOL_JOIN, (uint8_t)EEPROM::Disabled);
-    }
-}
-
-bool SpoolJoin::isSpoolJoinEnabled()
-{
-    if(eeprom_read_byte((uint8_t*)EEPROM_SPOOL_JOIN) == (uint8_t)EEPROM::Enabled) {
-        return true;
-    } else {
-        return false;
-    }
-}
-
-void SpoolJoin::setSlot(uint8_t slot)
-{
-    currentMMUSlot = slot;
-}
-
-uint8_t SpoolJoin::nextSlot()
-{
-    SERIAL_ECHOPGM("SpoolJoin: ");
-    SERIAL_ECHO((int)currentMMUSlot);
-
-    if (currentMMUSlot >= MMU_FILAMENT_COUNT-1) currentMMUSlot = 0;
-    else currentMMUSlot++;
-
-    SERIAL_ECHOPGM(" -> ");
-    SERIAL_ECHOLN((int)currentMMUSlot);
-
-    return currentMMUSlot;
-}
-
-}
--- a/Firmware/SpoolJoin.h
+++ b/Firmware/SpoolJoin.h
@ -1,46 +0,0 @@
-/// @file
-#pragma once
-#include <stdint.h>
-
-// See documentation here: https://help.prusa3d.com/article/spooljoin-mmu2s_134252
-
-namespace SpoolJoin {
-
-class SpoolJoin {
-public:
-    SpoolJoin();
-
-    enum class EEPROM : uint8_t {
-        Unknown, ///< SpoolJoin is unknown while printer is booting up
-        Enabled, ///< SpoolJoin is enabled in EEPROM
-        Disabled, ///< SpoolJoin is disabled in EEPROM
-        Empty = 0xFF ///< EEPROM has not been set before and all bits are 1 (0xFF) - either a new printer or user erased the memory
-    };
-
-    /// @brief Called when EEPROM is ready to be read
-    void initSpoolJoinStatus();
-
-    /// @brief Toggle SpoolJoin
-    static void toggleSpoolJoin();
-
-    /// @brief Check if SpoolJoin is enabled
-    /// @returns true if enabled, false if disabled
-    bool isSpoolJoinEnabled();
-
-    /// @brief Update the saved MMU slot number so SpoolJoin can determine the next slot to use
-    /// @param slot number of the slot to set
-    void setSlot(uint8_t slot);
-
-    /// @brief Fetch the next slot number should count from 0 to 4.
-    /// When filament slot 4 is depleted, the next slot should be 0.
-    /// @returns the next slot, ranges from 0 to 4
-    uint8_t nextSlot();
-
-private:
-    /// @brief Currently used slot, ranges from 0 to 4
-    uint8_t currentMMUSlot;
-};
-
-extern SpoolJoin spooljoin;
-
-} // namespace SpoolJoin
--- a/Firmware/Tcodes.cpp
+++ b/Firmware/Tcodes.cpp
@ -1,71 +0,0 @@
-#include "Tcodes.h"
-#include "SpoolJoin.h"
-#include "Marlin.h"
-#include "language.h"
-#include "messages.h"
-#include "mmu2.h"
-#include "ultralcd.h"
-#include <avr/pgmspace.h>
-#include <stdint.h>
-#include <stdio.h>
-
-static const char duplicate_Tcode_ignored[] PROGMEM = "Duplicate T-code ignored.";
-
-inline bool IsInvalidTCode(char *const s, uint8_t i) {
-    return ((s[i] < '0' || s[i] > '4') && s[i] != '?' && s[i] != 'x' && s[i] != 'c');
-}
-
-inline void TCodeInvalid() {
-    SERIAL_ECHOLNPGM("Invalid T code.");
-}
-
-void TCodes(char *const strchr_pointer, const uint8_t codeValue) {
-    uint8_t index = 1;
-    for ( /*nothing*/ ; strchr_pointer[index] == ' ' || strchr_pointer[index] == '\t'; index++)
-        ;
-
-    strchr_pointer[index] = tolower(strchr_pointer[index]);
-
-    if (IsInvalidTCode(strchr_pointer, index)){
-        TCodeInvalid();
-    } else if (strchr_pointer[index] == 'x' || strchr_pointer[index] == '?'){
-        // load to extruder gears; if mmu is not present do nothing
-        if (MMU2::mmu2.Enabled()) {
-            MMU2::mmu2.tool_change(strchr_pointer[index], choose_menu_P(_T(MSG_SELECT_FILAMENT), MSG_FILAMENT));
-        }
-    } else if (strchr_pointer[index] == 'c'){
-        // load from extruder gears to nozzle (nozzle should be preheated)
-        if (MMU2::mmu2.Enabled()) {
-            MMU2::mmu2.tool_change(strchr_pointer[index], MMU2::mmu2.get_current_tool());
-        }
-    } else { // Process T0 ... T4
-        if (MMU2::mmu2.Enabled()) {
-            if (codeValue == MMU2::mmu2.get_current_tool()){
-                // don't execute the same T-code twice in a row
-                puts_P(duplicate_Tcode_ignored);
-            } else {
-#if defined(MMU_HAS_CUTTER) && defined(MMU_ALWAYS_CUT)
-                if (EEPROM_MMU_CUTTER_ENABLED_always == eeprom_read_byte((uint8_t *)EEPROM_MMU_CUTTER_ENABLED)) {
-                    MMU2::mmu2.cut_filament(codeValue);
-                }
-#endif // defined(MMU_HAS_CUTTER) && defined(MMU_ALWAYS_CUT)
-                MMU2::mmu2.tool_change(codeValue);
-            }
-        } else {
-            SERIAL_ECHO_START;
-            if (codeValue >= EXTRUDERS) {
-                SERIAL_ECHO('T');
-                SERIAL_ECHOLN(codeValue + '0');
-                SERIAL_ECHOLNRPGM(_n("Invalid extruder")); ////MSG_INVALID_EXTRUDER
-            } else {
-// @@TODO               if (code_seen('F')) {
-//                    next_feedrate = code_value();
-//                    if (next_feedrate > 0.0) {
-//                        feedrate = next_feedrate;
-//                    }
-//                }
-                SERIAL_ECHORPGM(_n("Active Extruder: 0")); ////MSG_ACTIVE_EXTRUDER
-            }
-        }
-    }
-}
--- a/Firmware/Tcodes.h
+++ b/Firmware/Tcodes.h
@ -1,5 +0,0 @@
-/// @file
-#pragma once
-#include <stdint.h>
-
-void TCodes(char * const strchr_pointer, const uint8_t codeValue);
--- a/Firmware/Timer.cpp
+++ b/Firmware/Timer.cpp
@ -6,6 +6,17 @@
 #include "Timer.h"
 #include "system_timer.h"

+/**
+ * @brief construct Timer
+ *
+ * It is guaranteed, that construction is equivalent with zeroing all members.
+ * This property can be exploited in menu_data.
+ */
+template<typename T>
+Timer<T>::Timer() : m_isRunning(false), m_started()
+{
+}
+
 /**
 * @brief Start timer
 */
@ -53,24 +64,5 @@ bool Timer<T>::expired(T msPeriod)
    return expired;
 }

-/**
- * @brief Ticks since the timer was started
- *
- * This function returns 0 if the timer is not started. Otherwise, it returns
- * the time in milliseconds since the timer was started.
- * This function is expected to handle wrap around of time register well.
- * The maximum elapsed time is dictated by the template type
- */
-template<typename T>
-T Timer<T>::elapsed() {
-  return m_isRunning ? (_millis() - m_started) : 0;
-}
-
-template<typename T>
-bool Timer<T>::expired_cont(T msPeriod)
-{
-    return !m_isRunning || expired(msPeriod);
-}
-
 template class Timer<unsigned long>;
 template class Timer<unsigned short>;
--- a/Firmware/Timer.h
+++ b/Firmware/Timer.h
@ -17,18 +17,13 @@ template <class T>
 class Timer
 {
 public:
-    inline constexpr Timer()
-    : m_isRunning(false)
-    , m_started(0) {};
-
+    Timer();
    void start();
    void stop(){m_isRunning = false;}
-    bool running()const {return m_isRunning;}
-    bool expired(T msPeriod); // returns true only once after expiration, then stops running
-    T elapsed(); // returns the time in milliseconds since the timer was started or 0 otherwise
-    bool expired_cont(T msPeriod); // return true when continuosly when expired / not running
+    bool running(){return m_isRunning;}
+    bool expired(T msPeriod);
 protected:
-    T started()const {return m_started;}
+    T started(){return m_started;}
 private:
    bool m_isRunning;
    T m_started;
--- a/Firmware/adc.c
+++ b/Firmware/adc.c
@ -0,0 +1,95 @@
+//adc.c
+
+#include "adc.h"
+#include <stdio.h>
+#include <avr/io.h>
+#include <avr/pgmspace.h>
+#include "pins.h"
+
+uint8_t adc_state;
+uint8_t adc_count;
+uint16_t adc_values[ADC_CHAN_CNT];
+uint16_t adc_sim_mask;
+
+
+#ifdef ADC_CALLBACK
+	extern void ADC_CALLBACK(void);
+#endif //ADC_CALLBACK
+
+
+void adc_init(void)
+{
+	printf_P(PSTR("adc_init\n"));
+	adc_sim_mask = 0x00;
+	ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
+	ADMUX |= (1 << REFS0);
+	ADCSRA |= (1 << ADEN);
+//	ADCSRA |= (1 << ADIF) | (1 << ADSC);
+	DIDR0 = ((ADC_CHAN_MSK & ADC_DIDR_MSK) & 0xff);
+	DIDR2 = ((ADC_CHAN_MSK & ADC_DIDR_MSK) >> 8);
+	adc_reset();
+//	adc_sim_mask = 0b0101;
+//	adc_sim_mask = 0b100101;
+//	adc_values[0] = 1023 * 16;
+//	adc_values[2] = 1023 * 16;
+//	adc_values[5] = 1002 * 16;
+}
+
+void adc_reset(void)
+{
+	adc_state = 0;
+	adc_count = 0;
+	uint8_t i; for (i = 0; i < ADC_CHAN_CNT; i++)
+	if ((adc_sim_mask & (1 << i)) == 0)
+		adc_values[i] = 0;
+}
+
+void adc_setmux(uint8_t ch)
+{
+	ch &= 0x0f;
+	if (ch & 0x08) ADCSRB |= (1 << MUX5);
+	else ADCSRB &= ~(1 << MUX5);
+	ADMUX = (ADMUX & ~(0x07)) | (ch & 0x07);
+}
+
+uint8_t adc_chan(uint8_t index)
+{
+	uint8_t chan = 0;
+	uint16_t mask = 1;
+	while (mask)
+	{
+		if ((mask & ADC_CHAN_MSK) && (index-- == 0)) break;
+		mask <<= 1;
+		chan++;
+	}
+	return chan;
+}
+
+void adc_cycle(void)
+{
+	if (adc_state & 0x80)
+	{
+		uint8_t index = adc_state & 0x0f;
+		if ((adc_sim_mask & (1 << index)) == 0)
+			adc_values[index] += ADC;
+		if (++index >= ADC_CHAN_CNT)
+		{
+			index = 0;
+			adc_count++;
+			if (adc_count >= ADC_OVRSAMPL)
+			{
+#ifdef ADC_CALLBACK
+				ADC_CALLBACK();
+#endif //ADC_CALLBACK
+				adc_reset();
+			}
+		}
+		adc_setmux(adc_chan(index));
+		adc_state = index;
+	}
+	else
+	{
+		ADCSRA |= (1 << ADSC); //start conversion
+		adc_state |= 0x80;
+	}
+}
--- a/Firmware/adc.cpp
+++ b/Firmware/adc.cpp
@ -1,81 +0,0 @@
-#include "adc.h"
-#include <stdio.h>
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include <avr/pgmspace.h>
-#include <string.h>
-#include "pins.h"
-
-static uint8_t adc_count; //used for oversampling
-static uint8_t adc_channel_idx; //bitmask index
-volatile uint8_t adc_channel; //regular index
-volatile uint16_t adc_values[ADC_CHAN_CNT];
-
-static void adc_reset();
-static void adc_setmux(uint8_t ch);
-
-void adc_init()
-{
-	puts_P(PSTR("adc_init"));
-    DIDR0 = ((ADC_CHAN_MSK & ADC_DIDR_MSK) & 0xff); //disable digital inputs PORTF
-    DIDR2 = ((ADC_CHAN_MSK & ADC_DIDR_MSK) >> 8); //disable digital inputs PORTK
-    ADMUX |= (1 << REFS0); //use AVCC as reference
-
-    //enable ADC, set prescaler/128, enable interrupt
-    ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0) | (1 << ADIF) | (1 << ADIE);
-}
-
-static void adc_reset()
-{
-    static const uint8_t first_channel_idx = 0;
-    static_assert((1 << first_channel_idx) & ADC_CHAN_MSK);
-
-    ADCSRA &= ~(1 << ADSC); //stop conversion just in case
-    adc_count = 0;
-    adc_channel = 0;
-    adc_channel_idx = first_channel_idx;
-    adc_setmux(adc_channel_idx);
-    memset((void*)adc_values, 0, sizeof(adc_values));
-}
-
-static void adc_setmux(uint8_t ch)
-{
-	ch &= 0x0f;
-	if (ch & 0x08) ADCSRB |= (1 << MUX5);
-	else ADCSRB &= ~(1 << MUX5);
-	ADMUX = (ADMUX & ~(0x07)) | (ch & 0x07);
-}
-
-void adc_start_cycle() {
-	adc_reset();
-	ADCSRA |= (1 << ADSC); //start conversion
-}
-
-#ifdef ADC_CALLBACK
-extern void ADC_CALLBACK();
-#endif //ADC_CALLBACK
-
-ISR(ADC_vect)
-{
-    adc_values[adc_channel] += ADC;
-    if (++adc_count == ADC_OVRSAMPL)
-    {
-        // go to the next channel
-        if (++adc_channel == ADC_CHAN_CNT) {
-#ifdef ADC_CALLBACK
-            ADC_CALLBACK();
-#endif
-            return; // do not start the next measurement since there are no channels remaining
-        }
-
-        // find the next channel
-        while (++adc_channel_idx) {
-            if (ADC_CHAN_MSK & (1 << adc_channel_idx)) {
-                adc_setmux(adc_channel_idx);
-                adc_count = 0;
-                break;
-            }
-        }
-    }
-    ADCSRA |= (1 << ADSC); //start conversion
-}
--- a/Firmware/adc.h
+++ b/Firmware/adc.h
@ -1,8 +1,15 @@
-#pragma once
+//adc.h
+#ifndef _ADC_H
+#define _ADC_H

 #include <inttypes.h>
 #include "config.h"

+
+#if defined(__cplusplus)
+extern "C" {
+#endif //defined(__cplusplus)
+
 /*
 http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
 */
@ -15,11 +22,24 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
 # error "ADC_CHAN_MSK oes not match ADC_CHAN_CNT"
 #endif

-#define VOLT_DIV_REF 5 //[V]
+extern uint8_t adc_state;
+extern uint8_t adc_count;
+extern uint16_t adc_values[ADC_CHAN_CNT];
+extern uint16_t adc_sim_mask;

-extern volatile uint8_t adc_channel;
-extern volatile uint16_t adc_values[ADC_CHAN_CNT];

-extern void adc_init();
-extern void adc_start_cycle(); //should be called from an atomic context only
-static inline bool adc_cycle_done() { return adc_channel >= ADC_CHAN_CNT; }
+extern void adc_init(void);
+
+extern void adc_reset(void);
+
+extern void adc_setmux(uint8_t ch);
+
+extern uint8_t adc_chan(uint8_t index);
+
+extern void adc_cycle(void);
+
+
+#if defined(__cplusplus)
+}
+#endif //defined(__cplusplus)
+#endif //_ADC_H
--- a/Firmware/asm.h
+++ b/Firmware/asm.h
@ -1,21 +0,0 @@
-#pragma once
-#include <stdint.h>
-#include "macros.h"
-
-#ifdef __AVR_ATmega2560__
-
-// return the current PC (on AVRs with 22bit PC)
-FORCE_INLINE __uint24 GETPC(void)
-{
-  __uint24 ret;
-  asm (
-      "rcall .\n"
-      "pop %A0\n"
-      "pop %B0\n"
-      "pop %C0\n"
-      : "=&r" (ret)
-  );
-  return ret;
-}
-
-#endif
--- a/Firmware/backlight.cpp
+++ b/Firmware/backlight.cpp
@ -1,12 +1,12 @@
 //backlight.cpp
-#include <Arduino.h>
-#include <avr/eeprom.h>
+
 #include "backlight.h"
+#include <avr/eeprom.h>
+#include <Arduino.h>
 #include "eeprom.h"
-#include "fastio.h"
-#include "macros.h"
+#include "Marlin.h"
 #include "pins.h"
-#include "system_timer.h"
+#include "fastio.h"
 #include "Timer.h"

 #ifdef LCD_BL_PIN
@ -14,10 +14,10 @@
 #define BL_FLASH_DELAY_MS 25

 bool backlightSupport = 0; //only if it's true will any of the settings be visible to the user
-uint8_t backlightLevel_HIGH = 0;
-uint8_t backlightLevel_LOW = 0;
+int16_t backlightLevel_HIGH = 0;
+int16_t backlightLevel_LOW = 0;
 uint8_t backlightMode = BACKLIGHT_MODE_BRIGHT;
-int16_t backlightTimer_period = LCD_BACKLIGHT_TIMEOUT;
+int16_t backlightTimer_period = 10;
 LongTimer backlightTimer;

 static void backlightTimer_reset() //used for resetting the timer and waking the display. Triggered on user interactions.
@ -32,7 +32,7 @@ void force_bl_on(bool section_start)
    if (section_start)
    {
        backlightMode = BACKLIGHT_MODE_BRIGHT;
-        if (backlightLevel_HIGH < LCD_BACKLIGHT_FORCE_ON) backlightLevel_HIGH = LCD_BACKLIGHT_FORCE_ON;
+        if (backlightLevel_HIGH < 30) backlightLevel_HIGH = 30;
    }
    else
    {
@ -45,7 +45,7 @@ void force_bl_on(bool section_start)
 void backlight_wake(const uint8_t flashNo)
 {
    if (!backlightSupport) return;
-
+    
    if (flashNo)
    {
        uint8_t backlightMode_bck = backlightMode;
@ -62,16 +62,16 @@ void backlight_wake(const uint8_t flashNo)

 void backlight_save() //saves all backlight data to eeprom.
 {
-    eeprom_update_byte_notify((uint8_t *)EEPROM_BACKLIGHT_LEVEL_HIGH, backlightLevel_HIGH);
-    eeprom_update_byte_notify((uint8_t *)EEPROM_BACKLIGHT_LEVEL_LOW, backlightLevel_LOW);
-    eeprom_update_byte_notify((uint8_t *)EEPROM_BACKLIGHT_MODE, backlightMode);
-    eeprom_update_word_notify((uint16_t *)EEPROM_BACKLIGHT_TIMEOUT, backlightTimer_period);
+    eeprom_update_byte((uint8_t *)EEPROM_BACKLIGHT_LEVEL_HIGH, (uint8_t)backlightLevel_HIGH);
+    eeprom_update_byte((uint8_t *)EEPROM_BACKLIGHT_LEVEL_LOW, (uint8_t)backlightLevel_LOW);
+    eeprom_update_byte((uint8_t *)EEPROM_BACKLIGHT_MODE, backlightMode);
+    eeprom_update_word((uint16_t *)EEPROM_BACKLIGHT_TIMEOUT, backlightTimer_period);
 }

 void backlight_update()
 {
    if (!backlightSupport) return;
-
+    
    if (backlightMode == BACKLIGHT_MODE_AUTO)
    {
        if (backlightTimer.expired((uint32_t)backlightTimer_period * 1000ul)) analogWrite(LCD_BL_PIN, backlightLevel_LOW);
@ -91,22 +91,30 @@ void backlight_init()
    backlightSupport = !READ(LCD_BL_PIN);
    if (!backlightSupport) return;

-    //initialize backlight
-    backlightMode = eeprom_init_default_byte((uint8_t *)EEPROM_BACKLIGHT_MODE, BACKLIGHT_MODE_AUTO);
-    backlightLevel_HIGH = eeprom_init_default_byte((uint8_t *)EEPROM_BACKLIGHT_LEVEL_HIGH, LCD_BACKLIGHT_LEVEL_HIGH);
-    backlightLevel_LOW = eeprom_init_default_byte((uint8_t *)EEPROM_BACKLIGHT_LEVEL_LOW, LCD_BACKLIGHT_LEVEL_LOW);
-    backlightTimer_period = eeprom_init_default_word((uint16_t *)EEPROM_BACKLIGHT_TIMEOUT, LCD_BACKLIGHT_TIMEOUT); // in seconds
-
+//initialize backlight
+    backlightMode = eeprom_read_byte((uint8_t *)EEPROM_BACKLIGHT_MODE);
+    if (backlightMode == 0xFF) //set default values
+    {
+        backlightMode = BACKLIGHT_MODE_AUTO;
+        backlightLevel_HIGH = 130;
+        backlightLevel_LOW = 50;
+        backlightTimer_period = 10; //in seconds
+        backlight_save();
+    }
+    backlightLevel_HIGH = eeprom_read_byte((uint8_t *)EEPROM_BACKLIGHT_LEVEL_HIGH);
+    backlightLevel_LOW = eeprom_read_byte((uint8_t *)EEPROM_BACKLIGHT_LEVEL_LOW);
+    backlightTimer_period = eeprom_read_word((uint16_t *)EEPROM_BACKLIGHT_TIMEOUT);
+    
    SET_OUTPUT(LCD_BL_PIN);
    backlightTimer_reset();
 }

 #else //LCD_BL_PIN

-void force_bl_on(bool) {}
+void force_bl_on(__attribute__((unused)) bool section_start) {}
 void backlight_update() {}
 void backlight_init() {}
 void backlight_save() {}
-void backlight_wake(const uint8_t) {}
+void backlight_wake(__attribute__((unused)) const uint8_t flashNo) {}

-#endif //LCD_BL_PIN
+#endif //LCD_BL_PIN
--- a/Firmware/backlight.h
+++ b/Firmware/backlight.h
@ -3,6 +3,8 @@
 #define _BACKLIGHT_H

 #include <inttypes.h>
+#include "Marlin.h"
+#include "pins.h"

 enum Backlight_Mode
 {
@ -11,8 +13,8 @@ enum Backlight_Mode
 	BACKLIGHT_MODE_AUTO    = 2,
 };

-extern uint8_t backlightLevel_HIGH;
-extern uint8_t backlightLevel_LOW;
+extern int16_t backlightLevel_HIGH;
+extern int16_t backlightLevel_LOW;
 extern uint8_t backlightMode;
 extern bool backlightSupport;
 extern int16_t backlightTimer_period;
--- a/Firmware/bootapp.c
+++ b/Firmware/bootapp.c
@ -6,17 +6,19 @@


 #include <stdio.h>
+extern FILE _uartout;
+#define uartout (&_uartout)

 extern void softReset();

 void bootapp_print_vars(void)
 {
-	printf_P(PSTR("boot_src_addr  =0x%08lx\n"), boot_src_addr);
-	printf_P(PSTR("boot_dst_addr  =0x%08lx\n"), boot_dst_addr);
-	printf_P(PSTR("boot_copy_size =0x%04x\n"), boot_copy_size);
-	printf_P(PSTR("boot_reserved  =0x%02x\n"), boot_reserved);
-	printf_P(PSTR("boot_app_flags =0x%02x\n"), boot_app_flags);
-	printf_P(PSTR("boot_app_magic =0x%08lx\n"), boot_app_magic);
+	fprintf_P(uartout, PSTR("boot_src_addr  =0x%08lx\n"), boot_src_addr);
+	fprintf_P(uartout, PSTR("boot_dst_addr  =0x%08lx\n"), boot_dst_addr);
+	fprintf_P(uartout, PSTR("boot_copy_size =0x%04x\n"), boot_copy_size);
+	fprintf_P(uartout, PSTR("boot_reserved  =0x%02x\n"), boot_reserved);
+	fprintf_P(uartout, PSTR("boot_app_flags =0x%02x\n"), boot_app_flags);
+	fprintf_P(uartout, PSTR("boot_app_magic =0x%08lx\n"), boot_app_magic);
 }


@ -24,11 +26,21 @@ void bootapp_ram2flash(uint16_t rptr, uint16_t fptr, uint16_t size)
 {
 	cli();
 	boot_app_magic = BOOT_APP_MAGIC;
-	boot_app_flags |= BOOT_APP_FLG_COPY | BOOT_APP_FLG_ERASE;
+	boot_app_flags |= BOOT_APP_FLG_COPY;
+	boot_app_flags |= BOOT_APP_FLG_ERASE;
+/*	uint16_t ui; for (ui = 0; ui < size; ui++)
+	{
+		uint8_t uc = ram_array[ui+rptr];
+		if (pgm_read_byte(ui+fptr) & uc != uc)
+		{
+			boot_app_flags |= BOOT_APP_FLG_ERASE;
+			break;
+		}
+	}*/
 	boot_copy_size = (uint16_t)size;
 	boot_src_addr = (uint32_t)rptr;
 	boot_dst_addr = (uint32_t)fptr;
-	// bootapp_print_vars();
+	bootapp_print_vars();
 	softReset();
 }

@ -37,8 +49,7 @@ void bootapp_reboot_user0(uint8_t reserved)
 	cli();
 	boot_app_magic = BOOT_APP_MAGIC;
 	boot_app_flags = BOOT_APP_FLG_USER0;
-	boot_copy_size = 0;
 	boot_reserved = reserved;
-	// bootapp_print_vars();
+	bootapp_print_vars();
 	softReset();
 }
--- a/Firmware/bootapp.h
+++ b/Firmware/bootapp.h
@ -3,11 +3,11 @@
 #define BOOTAPP_H

 #include "config.h"
-#include <avr/io.h>
 #include <inttypes.h>


-#define RAMSIZE        (RAMEND+1-RAMSTART)
+#define RAMSIZE        0x2000
+#define ram_array ((uint8_t*)(0))
 #define boot_src_addr  (*((uint32_t*)(RAMSIZE - 16)))
 #define boot_dst_addr  (*((uint32_t*)(RAMSIZE - 12)))
 #define boot_copy_size (*((uint16_t*)(RAMSIZE - 8)))
@ -17,10 +17,11 @@
 #define BOOT_APP_FLG_ERASE 0x01
 #define BOOT_APP_FLG_COPY  0x02
 #define BOOT_APP_FLG_FLASH 0x04
+#define BOOT_APP_FLG_RUN 0x08

 #define BOOT_APP_FLG_USER0 0x80

-#define BOOT_APP_MAGIC 0x55aa55aaUL
+#define BOOT_APP_MAGIC 0x55aa55aa


 #if defined(__cplusplus)
--- a/Firmware/cardreader.cpp
+++ b/Firmware/cardreader.cpp
--- a/Firmware/cardreader.h
+++ b/Firmware/cardreader.h
@ -1,80 +1,66 @@
 #ifndef CARDREADER_H
 #define CARDREADER_H

-#define SDSUPPORT
-
 #ifdef SDSUPPORT

-#define MAX_DIR_DEPTH 6
+#define MAX_DIR_DEPTH 10

 #include "SdFile.h"
+enum LsAction {LS_SerialPrint,LS_Count,LS_GetFilename};
 class CardReader
 {
 public:
  CardReader();
-
-  enum LsAction : uint8_t
-  {
-    LS_SerialPrint,
-    LS_Count,
-    LS_GetFilename,
-  };
-  struct ls_param
-  {
-    bool LFN : 1;
-    bool timestamp : 1;
-    inline ls_param():LFN(0), timestamp(0) { }
-    inline ls_param(bool LFN, bool timestamp):LFN(LFN), timestamp(timestamp) { }
-  } __attribute__((packed));
-
-  void mount(bool doPresort = true);
+  
+  void initsd();
  void write_command(char *buf);
  void write_command_no_newline(char *buf);
  //files auto[0-9].g on the sd card are performed in a row
  //this is to delay autostart and hence the initialisaiton of the sd card to some seconds after the normal init, so the device is available quick after a reset

-  void checkautostart(bool x);
-  void openFileWrite(const char* name);
-  void openFileReadFilteredGcode(const char* name, bool replace_current = false);
+  void checkautostart(bool x); 
+  void openFile(const char* name,bool read,bool replace_current=true);
  void openLogFile(const char* name);
  void removeFile(const char* name);
  void closefile(bool store_location=false);
  void release();
  void startFileprint();
  uint32_t getFileSize();
-  void getStatus(bool arg_P);
+  void getStatus();
  void printingHasFinished();

  void getfilename(uint16_t nr, const char* const match=NULL);
-  void getfilename_simple(uint16_t entry, const char * const match = NULL);
-  void getfilename_next(uint32_t position, const char * const match = NULL);
+  void getfilename_simple(uint32_t position, const char * const match = NULL);
  uint16_t getnrfilenames();
-
+  
  void getAbsFilename(char *t);
-  void printAbsFilenameFast();
  void getDirName(char* name, uint8_t level);
-  uint8_t getWorkDirDepth();
+  uint16_t getWorkDirDepth();
+  

-  void ls(ls_param params);
-  bool chdir(const char * relpath, bool doPresort);
+  void ls();
+  void chdir(const char * relpath);
  void updir();
-  void cdroot(bool doPresort);
+  void setroot();

  #ifdef SDCARD_SORT_ALPHA
     void presort();
-     void getfilename_sorted(const uint16_t nr, uint8_t sdSort);
-     void getfilename_afterMaxSorting(uint16_t entry, const char * const match = NULL);
+	 #ifdef SDSORT_QUICKSORT
+		void swap(uint8_t left, uint8_t right);
+		void quicksort(uint8_t left, uint8_t right);
+	 #endif //SDSORT_QUICKSORT
+     void getfilename_sorted(const uint16_t nr);
+     #if SDSORT_GCODE
+	 FORCE_INLINE void setSortOn(bool b) { sort_alpha = b; presort(); }
+     FORCE_INLINE void setSortFolders(int i) { sort_folders = i; presort(); }
+     //FORCE_INLINE void setSortReverse(bool b) { sort_reverse = b; }
+	 #endif
  #endif

  FORCE_INLINE bool isFileOpen() { return file.isOpen(); }
-  bool eof() { return sdpos>=filesize; }
-  FORCE_INLINE int16_t getFilteredGcodeChar()
-  {
-      int16_t c = (int16_t)file.readFilteredGcode();
-      sdpos = file.curPosition();
-      return c;
-  };
-  void setIndex(long index) {sdpos = index;file.seekSetFilteredGcode(index);};
+  FORCE_INLINE bool eof() { return sdpos>=filesize ;};
+  FORCE_INLINE int16_t get() {  sdpos = file.curPosition();return (int16_t)file.read();};
+  FORCE_INLINE void setIndex(long index) {sdpos = index;file.seekSet(index);};
  FORCE_INLINE uint8_t percentDone(){if(!isFileOpen()) return 0; if(filesize) return sdpos/((filesize+99)/100); else return 0;};
  FORCE_INLINE char* getWorkDirName(){workDir.getFilename(filename);return filename;};
  FORCE_INLINE uint32_t get_sdpos() { if (!isFileOpen()) return 0; else return(sdpos); };
@ -83,35 +69,65 @@ public:
  void ToshibaFlashAir_enable(bool enable) { card.setFlashAirCompatible(enable); }
  bool ToshibaFlashAir_GetIP(uint8_t *ip);

-  //Reprint
-  bool FileExists(const char* filename);
-
 public:
  bool saving;
  bool logging;
-  bool sdprinting;
-  bool mounted;
-  char filename[FILENAME_LENGTH];
+  bool sdprinting ;  
+  bool cardOK ;
+  char filename[13];
  // There are scenarios when simple modification time is not enough (on MS Windows)
  // Therefore these timestamps hold the most recent one of creation/modification date/times
  uint16_t crmodTime, crmodDate;
-  uint32_t /* cluster, */ position;
+  uint32_t cluster, position;
  char longFilename[LONG_FILENAME_LENGTH];
  bool filenameIsDir;
  int lastnr; //last number of the autostart;
-#ifdef SDCARD_SORT_ALPHA
-  bool presort_flag;
-#endif // SDCARD_SORT_ALPHA
-  char dir_names[MAX_DIR_DEPTH][9];
 private:
  SdFile root,*curDir,workDir,workDirParents[MAX_DIR_DEPTH];
-  uint8_t workDirDepth;
+  uint16_t workDirDepth;

  // Sort files and folders alphabetically.
 #ifdef SDCARD_SORT_ALPHA
  uint16_t sort_count;        // Count of sorted items in the current directory
-  uint16_t sort_entries[SDSORT_LIMIT];
-  uint16_t lastSortedFilePosition;
+  #if SDSORT_GCODE
+  bool sort_alpha;          // Flag to enable / disable the feature
+  int sort_folders;         // Flag to enable / disable folder sorting
+							//bool sort_reverse;      // Flag to enable / disable reverse sorting
+  #endif
+
+							// By default the sort index is static
+  #if SDSORT_DYNAMIC_RAM
+  uint8_t *sort_order;
+  #else
+  uint8_t sort_order[SDSORT_LIMIT];
+  #endif
+  // Cache filenames to speed up SD menus.
+  #if SDSORT_USES_RAM
+
+  // If using dynamic ram for names, allocate on the heap.
+  #if SDSORT_CACHE_NAMES
+    #if SDSORT_DYNAMIC_RAM
+      char **sortshort, **sortnames;
+    #else
+      char sortshort[SDSORT_LIMIT][FILENAME_LENGTH];
+      char sortnames[SDSORT_LIMIT][FILENAME_LENGTH];
+    #endif
+  #elif !SDSORT_USES_STACK
+    char sortnames[SDSORT_LIMIT][FILENAME_LENGTH];
+    uint16_t modification_time[SDSORT_LIMIT];
+    uint16_t modification_date[SDSORT_LIMIT];
+  #endif
+
+  // Folder sorting uses an isDir array when caching items.
+  #if HAS_FOLDER_SORTING
+    #if SDSORT_DYNAMIC_RAM
+      uint8_t *isDir;
+    #elif (SDSORT_CACHE_NAMES) || !(SDSORT_USES_STACK)
+      uint8_t isDir[(SDSORT_LIMIT + 7) >> 3];
+    #endif
+  #endif
+
+  #endif // SDSORT_USES_RAM

 #endif // SDCARD_SORT_ALPHA

@ -130,13 +146,17 @@ private:
  char filenames[SD_PROCEDURE_DEPTH][MAXPATHNAMELENGTH];
  uint32_t filesize;
  //int16_t n;
-  ShortTimer autostart_atmillis;
+  unsigned long autostart_atmillis;
  uint32_t sdpos ;

-  uint16_t nrFiles; //counter for the files in the current directory and recycled as position counter for getting the nrFiles'th name in the directory.
+  bool autostart_stilltocheck; //the sd start is delayed, because otherwise the serial cannot answer fast enought to make contact with the hostsoftware.
+  
+  LsAction lsAction; //stored for recursion.
+  int16_t nrFiles; //counter for the files in the current directory and recycled as position counter for getting the nrFiles'th name in the directory.
+  char* diveDirName;

-  bool diveSubfolder (const char *&fileName);
-  void lsDive(const char *prepend, SdFile parent, const char * const match=NULL, LsAction lsAction = LS_GetFilename, ls_param lsParams = ls_param());
+  void diveSubfolder (const char *fileName, SdFile& dir);
+  void lsDive(const char *prepend, SdFile parent, const char * const match=NULL);
 #ifdef SDCARD_SORT_ALPHA
  void flush_presort();
 #endif
@ -146,7 +166,7 @@ extern CardReader card;
 #define IS_SD_PRINTING (card.sdprinting)

 #if (SDCARDDETECT > -1)
-# ifdef SDCARDDETECTINVERTED
+# ifdef SDCARDDETECTINVERTED 
 #  define IS_SD_INSERTED (READ(SDCARDDETECT)!=0)
 # else
 #  define IS_SD_INSERTED (READ(SDCARDDETECT)==0)
--- a/Firmware/cmdqueue.cpp
+++ b/Firmware/cmdqueue.cpp
@ -1,14 +1,8 @@
-#include <stdarg.h>
-#include <util/atomic.h>
 #include "cmdqueue.h"
 #include "cardreader.h"
 #include "ultralcd.h"
-#include "Prusa_farm.h"
-#include "meatpack.h"
-#include "messages.h"
-#include "language.h"
-#include "stopwatch.h"
-#include "power_panic.h"
+
+extern bool Stopped;

 // Reserve BUFSIZE lines of length MAX_CMD_SIZE plus CMDBUFFER_RESERVE_FRONT.
 char cmdbuffer[BUFSIZE * (MAX_CMD_SIZE + 1) + CMDBUFFER_RESERVE_FRONT];
@ -24,16 +18,17 @@ int buflen = 0;
 // Therefore don't remove the command from the queue in the loop() function.
 bool cmdbuffer_front_already_processed = false;

-// Used for temporarely preventing accidental adding of Serial commands to the queue.
-// For now only check_file and the fancheck pause use this.
-bool cmdqueue_serial_disabled = false;
-
 int serial_count = 0;  //index of character read from serial line
-bool comment_mode = false;
+boolean comment_mode = false;
 char *strchr_pointer; // just a pointer to find chars in the command string like X, Y, Z, E, etc

-ShortTimer serialTimeoutTimer;
+unsigned long TimeSent = _millis();
+unsigned long TimeNow = _millis();
+
+long gcode_N = 0;
 long gcode_LastN = 0;
+long Stopped_gcode_LastN = 0;
+
 uint32_t sdpos_atomic = 0;


@ -96,19 +91,14 @@ bool cmdqueue_pop_front()

 void cmdqueue_reset()
 {
-	while (buflen)
-	{
-		// printf_P(PSTR("dumping: \"%s\" of type %u\n"), cmdbuffer+bufindr+CMDHDRSIZE, CMDBUFFER_CURRENT_TYPE);
-		ClearToSend();
-		cmdqueue_pop_front();
-	}
-	bufindr = 0;
-	bufindw = 0;
+    bufindr = 0;
+    bufindw = 0;
+    buflen = 0;

 	//commands are removed from command queue after process_command() function is finished
 	//reseting command queue and enqueing new commands during some (usually long running) command processing would cause that new commands are immediately removed from queue (or damaged)
 	//this will ensure that all new commands which are enqueued after cmdqueue reset, will be always executed
-	cmdbuffer_front_already_processed = true;
+    cmdbuffer_front_already_processed = true; 
 }

 // How long a string could be pushed to the front of the command queue?
@ -159,7 +149,7 @@ static bool cmdqueue_could_enqueue_front(size_t len_asked)
 // len_asked does not contain the zero terminator size.
 // This function may update bufindw, therefore for the power panic to work, this function must be called
 // with the interrupts disabled!
-static bool __attribute__((noinline)) cmdqueue_could_enqueue_back(size_t len_asked)
+static bool cmdqueue_could_enqueue_back(size_t len_asked, bool atomic_update = false)
 {
    // MAX_CMD_SIZE has to accommodate the zero terminator.
    if (len_asked >= MAX_CMD_SIZE)
@ -169,29 +159,61 @@ static bool __attribute__((noinline)) cmdqueue_could_enqueue_back(size_t len_ask
        // Full buffer.
        return false;

-    // If there is some data stored starting at bufindw, len_asked is certainly smaller than
-    // the allocated data buffer. Try to reserve a new buffer and to move the already received
-    // serial data.
-    // How much memory to reserve for the commands pushed to the front?
-    // End of the queue, when pushing to the end.
-    size_t endw = bufindw + len_asked + (1 + CMDHDRSIZE);
-    if (bufindw < bufindr)
-        // Simple case. There is a contiguous space between the write buffer and the read buffer.
-        return endw + CMDBUFFER_RESERVE_FRONT <= bufindr;
-    // Otherwise the free space is split between the start and end.
-    if (// Could one fit to the end, including the reserve?
-        endw + CMDBUFFER_RESERVE_FRONT <= sizeof(cmdbuffer) ||
-        // Could one fit to the end, and the reserve to the start?
-        (endw <= sizeof(cmdbuffer) && CMDBUFFER_RESERVE_FRONT <= bufindr))
-        return true;
-    // Could one fit both to the start?
-    if (len_asked + (1 + CMDHDRSIZE) + CMDBUFFER_RESERVE_FRONT <= bufindr) {
-        // Mark the rest of the buffer as used.
-        memset(cmdbuffer+bufindw, 0, sizeof(cmdbuffer)-bufindw);
-        // and point to the start.
-        // Be careful! The bufindw needs to be changed atomically for the power panic & filament panic to work.
-        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { bufindw = 0; }
-        return true;
+    if (serial_count > 0) {
+        // If there is some data stored starting at bufindw, len_asked is certainly smaller than
+        // the allocated data buffer. Try to reserve a new buffer and to move the already received
+        // serial data.
+        // How much memory to reserve for the commands pushed to the front?
+        // End of the queue, when pushing to the end.
+        size_t endw = bufindw + len_asked + (1 + CMDHDRSIZE);
+        if (bufindw < bufindr)
+            // Simple case. There is a contiguous space between the write buffer and the read buffer.
+            return endw + CMDBUFFER_RESERVE_FRONT <= bufindr;
+        // Otherwise the free space is split between the start and end.
+        if (// Could one fit to the end, including the reserve?
+            endw + CMDBUFFER_RESERVE_FRONT <= sizeof(cmdbuffer) ||
+            // Could one fit to the end, and the reserve to the start?
+            (endw <= sizeof(cmdbuffer) && CMDBUFFER_RESERVE_FRONT <= bufindr))
+            return true;
+        // Could one fit both to the start?
+        if (len_asked + (1 + CMDHDRSIZE) + CMDBUFFER_RESERVE_FRONT <= bufindr) {
+            // Mark the rest of the buffer as used.
+            memset(cmdbuffer+bufindw, 0, sizeof(cmdbuffer)-bufindw);
+            // and point to the start.
+            // Be careful! The bufindw needs to be changed atomically for the power panic & filament panic to work.
+            if (atomic_update)
+                cli();
+            bufindw = 0;
+            if (atomic_update)
+                sei();
+            return true;
+        }
+    } else {
+        // How much memory to reserve for the commands pushed to the front?
+        // End of the queue, when pushing to the end.
+        size_t endw = bufindw + len_asked + (1 + CMDHDRSIZE);
+        if (bufindw < bufindr)
+            // Simple case. There is a contiguous space between the write buffer and the read buffer.
+            return endw + CMDBUFFER_RESERVE_FRONT <= bufindr;
+        // Otherwise the free space is split between the start and end.
+        if (// Could one fit to the end, including the reserve?
+            endw + CMDBUFFER_RESERVE_FRONT <= sizeof(cmdbuffer) ||
+            // Could one fit to the end, and the reserve to the start?
+            (endw <= sizeof(cmdbuffer) && CMDBUFFER_RESERVE_FRONT <= bufindr))
+            return true;
+        // Could one fit both to the start?
+        if (len_asked + (1 + CMDHDRSIZE) + CMDBUFFER_RESERVE_FRONT <= bufindr) {
+            // Mark the rest of the buffer as used.
+            memset(cmdbuffer+bufindw, 0, sizeof(cmdbuffer)-bufindw);
+            // and point to the start.
+            // Be careful! The bufindw needs to be changed atomically for the power panic & filament panic to work.
+            if (atomic_update)
+                cli();
+            bufindw = 0;
+            if (atomic_update)
+                sei();
+            return true;
+        }
    }
    return false;
 }
@ -254,22 +276,6 @@ void cmdqueue_dump_to_serial()
 }
 #endif /* CMDBUFFER_DEBUG */

-static const char bufferFull[] PROGMEM = "\" failed: Buffer full!";
-static const char enqueingFront[] PROGMEM = "Enqueing to the front: \"";
-
-
-void enquecommandf_P(const char *fmt, ...)
-{
-    // MAX_CMD_SIZE is 96, but for formatting
-    // string we usually don't need more than 30 bytes
-    char cmd_buffer[30];
-    va_list ap;
-    va_start(ap, fmt);
-    vsnprintf_P(cmd_buffer, sizeof(cmd_buffer), fmt, ap);
-    va_end(ap);
-
-    enquecommand(cmd_buffer, false);
-}
 //adds an command to the main command buffer
 //thats really done in a non-safe way.
 //needs overworking someday
@ -305,7 +311,7 @@ void enquecommand(const char *cmd, bool from_progmem)
            SERIAL_PROTOCOLRPGM(cmd);
        else
            SERIAL_ECHO(cmd);
-        SERIAL_ECHOLNRPGM(bufferFull);
+        SERIAL_ECHOLNPGM("\" failed: Buffer full!");
 #ifdef CMDBUFFER_DEBUG
        cmdqueue_dump_to_serial();
 #endif /* CMDBUFFER_DEBUG */
@ -329,7 +335,7 @@ void enquecommand_front(const char *cmd, bool from_progmem)
            strcpy(cmdbuffer + bufindr + CMDHDRSIZE, cmd);
        ++ buflen;
        SERIAL_ECHO_START;
-        SERIAL_ECHORPGM(enqueingFront);
+        SERIAL_ECHOPGM("Enqueing to the front: \"");
        SERIAL_ECHO(cmdbuffer + bufindr + CMDHDRSIZE);
        SERIAL_ECHOLNPGM("\"");
 #ifdef CMDBUFFER_DEBUG
@ -337,12 +343,12 @@ void enquecommand_front(const char *cmd, bool from_progmem)
 #endif /* CMDBUFFER_DEBUG */
    } else {
        SERIAL_ERROR_START;
-        SERIAL_ECHORPGM(enqueingFront);
+        SERIAL_ECHOPGM("Enqueing to the front: \"");
        if (from_progmem)
            SERIAL_PROTOCOLRPGM(cmd);
        else
            SERIAL_ECHO(cmd);
-        SERIAL_ECHOLNRPGM(bufferFull);
+        SERIAL_ECHOLNPGM("\" failed: Buffer full!");
 #ifdef CMDBUFFER_DEBUG
        cmdqueue_dump_to_serial();
 #endif /* CMDBUFFER_DEBUG */
@ -354,12 +360,28 @@ void enquecommand_front(const char *cmd, bool from_progmem)
 void repeatcommand_front()
 {
    cmdbuffer_front_already_processed = true;
+} 
+
+bool is_buffer_empty()
+{
+    if (buflen == 0) return true;
+    else return false;
+}
+
+void proc_commands() {
+	if (buflen)
+	{
+		process_commands();
+		if (!cmdbuffer_front_already_processed)
+			cmdqueue_pop_front();
+		cmdbuffer_front_already_processed = false;
+	}
 }

 void get_command()
 {
    // Test and reserve space for the new command string.
-    if (! cmdqueue_could_enqueue_back(MAX_CMD_SIZE - 1))
+    if (! cmdqueue_could_enqueue_back(MAX_CMD_SIZE - 1, true))
      return;

 	if (MYSERIAL.available() == RX_BUFFER_SIZE - 1) { //compare number of chars buffered in rx buffer with rx buffer size
@ -368,24 +390,17 @@ void get_command()
 	}

  // start of serial line processing loop
-  while (((MYSERIAL.available() > 0 && !saved_printing) || (MYSERIAL.available() > 0 && printingIsPaused())) && !cmdqueue_serial_disabled) {  //is print is saved (crash detection or filament detection), dont process data from serial line
-
-#ifdef ENABLE_MEATPACK
-    // MeatPack Changes
-    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-      const int rec = MYSERIAL.read();
-      if (rec < 0) continue;
-
-      mp_handle_rx_char((uint8_t)rec);
-      char c_res[2] = {0, 0};
-      const uint8_t char_count = mp_get_result_char(c_res);
-      // Note -- Paired bracket in preproc switch below
-      for (uint8_t i = 0; i < char_count; ++i) { char serial_char = c_res[i];
-#else
+  while ((MYSERIAL.available() > 0 && !saved_printing) || (MYSERIAL.available() > 0 && isPrintPaused)) {  //is print is saved (crash detection or filament detection), dont process data from serial line
+	
    char serial_char = MYSERIAL.read();
-#endif
-
-    serialTimeoutTimer.start();
+/*    if (selectedSerialPort == 1)
+    {
+        selectedSerialPort = 0; 
+        MYSERIAL.write(serial_char); // for debuging serial line 2 in farm_mode
+        selectedSerialPort = 1; 
+    } */ //RP - removed
+      TimeSent = _millis();
+      TimeNow = _millis();

    if (serial_char < 0)
        // Ignore extended ASCII characters. These characters have no meaning in the G-code apart from the file names
@ -401,23 +416,22 @@ void get_command()
        comment_mode = false; //for new command
        return;
      }
-      cmdbuffer[bufindw+serial_count+CMDHDRSIZE] = 0; // terminate string
-      char* cmd_head = cmdbuffer+bufindw+CMDHDRSIZE; // current command pointer
-      char* cmd_start = cmd_head; // pointer past the line number (if any)
-
+      cmdbuffer[bufindw+serial_count+CMDHDRSIZE] = 0; //terminate string
      if(!comment_mode){
-		  long gcode_N = -1; // seen line number
+		  
+		  gcode_N = 0;

 		  // Line numbers must be first in buffer
-		  if (*cmd_head == 'N') {

-			  // Line number met: decode the number, then move cmd_start past all spaces.
-			  gcode_N = (strtol(cmd_head+1, &cmd_start, 10));
-			  while (*cmd_start == ' ') ++cmd_start;
+		  if ((strstr(cmdbuffer+bufindw+CMDHDRSIZE, "PRUSA") == NULL) &&
+			  (cmdbuffer[bufindw+CMDHDRSIZE] == 'N')) {

-			  // Test whether the successive lines are stamped with an increasing line number ID.
-			  if(gcode_N != gcode_LastN+1 && strncmp_P(cmd_start, PSTR("M110"), 4)) {
-				  // Line numbers not sent in succession and M110 not seen.
+			  // Line number met. When sending a G-code over a serial line, each line may be stamped with its index,
+			  // and Marlin tests, whether the successive lines are stamped with an increasing line number ID
+			  gcode_N = (strtol(cmdbuffer+bufindw+CMDHDRSIZE+1, NULL, 10));
+			  if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer+bufindw+CMDHDRSIZE, PSTR("M110")) == NULL) ) {
+				  // M110 - set current line number.
+				  // Line numbers not sent in succession.
 				  SERIAL_ERROR_START;
 				  SERIAL_ERRORRPGM(_n("Line Number is not Last Line Number+1, Last Line: "));////MSG_ERR_LINE_NO
 				  SERIAL_ERRORLN(gcode_LastN);
@ -427,13 +441,13 @@ void get_command()
 				  return;
 			  }

-			  if((strchr_pointer = strchr(cmd_start, '*')) != NULL)
+			  if((strchr_pointer = strchr(cmdbuffer+bufindw+CMDHDRSIZE, '*')) != NULL)
 			  {
 				  byte checksum = 0;
-				  char *p = cmd_head;
+				  char *p = cmdbuffer+bufindw+CMDHDRSIZE;
 				  while (p != strchr_pointer)
 					  checksum = checksum^(*p++);
-				  if (code_value_short() != (int16_t)checksum) {
+				  if (int(strtol(strchr_pointer+1, NULL, 10)) != int(checksum)) {
 					  SERIAL_ERROR_START;
 					  SERIAL_ERRORRPGM(_n("checksum mismatch, Last Line: "));////MSG_ERR_CHECKSUM_MISMATCH
 					  SERIAL_ERRORLN(gcode_LastN);
@ -453,82 +467,54 @@ void get_command()
 				  serial_count = 0;
 				  return;
 			  }
+
+			  // Don't parse N again with code_seen('N')
+			  cmdbuffer[bufindw + CMDHDRSIZE] = '$';
+			  //if no errors, continue parsing
+			  gcode_LastN = gcode_N;
 		}
-        else
+        // if we don't receive 'N' but still see '*'
+        if ((cmdbuffer[bufindw + CMDHDRSIZE] != 'N') && (cmdbuffer[bufindw + CMDHDRSIZE] != '$') && (strchr(cmdbuffer+bufindw+CMDHDRSIZE, '*') != NULL))
        {
-            // move cmd_start past all spaces
-            while (*cmd_start == ' ') ++cmd_start;

-            // if we didn't receive 'N' but still see '*'
-            if (strchr(cmd_start, '*') != NULL)
-            {
-                SERIAL_ERROR_START;
-                SERIAL_ERRORRPGM(_n("No Line Number with checksum, Last Line: "));////MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM
-                SERIAL_ERRORLN(gcode_LastN);
-                FlushSerialRequestResend();
-                serial_count = 0;
-                return;
-            }
-        }
-
-        // Handle KILL early, even when Stopped
-        if(strcmp_P(cmd_start, PSTR("M112")) == 0)
-          kill(MSG_M112_KILL);
-
-        // Bypass Stopped for some commands
-        bool allow_when_stopped = false;
-        if(strncmp_P(cmd_start, PSTR("M310"), 4) == 0)
-            allow_when_stopped = true;
-
-        // Handle the USB timer
-        if ((*cmd_start == 'G') && (GetPrinterState() != PrinterState::IsSDPrinting)) {
-            usb_timer.start();
-            SetPrinterState(PrinterState::IsHostPrinting); //set printer state busy printing to hide LCD menu while USB printing
-            eeprom_update_byte_notify((uint8_t*)EEPROM_UVLO, PowerPanic::NO_PENDING_RECOVERY);
-        }
-        if (allow_when_stopped == false && Stopped == true) {
-            // Stopped can be set either during error states (thermal error: cannot continue), or
-            // when a printer-initiated action is processed. In such case the printer will send to
-            // the host an action, but cannot know if the action has been processed while new
-            // commands are being sent. In this situation we just drop the command while issuing
-            // periodic "busy" messages in the main loop. Since we're not incrementing the received
-            // line number, a request for resend will happen (if necessary), ensuring we don't skip
-            // commands whenever Stopped is cleared and processing resumes.
+            SERIAL_ERROR_START;
+            SERIAL_ERRORRPGM(_n("No Line Number with checksum, Last Line: "));////MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM
+            SERIAL_ERRORLN(gcode_LastN);
+			FlushSerialRequestResend();
            serial_count = 0;
            return;
        }
+        if ((strchr_pointer = strchr(cmdbuffer+bufindw+CMDHDRSIZE, 'G')) != NULL) {
+              if (! IS_SD_PRINTING) {
+                      usb_printing_counter = 10;
+                      is_usb_printing = true;
+              }
+            if (Stopped == true) {
+                int gcode = strtol(strchr_pointer+1, NULL, 10);
+                if (gcode >= 0 && gcode <= 3) {
+                    SERIAL_ERRORLNRPGM(MSG_ERR_STOPPED);
+                    LCD_MESSAGERPGM(_T(MSG_STOPPED));
+                }
+            }
+        } // end of 'G' command

-        // Command is complete: store the current line into buffer, move to the next line.
-
+        //If command was e-stop process now
+        if(strcmp(cmdbuffer+bufindw+CMDHDRSIZE, "M112") == 0)
+          kill(MSG_M112_KILL, 2);
+        
+        // Store the current line into buffer, move to the next line.
 		// Store type of entry
-        cmdbuffer[bufindw] = gcode_N >= 0 ? CMDBUFFER_CURRENT_TYPE_USB_WITH_LINENR : CMDBUFFER_CURRENT_TYPE_USB;
-
+        cmdbuffer[bufindw] = gcode_N ? CMDBUFFER_CURRENT_TYPE_USB_WITH_LINENR : CMDBUFFER_CURRENT_TYPE_USB;
 #ifdef CMDBUFFER_DEBUG
        SERIAL_ECHO_START;
        SERIAL_ECHOPGM("Storing a command line to buffer: ");
-        SERIAL_ECHO(cmd_start);
+        SERIAL_ECHO(cmdbuffer+bufindw+CMDHDRSIZE);
        SERIAL_ECHOLNPGM("");
 #endif /* CMDBUFFER_DEBUG */
-
-        // Store the command itself (without line number or checksum)
-        size_t cmd_len;
-        if (cmd_head == cmd_start)
-            cmd_len = strlen(cmd_start) + 1;
-        else {
-            // strip the line number
-            cmd_len = 0;
-            do { cmd_head[cmd_len] = cmd_start[cmd_len]; }
-            while (cmd_head[cmd_len++]);
-        }
-        bufindw += cmd_len + CMDHDRSIZE;
+        bufindw += strlen(cmdbuffer+bufindw+CMDHDRSIZE) + (1 + CMDHDRSIZE);
        if (bufindw == sizeof(cmdbuffer))
            bufindw = 0;
        ++ buflen;
-
-        // Update the processed gcode line
-        if (gcode_N >= 0)
-            gcode_LastN = gcode_N;
-
 #ifdef CMDBUFFER_DEBUG
        SERIAL_ECHOPGM("Number of commands in the buffer: ");
        SERIAL_ECHO(buflen);
@ -538,7 +524,7 @@ void get_command()
      serial_count = 0; //clear buffer
      // Don't call cmdqueue_could_enqueue_back if there are no characters waiting
      // in the queue, as this function will reserve the memory.
-      if (MYSERIAL.available() == 0 || ! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1))
+      if (MYSERIAL.available() == 0 || ! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1, true))
          return;
    } // end of "end of line" processing
    else {
@ -546,20 +532,28 @@ void get_command()
      if(serial_char == ';') comment_mode = true;
      if(!comment_mode) cmdbuffer[bufindw+CMDHDRSIZE+serial_count++] = serial_char;
    }
-    #ifdef ENABLE_MEATPACK
-     }
-    #endif
  } // end of serial line processing loop

-    if (serial_count > 0 && serialTimeoutTimer.expired(farm_mode ? 800 : 2000)) {
-        comment_mode = false;
-        serial_count = 0;
-        SERIAL_ECHOLNPGM("RX timeout");
-        return;
+    if(farm_mode){
+        TimeNow = _millis();
+        if ( ((TimeNow - TimeSent) > 800) && (serial_count > 0) ) {
+            cmdbuffer[bufindw+serial_count+CMDHDRSIZE] = 0;
+            
+            bufindw += strlen(cmdbuffer+bufindw+CMDHDRSIZE) + (1 + CMDHDRSIZE);
+            if (bufindw == sizeof(cmdbuffer))
+                bufindw = 0;
+            ++ buflen;
+            
+            serial_count = 0;
+            
+            SERIAL_ECHOPGM("TIMEOUT:");
+            //memset(cmdbuffer, 0 , sizeof(cmdbuffer));
+            return;
+        }
    }

  #ifdef SDSUPPORT
-  if(!card.sdprinting || !card.isFileOpen() || serial_count!=0){
+  if(!card.sdprinting || serial_count!=0){
    // If there is a half filled buffer from serial line, wait until return before
    // continuing with the serial line.
     return;
@ -581,14 +575,15 @@ void get_command()
  sd_count.value = 0;
  // Reads whole lines from the SD card. Never leaves a half-filled line in the cmdbuffer.
  while( !card.eof() && !stop_buffering) {
-    int16_t n=card.getFilteredGcodeChar();
+    int16_t n=card.get();
    char serial_char = (char)n;
-    if( serial_char == '\n'
-     || serial_char == '\r'
-     || serial_char == '#'
-     || serial_count >= (MAX_CMD_SIZE - 1)
-     || n==-1
-    ){
+    if(serial_char == '\n' ||
+       serial_char == '\r' ||
+       ((serial_char == '#' || serial_char == ':') && comment_mode == false) ||
+       serial_count >= (MAX_CMD_SIZE - 1) || n==-1)
+    {
+      if(card.eof()) break;
+
      if(serial_char=='#')
        stop_buffering=true;

@ -596,14 +591,15 @@ void get_command()
      {
        // This is either an empty line, or a line with just a comment.
        // Continue to the following line, and continue accumulating the number of bytes
-        // read from the sdcard into sd_count,
-        // so that the length of the already read empty lines and comments will be added
-        // to the following non-empty line.
-        return; // prevent cycling indefinitely - let manage_heaters do their job
+        // read from the sdcard into sd_count, 
+        // so that the lenght of the already read empty lines and comments will be added
+        // to the following non-empty line. 
+        comment_mode = false;
+        continue; //if empty line
      }
      // The new command buffer could be updated non-atomically, because it is not yet considered
      // to be inside the active queue.
-      sd_count.value = card.get_sdpos() - sdpos_atomic;
+      sd_count.value = (card.get_sdpos()+1) - sdpos_atomic;
      cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_SDCARD;
      cmdbuffer[bufindw+1] = sd_count.lohi.lo;
      cmdbuffer[bufindw+2] = sd_count.lohi.hi;
@ -615,10 +611,10 @@ void get_command()
 //      MYSERIAL.print(sd_count.value, DEC);
 //      SERIAL_ECHOPGM(") ");
 //      SERIAL_ECHOLN(cmdbuffer+bufindw+CMDHDRSIZE);
-//      SERIAL_ECHOPGM("cmdbuffer:");
-//      MYSERIAL.print(cmdbuffer);
-//      SERIAL_ECHOPGM("buflen:");
-//      MYSERIAL.print(buflen+1);
+//    SERIAL_ECHOPGM("cmdbuffer:");
+//    MYSERIAL.print(cmdbuffer);
+//    SERIAL_ECHOPGM("buflen:");
+//    MYSERIAL.print(buflen+1);
      sd_count.value = 0;

      cli();
@ -628,24 +624,21 @@ void get_command()
      // or a 115200 Bd serial line receive interrupt, which will not trigger faster than 12kHz.
      ++ buflen;
      bufindw += len;
-      sdpos_atomic = card.get_sdpos();
+      sdpos_atomic = card.get_sdpos()+1;
      if (bufindw == sizeof(cmdbuffer))
          bufindw = 0;
      sei();

      comment_mode = false; //for new command
      serial_count = 0; //clear buffer
-
-      if(card.eof()) break;
-
      // The following line will reserve buffer space if available.
-      if (! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1))
+      if (! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1, true))
          return;
    }
    else
    {
-        // there are no comments coming from the filtered file
-        cmdbuffer[bufindw+CMDHDRSIZE+serial_count++] = serial_char;
+      if(serial_char == ';') comment_mode = true;
+      else if(!comment_mode) cmdbuffer[bufindw+CMDHDRSIZE+serial_count++] = serial_char;
    }
  }
  if(card.eof())
@ -656,28 +649,27 @@ void get_command()
      // cleared by printingHasFinished after peforming all remaining moves.
      if(!cmdqueue_calc_sd_length())
      {
-          // queue is complete, but before we process EOF commands prevent
-          // re-entry by disabling SD processing from any st_synchronize call
-          card.closefile();
-
          SERIAL_PROTOCOLLNRPGM(_n("Done printing file"));////MSG_FILE_PRINTED
+          stoptime=_millis();
          char time[30];
-          uint32_t t = print_job_timer.duration() / 60;
+          unsigned long t=(stoptime-starttime-pause_time)/1000;
+          pause_time = 0;
          int hours, minutes;
-          minutes = t % 60;
-          hours = t / 60;
-          save_statistics();
+          minutes=(t/60)%60;
+          hours=t/60/60;
+          save_statistics(total_filament_used, t);
          sprintf_P(time, PSTR("%i hours %i minutes"),hours, minutes);
          SERIAL_ECHO_START;
          SERIAL_ECHOLN(time);
-#ifndef SHOW_FILENAME_AFTER_FINISH
          lcd_setstatus(time);
-#endif //SHOW_FILENAME_AFTER_FINISH
          card.printingHasFinished();
          card.checkautostart(true);

          if (farm_mode)
-            prusa_statistics(6);
+          {
+              prusa_statistics(6);
+              lcd_commands_type = LcdCommands::FarmModeConfirm;
+          }
      }
  }

--- a/Firmware/cmdqueue.h
+++ b/Firmware/cmdqueue.h
@ -2,11 +2,13 @@
 #define CMDQUEUE_H

 #include "Marlin.h"
+#include "language.h"
+

 // String circular buffer. Commands may be pushed to the buffer from both sides:
-// Chained commands will be pushed to the front, interactive (from LCD menu)
+// Chained commands will be pushed to the front, interactive (from LCD menu) 
 // and printing commands (from serial line or from SD card) are pushed to the tail.
-// First character of each entry indicates the type of the entry:
+// First character of each entry indicates the type of the entry: 
 #define CMDBUFFER_CURRENT_TYPE_UNKNOWN  0
 // Command in cmdbuffer was sent over USB.
 #define CMDBUFFER_CURRENT_TYPE_USB      1
@ -16,8 +18,8 @@
 #define CMDBUFFER_CURRENT_TYPE_UI       3
 // Command in cmdbuffer was generated by another G-code.
 #define CMDBUFFER_CURRENT_TYPE_CHAINED  4
-// Command has been processed and its SD card length has been possibly pushed
-// to the planner queue, but not yet removed from the cmdqueue.
+// Command has been processed and its SD card length has been possibly pushed 
+// to the planner queue, but not yet removed from the cmdqueue. 
 // This is a temporary state to reduce stepper interrupt locking time.
 #define CMDBUFFER_CURRENT_TYPE_TO_BE_REMOVED 5
 //Command in cmdbuffer was sent over USB and contains line number
@ -33,7 +35,6 @@ extern char cmdbuffer[BUFSIZE * (MAX_CMD_SIZE + 1) + CMDBUFFER_RESERVE_FRONT];
 extern size_t bufindr;
 extern int buflen;
 extern bool cmdbuffer_front_already_processed;
-extern bool cmdqueue_serial_disabled;

 // Type of a command, which is to be executed right now.
 #define CMDBUFFER_CURRENT_TYPE   (cmdbuffer[bufindr])
@ -47,10 +48,15 @@ extern bool cmdqueue_serial_disabled;
 extern uint32_t sdpos_atomic;

 extern int serial_count;
-extern bool comment_mode;
+extern boolean comment_mode;
 extern char *strchr_pointer;

+extern unsigned long TimeSent;
+extern unsigned long TimeNow;
+
+extern long gcode_N;
 extern long gcode_LastN;
+extern long Stopped_gcode_LastN;

 extern bool cmdqueue_pop_front();
 extern void cmdqueue_reset();
@ -59,31 +65,30 @@ extern void cmdqueue_dump_to_serial_single_line(int nr, const char *p);
 extern void cmdqueue_dump_to_serial();
 #endif /* CMDBUFFER_DEBUG */
 extern bool cmd_buffer_empty();
-
-/// @brief Variant of enquecommand which accepts a format string
-/// @param fmt a format string residing in PROGMEM
-void enquecommandf_P(const char *fmt, ...);
-extern void enquecommand(const char *cmd, bool from_progmem = false);
-extern void enquecommand_front(const char *cmd, bool from_progmem = false);
+extern void enquecommand(const char *cmd, bool from_progmem);
+extern void enquecommand_front(const char *cmd, bool from_progmem);
 extern void repeatcommand_front();
+extern bool is_buffer_empty();
 extern void get_command();
 extern uint16_t cmdqueue_calc_sd_length();

-
-#if defined(__cplusplus)
-extern "C" {
-#endif
-    extern double strtod_noE(const char* nptr, char** endptr);
-#if defined(__cplusplus)
-}
-#endif
-
 // Return True if a character was found
 static inline bool    code_seen(char code) { return (strchr_pointer = strchr(CMDBUFFER_CURRENT_STRING, code)) != NULL; }
-static inline bool    code_seen_P(const char *code_PROGMEM) { return (strchr_pointer = strstr_P(CMDBUFFER_CURRENT_STRING, code_PROGMEM)) != NULL; }
-static inline float   code_value()      { return strtod_noE(strchr_pointer+1, NULL);}
+static inline bool    code_seen(const char *code) { return (strchr_pointer = strstr(CMDBUFFER_CURRENT_STRING, code)) != NULL; }
+static inline float   code_value()      { return strtod(strchr_pointer+1, NULL);}
 static inline long    code_value_long()    { return strtol(strchr_pointer+1, NULL, 10); }
 static inline int16_t code_value_short()   { return int16_t(strtol(strchr_pointer+1, NULL, 10)); };
 static inline uint8_t code_value_uint8()   { return uint8_t(strtol(strchr_pointer+1, NULL, 10)); };

+static inline float code_value_float()
+{
+    char* e = strchr(strchr_pointer, 'E');
+    if (!e) return strtod(strchr_pointer + 1, NULL);
+    *e = 0;
+    float ret = strtod(strchr_pointer + 1, NULL);
+    *e = 'E';
+    return ret;
+}
+
+
 #endif //CMDQUEUE_H
--- a/Firmware/config.h
+++ b/Firmware/config.h
@ -2,24 +2,28 @@
 #define _CONFIG_H


-#include "Configuration_var.h"
+#include "Configuration_prusa.h"
 #include "pins.h"

+#if (defined(VOLT_IR_PIN) && defined(IR_SENSOR))
+# define IR_SENSOR_ANALOG
+#endif

 //ADC configuration
-#if defined(FILAMENT_SENSOR) && (FILAMENT_SENSOR_TYPE == FSENSOR_IR_ANALOG)
-#define ADC_CHAN_MSK      0b0000001101011111 //used AD channels bit mask (0,1,2,3,4,6,8,9)
-#define ADC_DIDR_MSK      0b0000001001011111 //AD channels DIDR mask (1 ~ disabled digital input)
-#define ADC_CHAN_CNT      8         //number of used channels)
-#else
+#ifndef IR_SENSOR_ANALOG
 #define ADC_CHAN_MSK      0b0000001001011111 //used AD channels bit mask (0,1,2,3,4,6,9)
 #define ADC_DIDR_MSK      0b0000001001011111 //AD channels DIDR mask (1 ~ disabled digital input)
 #define ADC_CHAN_CNT      7         //number of used channels)
-#endif
+#else //!IR_SENSOR_ANALOG
+#define ADC_CHAN_MSK      0b0000001101011111 //used AD channels bit mask (0,1,2,3,4,6,8,9)
+#define ADC_DIDR_MSK      0b0000001001011111 //AD channels DIDR mask (1 ~ disabled digital input)
+#define ADC_CHAN_CNT      8         //number of used channels)
+#endif //!IR_SENSOR_ANALOG
 #define ADC_OVRSAMPL      16        //oversampling multiplier
-#define ADC_CALLBACK      adc_callback //callback function ()
+#define ADC_CALLBACK      adc_ready //callback function ()

 //SWI2C configuration
+#define SWI2C
 //#define SWI2C_SDA         20 //SDA on P3
 //#define SWI2C_SCL         21 //SCL on P3
 #define SWI2C_A8
@ -27,23 +31,13 @@
 #define SWI2C_TMO         2048 //2048 cycles timeout

 //PAT9125 configuration
-#ifdef SWI2C_SCL
-#define PAT9125_SWI2C   // software I2C mode
-#else
-#define PAT9125_I2C     // hardware I2C mode
-#endif
-
+#define PAT9125_SWI2C
 #define PAT9125_I2C_ADDR  0x75  //ID=LO
 //#define PAT9125_I2C_ADDR  0x79  //ID=HI
 //#define PAT9125_I2C_ADDR  0x73  //ID=NC
 #define PAT9125_XRES      0
 #define PAT9125_YRES      240                   // maximum resolution (5*X cpi)
-#define PAT9125_YRES_MM   (5*PAT9125_YRES/25.4) // counts per mm
-#define PAT9125_INVERT_X  0 //1 means flipped
-#define PAT9125_INVERT_Y  1 //1 means flipped
-#define PAT9125_SWAP_XY   0 //X is Y and Y is X
-#define PAT9125_12B_RES   1 //8bit or 12bit signed motion data
-#define PAT9125_NEW_INIT  1 //set to 1 to use the magic sequence provided by pixart.
+#define PAT9124_YRES_MM   (5*PAT9125_YRES/25.4) // counts per mm

 //SM4 configuration
 #define SM4_DEFDELAY      500       //default step delay [us]
@ -55,73 +49,19 @@
 #define TMC2130_SPCR           SPI_SPCR(TMC2130_SPI_RATE, 1, 1, 1, 0)
 #define TMC2130_SPSR           SPI_SPSR(TMC2130_SPI_RATE)

-// This is set by the cmake build to be able to take control of
-// the language flag, without breaking existing build mechanisms.
-#ifndef CMAKE_CONTROL
+//W25X20CL configuration
+//pinout:
+#define W25X20CL_PIN_CS        32
+//spi:
+#define W25X20CL_SPI_RATE      0 // fosc/4 = 4MHz
+#define W25X20CL_SPCR          SPI_SPCR(W25X20CL_SPI_RATE, 1, 1, 1, 0)
+#define W25X20CL_SPSR          SPI_SPSR(W25X20CL_SPI_RATE)
+
 //LANG - Multi-language support
 //#define LANG_MODE              0 // primary language only
 #define LANG_MODE              1 // sec. language support
-#endif

-#define LANG_SIZE_RESERVED     0x3500 // reserved space for secondary language (13568 bytes).
-                                      // 0x3D00 Maximum 15616 bytes as it depends on xflash_layout.h
-                                      // 16 Languages max. per group including stock
+#define LANG_SIZE_RESERVED     0x3000 // reserved space for secondary language (12288 bytes)

-#if (LANG_SIZE_RESERVED % 256)
-  #error "LANG_SIZE_RESERVED should be a multiple of a page size"
-#endif

-//Community language support
-#define COMMUNITY_LANG_GROUP 1
-
-#if (COMMUNITY_LANG_GROUP == 1)
-#define COMMUNITY_LANG_GROUP1_NL // Community Dutch language
-#define COMMUNITY_LANG_GROUP1_RO // Community Romanian language
-#define COMMUNITY_LANG_GROUP1_HU // Community Hungarian language
-#define COMMUNITY_LANG_GROUP1_HR // Community Croatian language
-#define COMMUNITY_LANG_GROUP1_SK // Community Slovak language
-#define COMMUNITY_LANG_GROUP1_SV // Community Swedish language
-#define COMMUNITY_LANG_GROUP1_NO // Community Norwegian language
-//#define COMMUNITY_LANG_GROUP1_DA // Community Danish language
-//#define COMMUNITY_LANG_GROUP1_SL // Community Slovanian language
-//#define COMMUNITY_LANG_GROUP1_LB // Community Luxembourgish language
-#endif //COMMUNITY_LANG_GROUP 1
-
-#if (COMMUNITY_LANG_GROUP == 2)
-#define COMMUNITY_LANG_GROUP2_LT // Community Lithuanian language
-//#define COMMUNITY_LANG_GROUP1_QR // Community new language //..use this as a template and replace 'QR'
-#endif //COMMUNITY_LANG_GROUP 2
-
-#if (COMMUNITY_LANG_GROUP >=1 )
-#define COMMUNITY_LANGUAGE_SUPPORT
-#endif
-
-// Sanity checks for correct configuration of XFLASH_DUMP options
-#if defined(XFLASH_DUMP) && !defined(XFLASH)
-#error "XFLASH_DUMP requires XFLASH support"
-#endif
-#if (defined(MENU_DUMP) || defined(EMERGENCY_DUMP)) && !defined(XFLASH_DUMP)
-#error "MENU_DUMP and EMERGENCY_DUMP require XFLASH_DUMP"
-#endif
-
-// Support for serial dumps is mutually exclusive with XFLASH_DUMP features
-#if defined(EMERGENCY_DUMP) && defined(EMERGENCY_SERIAL_DUMP)
-#error "EMERGENCY_DUMP and EMERGENCY_SERIAL_DUMP are mutually exclusive"
-#endif
-#if defined(MENU_DUMP) && defined(MENU_SERIAL_DUMP)
-#error "MENU_DUMP and MENU_SERIAL_DUMP are mutually exclusive"
-#endif
-
-// Reduce internal duplication
-#if defined(EMERGENCY_DUMP) || defined(EMERGENCY_SERIAL_DUMP)
-#define EMERGENCY_HANDLERS
-#endif
-
-//FARM_MODE
-#if ( LANG_MODE == 0 ) && defined(XFLASH) //Save resources on EINSY and disable FARM_MODE on multi-language version
-#define PRUSA_FARM
-#endif //PRUSA_FARM only in english on EINSYs
-#ifndef XFLASH //enable FARM_MODE on miniRAMBo boards
-#define PRUSA_FARM
-#endif
 #endif //_CONFIG_H
--- a/Firmware/conv2str.cpp
+++ b/Firmware/conv2str.cpp
@ -0,0 +1,292 @@
+//conv2str.cpp - Float conversion utilities
+
+#include "conv2str.h"
+#include <stdlib.h>
+
+
+//  convert float to string with +123.4 format
+char conv[8];
+
+char *ftostr3(const float &x)
+{
+  return itostr3((int)x);
+}
+
+char *itostr2(const uint8_t &x)
+{
+  //sprintf(conv,"%5.1f",x);
+  int xx = x;
+  conv[0] = (xx / 10) % 10 + '0';
+  conv[1] = (xx) % 10 + '0';
+  conv[2] = 0;
+  return conv;
+}
+
+// Convert float to string with 123.4 format, dropping sign
+char *ftostr31(const float &x)
+{
+  int xx = x * 10;
+  conv[0] = (xx >= 0) ? '+' : '-';
+  xx = abs(xx);
+  conv[1] = (xx / 1000) % 10 + '0';
+  conv[2] = (xx / 100) % 10 + '0';
+  conv[3] = (xx / 10) % 10 + '0';
+  conv[4] = '.';
+  conv[5] = (xx) % 10 + '0';
+  conv[6] = 0;
+  return conv;
+}
+
+// Convert float to string with 123.4 format
+char *ftostr31ns(const float &x)
+{
+  int xx = x * 10;
+  //conv[0]=(xx>=0)?'+':'-';
+  xx = abs(xx);
+  conv[0] = (xx / 1000) % 10 + '0';
+  conv[1] = (xx / 100) % 10 + '0';
+  conv[2] = (xx / 10) % 10 + '0';
+  conv[3] = '.';
+  conv[4] = (xx) % 10 + '0';
+  conv[5] = 0;
+  return conv;
+}
+
+char *ftostr32(const float &x)
+{
+  long xx = x * 100;
+  if (xx >= 0)
+    conv[0] = (xx / 10000) % 10 + '0';
+  else
+    conv[0] = '-';
+  xx = abs(xx);
+  conv[1] = (xx / 1000) % 10 + '0';
+  conv[2] = (xx / 100) % 10 + '0';
+  conv[3] = '.';
+  conv[4] = (xx / 10) % 10 + '0';
+  conv[5] = (xx) % 10 + '0';
+  conv[6] = 0;
+  return conv;
+}
+
+//// Convert float to rj string with 123.45 format
+char *ftostr32ns(const float &x) {
+	long xx = abs(x);
+	conv[0] = xx >= 10000 ? (xx / 10000) % 10 + '0' : ' ';
+	conv[1] = xx >= 1000 ? (xx / 1000) % 10 + '0' : ' ';
+	conv[2] = xx >= 100 ? (xx / 100) % 10 + '0' : '0';
+	conv[3] = '.';
+	conv[4] = (xx / 10) % 10 + '0';
+	conv[5] = xx % 10 + '0';
+	return conv;
+}
+
+
+// Convert float to string with 1.234 format
+char *ftostr43(const float &x, uint8_t offset)
+{
+  const size_t maxOffset = sizeof(conv)/sizeof(conv[0]) - 6;
+  if (offset>maxOffset) offset = maxOffset;
+  long xx = x * 1000;
+  if (xx >= 0)
+    conv[offset] = (xx / 1000) % 10 + '0';
+  else
+    conv[offset] = '-';
+  xx = abs(xx);
+  conv[offset + 1] = '.';
+  conv[offset + 2] = (xx / 100) % 10 + '0';
+  conv[offset + 3] = (xx / 10) % 10 + '0';
+  conv[offset + 4] = (xx) % 10 + '0';
+  conv[offset + 5] = 0;
+  return conv;
+}
+
+//Float to string with 1.23 format
+char *ftostr12ns(const float &x)
+{
+  long xx = x * 100;
+
+  xx = abs(xx);
+  conv[0] = (xx / 100) % 10 + '0';
+  conv[1] = '.';
+  conv[2] = (xx / 10) % 10 + '0';
+  conv[3] = (xx) % 10 + '0';
+  conv[4] = 0;
+  return conv;
+}
+
+//Float to string with 1.234 format
+char *ftostr13ns(const float &x)
+{
+    long xx = x * 1000;
+    if (xx >= 0)
+        conv[0] = ' ';
+    else
+        conv[0] = '-';
+    xx = abs(xx);
+    conv[1] = (xx / 1000) % 10 + '0';
+    conv[2] = '.';
+    conv[3] = (xx / 100) % 10 + '0';
+    conv[4] = (xx / 10) % 10 + '0';
+    conv[5] = (xx) % 10 + '0';
+    conv[6] = 0;
+    return conv;
+}
+
+//  convert float to space-padded string with -_23.4_ format
+char *ftostr32sp(const float &x) {
+  long xx = abs(x * 100);
+  uint8_t dig;
+
+  if (x < 0) { // negative val = -_0
+    conv[0] = '-';
+    dig = (xx / 1000) % 10;
+    conv[1] = dig ? '0' + dig : ' ';
+  }
+  else { // positive val = __0
+    dig = (xx / 10000) % 10;
+    if (dig) {
+      conv[0] = '0' + dig;
+      conv[1] = '0' + (xx / 1000) % 10;
+    }
+    else {
+      conv[0] = ' ';
+      dig = (xx / 1000) % 10;
+      conv[1] = dig ? '0' + dig : ' ';
+    }
+  }
+
+  conv[2] = '0' + (xx / 100) % 10; // lsd always
+
+  dig = xx % 10;
+  if (dig) { // 2 decimal places
+    conv[5] = '0' + dig;
+    conv[4] = '0' + (xx / 10) % 10;
+    conv[3] = '.';
+  }
+  else { // 1 or 0 decimal place
+    dig = (xx / 10) % 10;
+    if (dig) {
+      conv[4] = '0' + dig;
+      conv[3] = '.';
+    }
+    else {
+      conv[3] = conv[4] = ' ';
+    }
+    conv[5] = ' ';
+  }
+  conv[6] = '\0';
+  return conv;
+}
+
+char *itostr31(const int &xx)
+{
+  conv[0] = (xx >= 0) ? '+' : '-';
+  conv[1] = (xx / 1000) % 10 + '0';
+  conv[2] = (xx / 100) % 10 + '0';
+  conv[3] = (xx / 10) % 10 + '0';
+  conv[4] = '.';
+  conv[5] = (xx) % 10 + '0';
+  conv[6] = 0;
+  return conv;
+}
+
+// Convert int to rj string with 123 or -12 format
+char *itostr3(const int &x)
+{
+  int xx = x;
+  if (xx < 0) {
+    conv[0] = '-';
+    xx = -xx;
+  } else if (xx >= 100)
+    conv[0] = (xx / 100) % 10 + '0';
+  else
+    conv[0] = ' ';
+  if (xx >= 10)
+    conv[1] = (xx / 10) % 10 + '0';
+  else
+    conv[1] = ' ';
+  conv[2] = (xx) % 10 + '0';
+  conv[3] = 0;
+  return conv;
+}
+
+// Convert int to lj string with 123 format
+char *itostr3left(const int &xx)
+{
+  if (xx >= 100)
+  {
+    conv[0] = (xx / 100) % 10 + '0';
+    conv[1] = (xx / 10) % 10 + '0';
+    conv[2] = (xx) % 10 + '0';
+    conv[3] = 0;
+  }
+  else if (xx >= 10)
+  {
+    conv[0] = (xx / 10) % 10 + '0';
+    conv[1] = (xx) % 10 + '0';
+    conv[2] = 0;
+  }
+  else
+  {
+    conv[0] = (xx) % 10 + '0';
+    conv[1] = 0;
+  }
+  return conv;
+}
+
+// Convert int to rj string with 1234 format
+char *itostr4(const int &xx) {
+  conv[0] = xx >= 1000 ? (xx / 1000) % 10 + '0' : ' ';
+  conv[1] = xx >= 100 ? (xx / 100) % 10 + '0' : ' ';
+  conv[2] = xx >= 10 ? (xx / 10) % 10 + '0' : ' ';
+  conv[3] = xx % 10 + '0';
+  conv[4] = 0;
+  return conv;
+}
+
+// Convert float to rj string with 12345 format
+char *ftostr5(const float &x) {
+  long xx = abs(x);
+  conv[0] = xx >= 10000 ? (xx / 10000) % 10 + '0' : ' ';
+  conv[1] = xx >= 1000 ? (xx / 1000) % 10 + '0' : ' ';
+  conv[2] = xx >= 100 ? (xx / 100) % 10 + '0' : ' ';
+  conv[3] = xx >= 10 ? (xx / 10) % 10 + '0' : ' ';
+  conv[4] = xx % 10 + '0';
+  conv[5] = 0;
+  return conv;
+}
+
+// Convert float to string with +1234.5 format
+char *ftostr51(const float &x)
+{
+  long xx = x * 10;
+  conv[0] = (xx >= 0) ? '+' : '-';
+  xx = abs(xx);
+  conv[1] = (xx / 10000) % 10 + '0';
+  conv[2] = (xx / 1000) % 10 + '0';
+  conv[3] = (xx / 100) % 10 + '0';
+  conv[4] = (xx / 10) % 10 + '0';
+  conv[5] = '.';
+  conv[6] = (xx) % 10 + '0';
+  conv[7] = 0;
+  return conv;
+}
+
+// Convert float to string with +123.45 format
+char *ftostr52(const float &x)
+{
+  long xx = x * 100;
+  conv[0] = (xx >= 0) ? '+' : '-';
+  xx = abs(xx);
+  conv[1] = (xx / 10000) % 10 + '0';
+  conv[2] = (xx / 1000) % 10 + '0';
+  conv[3] = (xx / 100) % 10 + '0';
+  conv[4] = '.';
+  conv[5] = (xx / 10) % 10 + '0';
+  conv[6] = (xx) % 10 + '0';
+  conv[7] = 0;
+  return conv;
+}
+
+
--- a/Firmware/conv2str.h
+++ b/Firmware/conv2str.h
@ -0,0 +1,28 @@
+//conv2str.h - Float conversion utilities
+#ifndef _CONV2STR_H
+#define _CONV2STR_H
+
+#include <inttypes.h>
+
+
+char *itostr2(const uint8_t &x);
+char *itostr31(const int &xx);
+char *itostr3(const int &xx);
+char *itostr3left(const int &xx);
+char *itostr4(const int &xx);
+
+char *ftostr3(const float &x);
+char *ftostr31ns(const float &x); // float to string without sign character
+char *ftostr31(const float &x);
+char *ftostr32(const float &x);
+char *ftostr32ns(const float &x);
+char *ftostr43(const float &x, uint8_t offset = 0);
+char *ftostr12ns(const float &x);
+char *ftostr13ns(const float &x);
+char *ftostr32sp(const float &x); // remove zero-padding from ftostr32
+char *ftostr5(const float &x);
+char *ftostr51(const float &x);
+char *ftostr52(const float &x);
+
+
+#endif //_CONV2STR_H
--- a/Firmware/eeprom.cpp
+++ b/Firmware/eeprom.cpp
@ -8,69 +8,95 @@
 #include <avr/eeprom.h>
 #include <stdint.h>

+
+#include "language.h"
+
+#if 0
+template <typename T>
+static T eeprom_read(T *address);
+
+template<>
+char eeprom_read<char>(char *address)
+{
+    return eeprom_read_byte(reinterpret_cast<uint8_t*>(address));
+}
+#endif
+
+template <typename T>
+static void eeprom_write(T *address, T value);
+
+template<>
+void eeprom_write<char>(char *addres, char value)
+{
+    eeprom_write_byte(reinterpret_cast<uint8_t*>(addres), static_cast<uint8_t>(value));
+}
+
+
+template <typename T>
+static bool eeprom_is_uninitialized(T *address);
+
+template <>
+bool eeprom_is_uninitialized<char>(char *address)
+{
+    return (0xff == eeprom_read_byte(reinterpret_cast<uint8_t*>(address)));
+}
+
+bool eeprom_is_sheet_initialized(uint8_t sheet_num)
+{
+  return (0xffff != eeprom_read_word(reinterpret_cast<uint16_t*>(&(EEPROM_Sheets_base->
+  s[sheet_num].z_offset))));
+}
+
 void eeprom_init()
 {
-    eeprom_init_default_byte((uint8_t*)EEPROM_POWER_COUNT, 0);
-    eeprom_init_default_byte((uint8_t*)EEPROM_CRASH_COUNT_X, 0);
-    eeprom_init_default_byte((uint8_t*)EEPROM_CRASH_COUNT_Y, 0);
-    eeprom_init_default_byte((uint8_t*)EEPROM_FERROR_COUNT, 0);
-    eeprom_init_default_word((uint16_t*)EEPROM_POWER_COUNT_TOT, 0);
-    eeprom_init_default_word((uint16_t*)EEPROM_CRASH_COUNT_X_TOT, 0);
-    eeprom_init_default_word((uint16_t*)EEPROM_CRASH_COUNT_Y_TOT, 0);
-    eeprom_init_default_word((uint16_t*)EEPROM_FERROR_COUNT_TOT, 0);
+    if (eeprom_read_byte((uint8_t*)EEPROM_POWER_COUNT) == 0xff) eeprom_write_byte((uint8_t*)EEPROM_POWER_COUNT, 0);
+    if (eeprom_read_byte((uint8_t*)EEPROM_CRASH_COUNT_X) == 0xff) eeprom_write_byte((uint8_t*)EEPROM_CRASH_COUNT_X, 0);
+    if (eeprom_read_byte((uint8_t*)EEPROM_CRASH_COUNT_Y) == 0xff) eeprom_write_byte((uint8_t*)EEPROM_CRASH_COUNT_Y, 0);
+    if (eeprom_read_byte((uint8_t*)EEPROM_FERROR_COUNT) == 0xff) eeprom_write_byte((uint8_t*)EEPROM_FERROR_COUNT, 0);
+    if (eeprom_read_word((uint16_t*)EEPROM_POWER_COUNT_TOT) == 0xffff) eeprom_write_word((uint16_t*)EEPROM_POWER_COUNT_TOT, 0);
+    if (eeprom_read_word((uint16_t*)EEPROM_CRASH_COUNT_X_TOT) == 0xffff) eeprom_write_word((uint16_t*)EEPROM_CRASH_COUNT_X_TOT, 0);
+    if (eeprom_read_word((uint16_t*)EEPROM_CRASH_COUNT_Y_TOT) == 0xffff) eeprom_write_word((uint16_t*)EEPROM_CRASH_COUNT_Y_TOT, 0);
+    if (eeprom_read_word((uint16_t*)EEPROM_FERROR_COUNT_TOT) == 0xffff) eeprom_write_word((uint16_t*)EEPROM_FERROR_COUNT_TOT, 0);

-    eeprom_init_default_word((uint16_t*)EEPROM_MMU_FAIL_TOT, 0);
-    eeprom_init_default_word((uint16_t*)EEPROM_MMU_LOAD_FAIL_TOT, 0);
-    eeprom_init_default_byte((uint8_t*)EEPROM_MMU_FAIL, 0);
-    eeprom_init_default_byte((uint8_t*)EEPROM_MMU_LOAD_FAIL, 0);
-    eeprom_init_default_dword((uint32_t*)EEPROM_MMU_MATERIAL_CHANGES, 0);
+    if (eeprom_read_word((uint16_t*)EEPROM_MMU_FAIL_TOT) == 0xffff) eeprom_update_word((uint16_t *)EEPROM_MMU_FAIL_TOT, 0);
+    if (eeprom_read_word((uint16_t*)EEPROM_MMU_LOAD_FAIL_TOT) == 0xffff) eeprom_update_word((uint16_t *)EEPROM_MMU_LOAD_FAIL_TOT, 0);
+    if (eeprom_read_byte((uint8_t*)EEPROM_MMU_FAIL) == 0xff) eeprom_update_byte((uint8_t *)EEPROM_MMU_FAIL, 0);
+    if (eeprom_read_byte((uint8_t*)EEPROM_MMU_LOAD_FAIL) == 0xff) eeprom_update_byte((uint8_t *)EEPROM_MMU_LOAD_FAIL, 0);
    if (eeprom_read_byte(&(EEPROM_Sheets_base->active_sheet)) == EEPROM_EMPTY_VALUE)
    {
-        eeprom_update_byte_notify(&(EEPROM_Sheets_base->active_sheet), 0);
+        eeprom_update_byte(&(EEPROM_Sheets_base->active_sheet), 0);
        // When upgrading from version older version (before multiple sheets were implemented in v3.8.0)
        // Sheet 1 uses the previous Live adjust Z (@EEPROM_BABYSTEP_Z)
        int last_babystep = eeprom_read_word((uint16_t *)EEPROM_BABYSTEP_Z);
        eeprom_update_word(reinterpret_cast<uint16_t *>(&(EEPROM_Sheets_base->s[0].z_offset)), last_babystep);
    }
+    
+    for (uint_least8_t i = 0; i < (sizeof(Sheets::s)/sizeof(Sheets::s[0])); ++i)
+    {
+        bool is_uninitialized = true;
+        for (uint_least8_t j = 0; j < (sizeof(Sheet::name)/sizeof(Sheet::name[0])); ++j)
+        {
+            if (!eeprom_is_uninitialized(&(EEPROM_Sheets_base->s[i].name[j]))) is_uninitialized = false;
+        }
+        if(is_uninitialized)
+        {
+            SheetName sheetName;
+            eeprom_default_sheet_name(i,sheetName);

-    // initialize the sheet names in eeprom
-    for (uint_least8_t i = 0; i < (sizeof(Sheets::s)/sizeof(Sheets::s[0])); i++) {
-        SheetName sheetName;
-        eeprom_default_sheet_name(i, sheetName);
-        eeprom_init_default_block(EEPROM_Sheets_base->s[i].name, (sizeof(Sheet::name)/sizeof(Sheet::name[0])), sheetName.c);
+            for (uint_least8_t a = 0; a < sizeof(Sheet::name); ++a){
+                eeprom_write(&(EEPROM_Sheets_base->s[i].name[a]), sheetName.c[a]);
+            }
+        }
    }
-
    if(!eeprom_is_sheet_initialized(eeprom_read_byte(&(EEPROM_Sheets_base->active_sheet))))
    {
        eeprom_switch_to_next_sheet();
    }
    check_babystep();

-    // initialize custom mendel name in eeprom
-    if (eeprom_read_byte((uint8_t*)EEPROM_CUSTOM_MENDEL_NAME) == EEPROM_EMPTY_VALUE) {
-        //SERIAL_ECHOLN("Init Custom Mendel Name");
-        eeprom_update_block_notify(CUSTOM_MENDEL_NAME, (uint8_t*)EEPROM_CUSTOM_MENDEL_NAME, sizeof(CUSTOM_MENDEL_NAME));
-    } //else SERIAL_ECHOLN("Found Custom Mendel Name");
-
 #ifdef PINDA_TEMP_COMP
-    eeprom_init_default_byte((uint8_t*)EEPROM_PINDA_TEMP_COMPENSATION, 0);
+if (eeprom_read_byte((uint8_t*)EEPROM_PINDA_TEMP_COMPENSATION) == 0xff) eeprom_update_byte((uint8_t *)EEPROM_PINDA_TEMP_COMPENSATION, 0);
 #endif //PINDA_TEMP_COMP
-
-    eeprom_init_default_dword((uint32_t*)EEPROM_JOB_ID, 0);
-    eeprom_init_default_dword((uint32_t*)EEPROM_TOTALTIME, 0);
-    eeprom_init_default_dword((uint32_t*)EEPROM_FILAMENTUSED, 0);
-    eeprom_init_default_byte((uint8_t*)EEPROM_MMU_CUTTER_ENABLED, 0);
-
-    eeprom_init_default_byte((uint8_t*)EEPROM_HEAT_BED_ON_LOAD_FILAMENT, 1);
-
-}
-
-void eeprom_adjust_bed_reset() {
-    eeprom_update_byte_notify((uint8_t*)EEPROM_BED_CORRECTION_VALID, 1);
-    eeprom_update_byte_notify((uint8_t*)EEPROM_BED_CORRECTION_LEFT, 0);
-    eeprom_update_byte_notify((uint8_t*)EEPROM_BED_CORRECTION_RIGHT, 0);
-    eeprom_update_byte_notify((uint8_t*)EEPROM_BED_CORRECTION_FRONT, 0);
-    eeprom_update_byte_notify((uint8_t*)EEPROM_BED_CORRECTION_REAR, 0);
 }

 //! @brief Get default sheet name for index
@ -81,10 +107,10 @@ void eeprom_adjust_bed_reset() {
 //! | 1     | Smooth2   |
 //! | 2     | Textur1   |
 //! | 3     | Textur2   |
-//! | 4     | Satin     |
-//! | 5     | NylonPA   |
-//! | 6     | Custom1   |
-//! | 7     | Custom2   |
+//! | 4     | Custom1   |
+//! | 5     | Custom2   |
+//! | 6     | Custom3   |
+//! | 7     | Custom4   |
 //!
 //! @param[in] index
 //! @param[out] sheetName
@ -100,23 +126,42 @@ void eeprom_default_sheet_name(uint8_t index, SheetName &sheetName)
    {
        strcpy_P(sheetName.c, PSTR("Textur"));
    }
-    else if (index < 5)
-    {
-        strcpy_P(sheetName.c, PSTR("Satin  "));
-    }
-    else if (index < 6)
-    {
-        strcpy_P(sheetName.c, PSTR("NylonPA"));
-    }
    else
    {
        strcpy_P(sheetName.c, PSTR("Custom"));
    }
-    if (index <4 || index >5)
+
+    switch (index)
    {
-        sheetName.c[6] = '0' + ((index % 2)+1);
-        sheetName.c[7] = '\0';
+    case 0:
+        sheetName.c[6] = '1';
+        break;
+    case 1:
+        sheetName.c[6] = '2';
+        break;
+    case 2:
+        sheetName.c[6] = '1';
+        break;
+    case 3:
+        sheetName.c[6] = '2';
+        break;
+    case 4:
+        sheetName.c[6] = '1';
+        break;
+    case 5:
+        sheetName.c[6] = '2';
+        break;
+    case 6:
+        sheetName.c[6] = '3';
+        break;
+    case 7:
+        sheetName.c[6] = '4';
+        break;
+    default:
+        break;
    }
+
+    sheetName.c[7] = '\0';
 }

 //! @brief Get next initialized sheet
@ -137,267 +182,10 @@ int8_t eeprom_next_initialized_sheet(int8_t sheet)
    return -1;
 }

-#ifdef DEBUG_EEPROM_CHANGES
-static void eeprom_byte_notify(uint8_t *dst, uint8_t previous_value, uint8_t value, bool write) {
-    printf_P(PSTR("EEPROMChng b %s %u %d -> %d\n"), write ? "write":"", dst , previous_value, value);
-}
-
-static void eeprom_word_notify(uint16_t *dst, uint16_t previous_value, uint16_t value, bool write) {
-    printf_P(PSTR("EEPROMChng w %s %u %u -> %u\n"), write ? "write":"", dst , previous_value, value);
-}
-
-static void eeprom_dword_notify(uint32_t *dst, uint32_t previous_value, uint32_t value, bool write) {
-    printf_P(PSTR("EEPROMChng d %s %u %x -> %x\n"), write ? "write":"", reinterpret_cast<const uint16_t>(dst) , previous_value, value);
-}
-
-static void eeprom_float_notify(float *dst, float previous_value, float value, bool write) {
-    printf_P(PSTR("EEPROMChng f %s %u %f -> %f\n"), write ? "write":"", reinterpret_cast<const uint16_t>(dst) , previous_value, value);
-}
-
-static void eeprom_block_notify(void *dst, const uint8_t *previous_values, const uint8_t *values, size_t size, bool write) {
-    for(size_t i = 0; i < size; ++i){
-        if (previous_values[i] != values[i] || write) {
-            printf_P(PSTR("EEPROMChng bl %s %u %x -> %x\n"), write ? "write":"", reinterpret_cast<const uint16_t>(dst) + i, previous_values[i], values[i]);
-        }
-    }
-}
-#endif //DEBUG_EEPROM_CHANGES
-
-#ifndef DEBUG_EEPROM_CHANGES
-void eeprom_write_byte_notify(uint8_t *dst, uint8_t value){
-#else
-void eeprom_write_byte_notify(uint8_t *dst, uint8_t value, bool active){
-    if (active) {
-        uint8_t previous_value = eeprom_read_byte(dst);
-        eeprom_byte_notify(dst, previous_value, value, true);
-    }
-#endif //DEBUG_EEPROM_CHANGES
-    eeprom_write_byte(dst, value);
-}
-
-#ifndef DEBUG_EEPROM_CHANGES
-void eeprom_update_byte_notify(uint8_t *dst, uint8_t value){
-#else
-void eeprom_update_byte_notify(uint8_t *dst, uint8_t value, bool active){
-
-    if (active) {
-        uint8_t previous_value = eeprom_read_byte(dst);
-        if (previous_value != value) {
-            eeprom_byte_notify(dst, previous_value, value, false);
-        }
-    }
-#endif //DEBUG_EEPROM_CHANGES
-    eeprom_update_byte(dst, value);
-}
-
-#ifndef DEBUG_EEPROM_CHANGES
-void eeprom_write_word_notify(uint16_t *dst, uint16_t value){
-#else
-void eeprom_write_word_notify(uint16_t *dst, uint16_t value, bool active){
-    if (active) {
-        uint16_t previous_value = eeprom_read_word(dst);
-        eeprom_word_notify(dst, previous_value, value, true);
-    }
-#endif //DEBUG_EEPROM_CHANGES
-    eeprom_write_word(dst, value);
-}
-
-#ifndef DEBUG_EEPROM_CHANGES
-void eeprom_update_word_notify(uint16_t *dst, uint16_t value){
-#else
-void eeprom_update_word_notify(uint16_t *dst, uint16_t value, bool active){
-    if (active) {
-        uint16_t previous_value = eeprom_read_word(dst);
-        if (previous_value != value) {
-            eeprom_word_notify(dst, previous_value, value, false);
-        }
-    }
-#endif //DEBUG_EEPROM_CHANGES
-    eeprom_update_word(dst, value);
-}
-
-#ifndef DEBUG_EEPROM_CHANGES
-void eeprom_write_dword_notify(uint32_t *dst, uint32_t value){
-#else
-void eeprom_write_dword_notify(uint32_t *dst, uint32_t value, bool active){
-    if (active) {
-        uint32_t previous_value = eeprom_read_dword(dst);
-        eeprom_dword_notify(dst, previous_value, value, true);
-    }
-#endif //DEBUG_EEPROM_CHANGES
-    eeprom_write_dword(dst, value);
-}
-
-#ifndef DEBUG_EEPROM_CHANGES
-void eeprom_update_dword_notify(uint32_t *dst, uint32_t value){
-#else
-void eeprom_update_dword_notify(uint32_t *dst, uint32_t value, bool active){
-    if (active) {
-        uint32_t previous_value = eeprom_read_dword(dst);
-        if (previous_value != value) {
-            eeprom_dword_notify(dst, previous_value, value, false);
-        }
-    }
-#endif //DEBUG_EEPROM_CHANGES
-    eeprom_update_dword(dst, value);
-}
-
-#ifndef DEBUG_EEPROM_CHANGES
-void eeprom_write_float_notify(float *dst, float value){
-#else
-void eeprom_write_float_notify(float *dst, float value, bool active){
-    if (active) {
-        float previous_value = eeprom_read_float(dst);
-        eeprom_float_notify(dst, previous_value, value, true);
-    }
-#endif //DEBUG_EEPROM_CHANGES
-    eeprom_write_float(dst, value);
-}
-
-#ifndef DEBUG_EEPROM_CHANGES
-void eeprom_update_float_notify(float *dst, float value){
-#else
-void eeprom_update_float_notify(float *dst, float value, bool active){
-    if (active) {
-        float previous_value = eeprom_read_float(dst);
-        if (previous_value != value) {
-            eeprom_float_notify(dst, previous_value, value, false);
-        }
-    }
-#endif //DEBUG_EEPROM_CHANGES
-    eeprom_update_float(dst, value);
-}
-
-#ifndef DEBUG_EEPROM_CHANGES
-void eeprom_write_block_notify(const void *__src, void *__dst, size_t __n){
-    eeprom_write_block(__src, __dst, __n);
-#else
-void eeprom_write_block_notify(const void *__src, void *__dst, size_t __n, bool active){
-    if (active) {
-        uint8_t previous_values[__n];
-        uint8_t new_values[__n];
-        eeprom_read_block(previous_values, __dst, __n);
-        eeprom_write_block(__src, __dst, __n);
-        eeprom_read_block(new_values, __dst, __n);
-        eeprom_block_notify(__dst, previous_values, new_values, __n, true);
-    }
-#endif //DEBUG_EEPROM_CHANGES
-}
-
-#ifndef DEBUG_EEPROM_CHANGES
-void eeprom_update_block_notify(const void *__src, void *__dst, size_t __n){
-    eeprom_update_block(__src, __dst, __n);
-#else
-void eeprom_update_block_notify(const void *__src, void *__dst, size_t __n, bool active){
-    if (active) {
-        uint8_t previous_values[__n];
-        uint8_t new_values[__n];
-        eeprom_read_block(previous_values, __dst, __n);
-        eeprom_update_block(__src, __dst, __n);
-        eeprom_read_block(new_values, __dst, __n);
-        eeprom_block_notify(__dst, previous_values, new_values, __n, false);
-    }
-#endif //DEBUG_EEPROM_CHANGES
-}
-
 void eeprom_switch_to_next_sheet()
 {
    int8_t sheet = eeprom_read_byte(&(EEPROM_Sheets_base->active_sheet));

    sheet = eeprom_next_initialized_sheet(sheet);
-    if (sheet >= 0) eeprom_update_byte_notify(&(EEPROM_Sheets_base->active_sheet), sheet);
-}
-
-bool __attribute__((noinline)) eeprom_is_sheet_initialized(uint8_t sheet_num) {
-    return (eeprom_read_word(reinterpret_cast<uint16_t*>(&(EEPROM_Sheets_base->s[sheet_num].z_offset))) != EEPROM_EMPTY_VALUE16);
-}
-
-
-bool __attribute__((noinline)) eeprom_is_initialized_block(const void *__p, size_t __n) {
-    const uint8_t *p = (const uint8_t*)__p;
-    while (__n--) {
-        if (eeprom_read_byte(p++) != EEPROM_EMPTY_VALUE)
-            return true;
-    }
-    return false;
-}
-
-void eeprom_update_block_P(const void *__src, void *__dst, size_t __n) {
-    const uint8_t *src = (const uint8_t*)__src;
-    uint8_t *dst = (uint8_t*)__dst;
-    while (__n--) {
-        eeprom_update_byte_notify(dst++, pgm_read_byte(src++));
-    }
-}
-
-void eeprom_toggle(uint8_t *__p) {
-    eeprom_write_byte_notify(__p, !eeprom_read_byte(__p));
-}
-
-void __attribute__((noinline)) eeprom_increment_byte(uint8_t *__p) {
-    eeprom_write_byte_notify(__p, eeprom_read_byte(__p) + 1);
-}
-
-void __attribute__((noinline)) eeprom_increment_word(uint16_t *__p) {
-    eeprom_write_word_notify(__p, eeprom_read_word(__p) + 1);
-}
-
-void __attribute__((noinline)) eeprom_increment_dword(uint32_t *__p) {
-    eeprom_write_dword_notify(__p, eeprom_read_dword(__p) + 1);
-}
-
-
-void __attribute__((noinline)) eeprom_add_byte(uint8_t *__p, uint8_t add) {
-    eeprom_write_byte_notify(__p, eeprom_read_byte(__p) + add);
-}
-
-void __attribute__((noinline)) eeprom_add_word(uint16_t *__p, uint16_t add) {
-    eeprom_write_word_notify(__p, eeprom_read_word(__p) + add);
-}
-
-void __attribute__((noinline)) eeprom_add_dword(uint32_t *__p, uint32_t add) {
-    eeprom_write_dword_notify(__p, eeprom_read_dword(__p) + add);
-}
-
-
-uint8_t __attribute__((noinline)) eeprom_init_default_byte(uint8_t *__p, uint8_t def) {
-    uint8_t val = eeprom_read_byte(__p);
-    if (val == EEPROM_EMPTY_VALUE) {
-        eeprom_write_byte_notify(__p, def);
-        return def;
-    }
-    return val;
-}
-
-uint16_t __attribute__((noinline)) eeprom_init_default_word(uint16_t *__p, uint16_t def) {
-    uint16_t val = eeprom_read_word(__p);
-    if (val == EEPROM_EMPTY_VALUE16) {
-        eeprom_write_word_notify(__p, def);
-        return def;
-    }
-    return val;
-}
-
-uint32_t __attribute__((noinline)) eeprom_init_default_dword(uint32_t *__p, uint32_t def) {
-    uint32_t val = eeprom_read_dword(__p);
-    if (val == EEPROM_EMPTY_VALUE32) {
-        eeprom_write_dword_notify(__p, def);
-        return def;
-    }
-    return val;
-}
-
-void __attribute__((noinline)) eeprom_init_default_float(float *__p, float def) {
-    if (eeprom_read_dword((uint32_t*)__p) == EEPROM_EMPTY_VALUE32)
-        eeprom_write_float_notify(__p, def);
-}
-
-void __attribute__((noinline)) eeprom_init_default_block(void *__p, size_t __n, const void *def) {
-    if (!eeprom_is_initialized_block(__p, __n))
-        eeprom_update_block_notify(def, __p, __n);
-}
-
-void __attribute__((noinline)) eeprom_init_default_block_P(void *__p, size_t __n, const void *def) {
-    if (!eeprom_is_initialized_block(__p, __n))
-        eeprom_update_block_P(def, __p, __n);
+    if (sheet >= 0) eeprom_update_byte(&(EEPROM_Sheets_base->active_sheet), sheet);
 }
--- a/Firmware/eeprom.h
+++ b/Firmware/eeprom.h
--- a/Firmware/fancheck.cpp
+++ b/Firmware/fancheck.cpp
@ -1,362 +0,0 @@
-// fan control and check
-#include "fancheck.h"
-#include "ultralcd.h"
-#include "sound.h"
-#include "messages.h"
-#include "temperature.h"
-#include "stepper.h"
-#include "stopwatch.h"
-
-#define FAN_CHECK_PERIOD 5000 //5s
-#define FAN_CHECK_DURATION 100 //100ms
-
-//Macro for print fan speed
-#define FAN_PULSE_WIDTH_LIMIT ((fanSpeed > 100) ? 3 : 4) //time in ms
-
-#ifdef FANCHECK
-volatile uint8_t fan_check_error = EFCE_OK;
-#endif
-
-#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
-static uint32_t t_fan_rising_edge;
-#endif // #if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
-
-#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1)
-  #ifdef EXTRUDER_ALTFAN_DETECT
-  static struct
-  {
-      uint8_t isAltfan : 1;
-      uint8_t altfanOverride : 1;
-  } altfanStatus;
-  #endif //EXTRUDER_ALTFAN_DETECT
-
-  unsigned long extruder_autofan_last_check = _millis();
-  bool fan_measuring = false;
-  static uint8_t fanState = 0;
-#endif
-
-#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1)
-  #if defined(FAN_PIN) && FAN_PIN > -1
-    #if EXTRUDER_0_AUTO_FAN_PIN == FAN_PIN
-       #error "You cannot set EXTRUDER_0_AUTO_FAN_PIN equal to FAN_PIN"
-    #endif
-  #endif
-
-void setExtruderAutoFanState(uint8_t state)
-{
-    //If bit 1 is set (0x02), then the hotend fan speed won't be adjusted according to temperature. Useful for forcing
-    //the fan to either On or Off during certain tests/errors.
-
-    fanState = state;
-    newFanSpeed = 0;
-    if (fanState & 0x01)
-    {
-#ifdef EXTRUDER_ALTFAN_DETECT
-        if (altfanStatus.isAltfan && !altfanStatus.altfanOverride) newFanSpeed = EXTRUDER_ALTFAN_SPEED_SILENT;
-        else newFanSpeed = EXTRUDER_AUTO_FAN_SPEED;
-#else //EXTRUDER_ALTFAN_DETECT
-        newFanSpeed = EXTRUDER_AUTO_FAN_SPEED;
-#endif //EXTRUDER_ALTFAN_DETECT
-    }
-    timer4_set_fan0(newFanSpeed);
-}
-
-#if (defined(FANCHECK) && (((defined(TACH_0) && (TACH_0 >-1)) || (defined(TACH_1) && (TACH_1 > -1)))))
-
-void countFanSpeed()
-{
-    //SERIAL_ECHOPGM("edge counter 1:"); MYSERIAL.println(fan_edge_counter[1]);
-    fan_speed[0] = (fan_edge_counter[0] * (float(250) / (_millis() - extruder_autofan_last_check)));
-    fan_speed[1] = (fan_edge_counter[1] * (float(250) / (_millis() - extruder_autofan_last_check)));
-    /*SERIAL_ECHOPGM("time interval: "); MYSERIAL.println(_millis() - extruder_autofan_last_check);
-    SERIAL_ECHOPGM("hotend fan speed:"); MYSERIAL.print(fan_speed[0]); SERIAL_ECHOPGM("; edge counter:"); MYSERIAL.println(fan_edge_counter[0]);
-    SERIAL_ECHOPGM("print fan speed:"); MYSERIAL.print(fan_speed[1]); SERIAL_ECHOPGM("; edge counter:"); MYSERIAL.println(fan_edge_counter[1]);
-    SERIAL_ECHOLNPGM(" ");*/
-    fan_edge_counter[0] = 0;
-    fan_edge_counter[1] = 0;
-}
-
-//! Prints serialMsg to serial port, displays lcdMsg onto the LCD and beeps.
-//! Extracted from fanSpeedError to save some space.
-//! @param serialMsg pointer into PROGMEM, this text will be printed to the serial port
-//! @param lcdMsg pointer into PROGMEM, this text will be printed onto the LCD
-static void fanSpeedErrorBeep(const char *serialMsg, const char *lcdMsg){
-    SERIAL_ECHOLNRPGM(serialMsg);
-    if (get_message_level() == 0) {
-        Sound_MakeCustom(200,0,true);
-        LCD_ALERTMESSAGERPGM(lcdMsg);
-    }
-}
-
-void fanSpeedError(unsigned char _fan) {
-    if (fan_check_error == EFCE_REPORTED) return;
-    fan_check_error = EFCE_REPORTED;
-
-    if (printJobOngoing()) {
-        // A print is ongoing, pause the print normally
-        if(!printingIsPaused()) {
-            if (usb_timer.running())
-                lcd_pause_usb_print();
-            else
-                lcd_pause_print();
-        }
-    }
-    else {
-        // Nothing is going on, but still turn off heaters and report the error
-        setTargetHotend(0);
-        heating_status = HeatingStatus::NO_HEATING;
-    }
-    switch (_fan) {
-    case 0:	// extracting the same code from case 0 and case 1 into a function saves 72B
-        fanSpeedErrorBeep(PSTR("Hotend fan speed is lower than expected"), MSG_FANCHECK_HOTEND);
-        break;
-    case 1:
-        fanSpeedErrorBeep(PSTR("Print fan speed is lower than expected"), MSG_FANCHECK_PRINT);
-        break;
-    }
-}
-
-void checkFanSpeed()
-{
-    uint8_t max_fan_errors[2];
-#ifdef FAN_SOFT_PWM
-    max_fan_errors[1] = 3;  // 15 seconds (Print fan)
-    max_fan_errors[0] = 2;  // 10 seconds (Hotend fan)
-#else //FAN_SOFT_PWM
-    max_fan_errors[1] = 15; // 15 seconds (Print fan)
-    max_fan_errors[0] = 5;  // 5  seconds (Hotend fan)
-#endif //FAN_SOFT_PWM
-
-    if(fans_check_enabled)
-        fans_check_enabled = (eeprom_read_byte((uint8_t*)EEPROM_FAN_CHECK_ENABLED) > 0);
-    static uint8_t fan_speed_errors[2] = { 0,0 };
-#if (defined(FANCHECK) && defined(TACH_0) && (TACH_0 >-1))
-    if ((fan_speed[0] < 20) && (current_temperature[0] > EXTRUDER_AUTO_FAN_TEMPERATURE)){ fan_speed_errors[0]++;}
-    else fan_speed_errors[0] = 0;
-#endif
-#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
-    if ((fan_speed[1] < 5) && ((blocks_queued() ? block_buffer[block_buffer_tail].fan_speed : fanSpeed) > MIN_PRINT_FAN_SPEED)) fan_speed_errors[1]++;
-    else fan_speed_errors[1] = 0;
-#endif
-
-    // drop the fan_check_error flag when both fans are ok
-    if( fan_speed_errors[0] == 0 && fan_speed_errors[1] == 0 && fan_check_error == EFCE_REPORTED){
-        // we may even send some info to the LCD from here
-        fan_check_error = EFCE_FIXED;
-    }
-    if ((fan_check_error == EFCE_FIXED) && !printer_active()){
-        fan_check_error = EFCE_OK; //if the issue is fixed while the printer is doing nothing, reenable processing immediately.
-        lcd_reset_alert_level(); //for another fan speed error
-        lcd_setstatuspgm(MSG_WELCOME); // Reset the status line message to visually show the error is gone
-    }
-    if (fans_check_enabled && (fan_check_error != EFCE_REPORTED))
-    {
-        for (uint8_t fan = 0; fan < 2; fan++)
-        {
-            if (fan_speed_errors[fan] > max_fan_errors[fan])
-            {
-                fan_speed_errors[fan] = 0;
-                fanSpeedError(fan);
-            }
-        }
-    }
-}
-#endif //(defined(TACH_0) && TACH_0 >-1) || (defined(TACH_1) && TACH_1 > -1)
-
-#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
-void setup_fan_interrupt() {
-//INT7
-	DDRE &= ~(1 << 7); //input pin
-	PORTE &= ~(1 << 7); //no internal pull-up
-
-	//start with sensing rising edge
-	EICRB &= ~(1 << 6);
-	EICRB |= (1 << 7);
-
-	//enable INT7 interrupt
-	EIMSK |= (1 << 7);
-}
-
-// The fan interrupt is triggered at maximum 325Hz (may be a bit more due to component tollerances),
-// and it takes 4.24 us to process (the interrupt invocation overhead not taken into account).
-ISR(INT7_vect) {
-	//measuring speed now works for fanSpeed > 18 (approximately), which is sufficient because MIN_PRINT_FAN_SPEED is higher
-#ifdef FAN_SOFT_PWM
-	if (!fan_measuring || (fanSpeedSoftPwm < MIN_PRINT_FAN_SPEED)) return;
-#else //FAN_SOFT_PWM
-	if (fanSpeed < MIN_PRINT_FAN_SPEED) return;
-#endif //FAN_SOFT_PWM
-
-	if ((1 << 6) & EICRB) { //interrupt was triggered by rising edge
-		t_fan_rising_edge = millis_nc();
-	}
-	else { //interrupt was triggered by falling edge
-		if ((millis_nc() - t_fan_rising_edge) >= FAN_PULSE_WIDTH_LIMIT) {//this pulse was from sensor and not from pwm
-			fan_edge_counter[1] += 2; //we are currently counting all edges so lets count two edges for one pulse
-		}
-	}
-	EICRB ^= (1 << 6); //change edge
-}
-#endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
-
-#ifdef EXTRUDER_ALTFAN_DETECT
-ISR(INT6_vect) {
-    fan_edge_counter[0]++;
-}
-
-bool extruder_altfan_detect()
-{
-    // override isAltFan setting for detection
-    altfanStatus.isAltfan = 0;
-
-    // During initialisation, use the EEPROM value
-    altfanStatus.altfanOverride = eeprom_init_default_byte((uint8_t*)EEPROM_ALTFAN_OVERRIDE, 0);
-    setExtruderAutoFanState(3);
-
-    SET_INPUT(TACH_0);
-    CRITICAL_SECTION_START;
-    EICRB &= ~(1 << ISC61);
-    EICRB |= (1 << ISC60);
-    EIMSK |= (1 << INT6);
-    fan_edge_counter[0] = 0;
-    CRITICAL_SECTION_END;
-    extruder_autofan_last_check = _millis();
-
-    _delay(1000);
-
-    EIMSK &= ~(1 << INT6);
-
-    countFanSpeed();
-
-    // restore fan state
-    altfanStatus.isAltfan = fan_speed[0] > 100;
-    setExtruderAutoFanState(1);
-
-    return altfanStatus.isAltfan;
-}
-
-void altfanOverride_toggle()
-{
-    altfanStatus.altfanOverride = !altfanStatus.altfanOverride;
-    eeprom_update_byte_notify((uint8_t *)EEPROM_ALTFAN_OVERRIDE, altfanStatus.altfanOverride);
-}
-
-bool altfanOverride_get()
-{
-    return altfanStatus.altfanOverride;
-}
-
-#endif //EXTRUDER_ALTFAN_DETECT
-
-void checkExtruderAutoFans()
-{
-#if defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1
-    if (!(fanState & 0x02))
-    {
-        fanState &= ~1;
-        fanState |= current_temperature[0] > EXTRUDER_AUTO_FAN_TEMPERATURE;
-        fanState |= get_temp_error();
-    }
-    setExtruderAutoFanState(fanState);
-#endif
-}
-
-#endif // any extruder auto fan pins set
-
-#if (defined(FANCHECK) && defined(TACH_0) && (TACH_0 > -1))
-void readFanTach() {
-    static bool fan_state[2];
-#ifdef FAN_SOFT_PWM
-    if (READ(TACH_0) != fan_state[0]) {
-        if(fan_measuring) fan_edge_counter[0] ++;
-        fan_state[0] = !fan_state[0];
-    }
-#else //FAN_SOFT_PWM
-    if (READ(TACH_0) != fan_state[0]) {
-        fan_edge_counter[0] ++;
-        fan_state[0] = !fan_state[0];
-    }
-#endif
-    //if (READ(TACH_1) != fan_state[1]) {
-    //	fan_edge_counter[1] ++;
-    //	fan_state[1] = !fan_state[1];
-    //}
-}
-#endif //TACH_0
-
-void checkFans()
-{
-#ifndef DEBUG_DISABLE_FANCHECK
-#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1)
-
-#ifdef FAN_SOFT_PWM
-#ifdef FANCHECK
-  if ((_millis() - extruder_autofan_last_check > FAN_CHECK_PERIOD) && (!fan_measuring)) {
-	  extruder_autofan_last_check = _millis();
-	  fanSpeedBckp = fanSpeedSoftPwm;
-
-	  if (fanSpeedSoftPwm >= MIN_PRINT_FAN_SPEED) { //if we are in rage where we are doing fan check, set full PWM range for a short time to measure fan RPM by reading tacho signal without modulation by PWM signal
-		//  printf_P(PSTR("fanSpeedSoftPwm 1: %d\n"), fanSpeedSoftPwm);
-		  fanSpeedSoftPwm = 255;
-	  }
-	  fan_measuring = true;
-  }
-  if ((_millis() - extruder_autofan_last_check > FAN_CHECK_DURATION) && (fan_measuring)) {
-	  countFanSpeed();
-	  checkFanSpeed();
-	  //printf_P(PSTR("fanSpeedSoftPwm 1: %d\n"), fanSpeedSoftPwm);
-	  fanSpeedSoftPwm = fanSpeedBckp;
-	  //printf_P(PSTR("fan PWM: %d; extr fanSpeed measured: %d; print fan speed measured: %d \n"), fanSpeedBckp, fan_speed[0], fan_speed[1]);
-	  extruder_autofan_last_check = _millis();
-	  fan_measuring = false;
-  }
-#endif //FANCHECK
-  checkExtruderAutoFans();
-#else //FAN_SOFT_PWM
-  if(_millis() - extruder_autofan_last_check > 1000)  // only need to check fan state very infrequently
-  {
-#if (defined(FANCHECK) && ((defined(TACH_0) && (TACH_0 >-1)) || (defined(TACH_1) && (TACH_1 > -1))))
-	countFanSpeed();
-	checkFanSpeed();
-#endif //(defined(TACH_0) && TACH_0 >-1) || (defined(TACH_1) && TACH_1 > -1)
-    checkExtruderAutoFans();
-    extruder_autofan_last_check = _millis();
-  }
-#endif //FAN_SOFT_PWM
-
-#endif
-#endif //DEBUG_DISABLE_FANCHECK
-}
-
-void resetFanCheck() {
-    fan_measuring = false;
-    extruder_autofan_last_check = _millis();
-}
-
-
-void hotendFanSetFullSpeed()
-{
-#ifdef EXTRUDER_ALTFAN_DETECT
-    altfanStatus.altfanOverride = 1; //full speed
-#endif //EXTRUDER_ALTFAN_DETECT
-    resetFanCheck();
-    setExtruderAutoFanState(3);
-    SET_OUTPUT(FAN_PIN);
-#ifdef FAN_SOFT_PWM
-    fanSpeedSoftPwm = 255;
-#else //FAN_SOFT_PWM
-    analogWrite(FAN_PIN, 255);
-#endif //FAN_SOFT_PWM
-    fanSpeed = 255;
-}
-
-void hotendDefaultAutoFanState()
-{
-#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1)
-#ifdef EXTRUDER_ALTFAN_DETECT
-    altfanStatus.altfanOverride = eeprom_read_byte((uint8_t*)EEPROM_ALTFAN_OVERRIDE);
-#endif
-    resetFanCheck();
-    setExtruderAutoFanState(1);
-#endif
-}
--- a/Firmware/fancheck.h
+++ b/Firmware/fancheck.h
@ -1,42 +0,0 @@
-// fan control and check
-#pragma once
-
-#include "Configuration.h"
-#include "config.h"
-
-#if (defined(FANCHECK) && defined(TACH_0) && (TACH_0 > -1))
-enum {
-	EFCE_OK = 0,   //!< normal operation, both fans are ok
-	EFCE_FIXED,    //!< previous fan error was fixed
-	EFCE_REPORTED  //!< fan error detected and reported to LCD and serial
-};
-extern volatile uint8_t fan_check_error;
-
-void readFanTach();
-#endif //(defined(TACH_0))
-
-#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
-void setup_fan_interrupt();
-#endif // (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
-
-#ifdef EXTRUDER_ALTFAN_DETECT
-extern bool extruder_altfan_detect();
-extern void altfanOverride_toggle();
-extern bool altfanOverride_get();
-#endif //EXTRUDER_ALTFAN_DETECT
-
-#if (defined(EXTRUDER_0_AUTO_FAN_PIN) && EXTRUDER_0_AUTO_FAN_PIN > -1)
-#ifdef FAN_SOFT_PWM
-extern bool fan_measuring;
-#endif //FAN_SOFT_PWM
-
-extern unsigned long extruder_autofan_last_check;
-void setExtruderAutoFanState(uint8_t state);
-void checkExtruderAutoFans();
-#endif
-
-void checkFans();
-void resetFanCheck(); // resets the fan measuring state
-
-void hotendFanSetFullSpeed();
-void hotendDefaultAutoFanState();
--- a/Firmware/fastio.h
+++ b/Firmware/fastio.h
@ -7,8 +7,15 @@
 #define	_FASTIO_ARDUINO_H

 #include <avr/io.h>
-#include "macros.h"

+/*
+  utility functions
+*/
+
+#ifndef MASK
+/// MASKING- returns \f$2^PIN\f$
+#define MASK(PIN)  (1 << PIN)
+#endif

 /*
  magic I/O routines
@ -16,20 +23,20 @@
 */

 /// Read a pin
-#define _READ(IO) ((bool)(DIO ## IO ## _RPORT & _BV(DIO ## IO ## _PIN)))
+#define _READ(IO) ((bool)(DIO ## IO ## _RPORT & MASK(DIO ## IO ## _PIN)))
 /// write to a pin
 // On some boards pins > 0x100 are used. These are not converted to atomic actions. An critical section is needed.

-#define _WRITE_NC(IO, v)  do { if (v) {DIO ##  IO ## _WPORT |= _BV(DIO ## IO ## _PIN); } else {DIO ##  IO ## _WPORT &= ~_BV(DIO ## IO ## _PIN); }; } while (0)
+#define _WRITE_NC(IO, v)  do { if (v) {DIO ##  IO ## _WPORT |= MASK(DIO ## IO ## _PIN); } else {DIO ##  IO ## _WPORT &= ~MASK(DIO ## IO ## _PIN); }; } while (0)

 #define _WRITE_C(IO, v)   do { if (v) { \
                                         CRITICAL_SECTION_START; \
-                                         {DIO ##  IO ## _WPORT |= _BV(DIO ## IO ## _PIN); }\
+                                         {DIO ##  IO ## _WPORT |= MASK(DIO ## IO ## _PIN); }\
                                         CRITICAL_SECTION_END; \
                                       }\
                                       else {\
                                         CRITICAL_SECTION_START; \
-                                         {DIO ##  IO ## _WPORT &= ~_BV(DIO ## IO ## _PIN); }\
+                                         {DIO ##  IO ## _WPORT &= ~MASK(DIO ## IO ## _PIN); }\
                                         CRITICAL_SECTION_END; \
                                       }\
                                     }\
@ -38,20 +45,20 @@
 #define _WRITE(IO, v)  do {  if (&(DIO ##  IO ## _RPORT) >= (uint8_t *)0x100) {_WRITE_C(IO, v); } else {_WRITE_NC(IO, v); }; } while (0)

 /// toggle a pin
-#define _TOGGLE(IO)  do {DIO ##  IO ## _RPORT = _BV(DIO ## IO ## _PIN); } while (0)
+#define _TOGGLE(IO)  do {DIO ##  IO ## _RPORT = MASK(DIO ## IO ## _PIN); } while (0)

 /// set pin as input
-#define	_SET_INPUT(IO) do {DIO ##  IO ## _DDR &= ~_BV(DIO ## IO ## _PIN); } while (0)
+#define	_SET_INPUT(IO) do {DIO ##  IO ## _DDR &= ~MASK(DIO ## IO ## _PIN); } while (0)
 /// set pin as output
-#define	_SET_OUTPUT(IO) do {DIO ##  IO ## _DDR |=  _BV(DIO ## IO ## _PIN); } while (0)
+#define	_SET_OUTPUT(IO) do {DIO ##  IO ## _DDR |=  MASK(DIO ## IO ## _PIN); } while (0)

 /// check if pin is an input
-#define	_GET_INPUT(IO)  ((DIO ## IO ## _DDR & _BV(DIO ## IO ## _PIN)) == 0)
+#define	_GET_INPUT(IO)  ((DIO ## IO ## _DDR & MASK(DIO ## IO ## _PIN)) == 0)
 /// check if pin is an output
-#define	_GET_OUTPUT(IO)  ((DIO ## IO ## _DDR & _BV(DIO ## IO ## _PIN)) != 0)
+#define	_GET_OUTPUT(IO)  ((DIO ## IO ## _DDR & MASK(DIO ## IO ## _PIN)) != 0)

 /// check if pin is an timer
-#define	_GET_TIMER(IO)  (DIO ## IO ## _PWM)
+#define	_GET_TIMER(IO)  ((DIO ## IO ## _PWM)

 //  why double up on these macros? see http://gcc.gnu.org/onlinedocs/cpp/Stringification.html

@ -62,7 +69,7 @@
 /// Write to a pin wrapper, non critical.
 /// This macro is cheaper than WRITE(IO,v) on ports H,I,J,K,L, as _WRITE_C disables / enables interrupts
 /// and stores the old CPU flags on the stack.
-/// This macro should only be called, where it cannot be interrupted.
+/// This macro should only be called, where it cannot be interrupted. 
 #define WRITE_NC(IO, v)  _WRITE_NC(IO, v)

 /// toggle a pin wrapper
@ -931,10 +938,10 @@ pins
 #define	TXD					DIO1

 // SPI
-#define	SCK					52
-#define	MISO				50
-#define	MOSI				51
-#define	SS					53
+#define	SCK					DIO52
+#define	MISO				DIO50
+#define	MOSI				DIO51
+#define	SS					DIO53

 // TWI (I2C)
 #define	SCL					DIO21
@ -2067,7 +2074,7 @@ pins
 pins
 */

-//#define AT90USBxx_TEENSYPP_ASSIGNMENTS // Use Teensy++ 2.0 assignments
+//#define AT90USBxx_TEENSYPP_ASSIGNMENTS // Use Teensy++ 2.0 assignments 
 #ifndef AT90USBxx_TEENSYPP_ASSIGNMENTS // Use traditional Marlin pin assignments

 #define DIO0_PIN        PINA0
@ -2707,8 +2714,8 @@ pins

 /*

-AT90USB  51 50 49 48 47 46 45 44 10 11 12 13 14 15 16 17 35 36 37 38 39 40 41 42 25 26 27 28 29 30 31 32 33 34 43 09 18 19 01 02 61 60 59 58 57 56 55 54
-Port     A0 A1 A2 A3 A4 A5 A6 A7 B0 B1 B2 B3 B4 B5 B6 B7 C0 C1 C2 C3 C4 C5 C6 C7 D0 D1 D2 D3 D4 D5 D6 D7 E0 E1 E2 E3 E4 E5 E6 E7 F0 F1 F2 F3 F4 F5 F6 F7
+AT90USB  51 50 49 48 47 46 45 44 10 11 12 13 14 15 16 17 35 36 37 38 39 40 41 42 25 26 27 28 29 30 31 32 33 34 43 09 18 19 01 02 61 60 59 58 57 56 55 54 
+Port     A0 A1 A2 A3 A4 A5 A6 A7 B0 B1 B2 B3 B4 B5 B6 B7 C0 C1 C2 C3 C4 C5 C6 C7 D0 D1 D2 D3 D4 D5 D6 D7 E0 E1 E2 E3 E4 E5 E6 E7 F0 F1 F2 F3 F4 F5 F6 F7 
 Marlin   00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47
 Teensy   28 29 30 31 32 33 34 35 20 21 22 23 24 25 26 27 10 11 12 13 14 15 16 17 00 01 02 03 04 05 06 07 08 09(46*47)36 37 18 19 38 39 40 41 42 43 44 45
         The pins 46 and 47 are not supported by Teensyduino, but are supported below.
--- a/Firmware/first_lay_cal.cpp
+++ b/Firmware/first_lay_cal.cpp
@ -4,13 +4,126 @@
 //! @brief First layer (Z offset) calibration

 #include "first_lay_cal.h"
-#include "Configuration_var.h"
+#include "Configuration_prusa.h"
+#include "language.h"
 #include "Marlin.h"
-#include "messages.h"
-#include "cmdqueue.h"
-#include "mmu2.h"
+#include "mmu.h"
 #include <avr/pgmspace.h>
-#include <math.h>
+
+//! @brief Wait for preheat
+void lay1cal_wait_preheat()
+{
+    static const char cmd_preheat_0[] PROGMEM = "M107";
+    static const char cmd_preheat_1[] PROGMEM = "M190";
+    static const char cmd_preheat_2[] PROGMEM = "M109";
+    static const char cmd_preheat_4[] PROGMEM = "G28";
+    static const char cmd_preheat_5[] PROGMEM = "G92 E0.0";
+
+    const char * const preheat_cmd[] =
+    {
+        cmd_preheat_0,
+        cmd_preheat_1,
+        cmd_preheat_2,
+        _T(MSG_M117_V2_CALIBRATION),
+        cmd_preheat_4,
+        cmd_preheat_5,
+    };
+
+    for (uint8_t i = 0; i < (sizeof(preheat_cmd)/sizeof(preheat_cmd[0])); ++i)
+    {
+        enquecommand_P(preheat_cmd[i]);
+    }
+
+}
+
+//! @brief Load filament
+//! @param cmd_buffer character buffer needed to format gcodes
+//! @param filament filament to use (applies for MMU only)
+void lay1cal_load_filament(char *cmd_buffer, uint8_t filament)
+{
+    if (mmu_enabled)
+    {
+        enquecommand_P(PSTR("M83"));
+        enquecommand_P(PSTR("G1 Y-3.0 F1000.0"));
+        enquecommand_P(PSTR("G1 Z0.4 F1000.0"));
+        sprintf_P(cmd_buffer, PSTR("T%d"), filament);
+        enquecommand(cmd_buffer);
+    }
+
+}
+
+//! @brief Print intro line
+void lay1cal_intro_line()
+{
+    static const char cmd_intro_mmu_3[] PROGMEM = "G1 X55.0 E32.0 F1073.0";
+    static const char cmd_intro_mmu_4[] PROGMEM = "G1 X5.0 E32.0 F1800.0";
+    static const char cmd_intro_mmu_5[] PROGMEM = "G1 X55.0 E8.0 F2000.0";
+    static const char cmd_intro_mmu_6[] PROGMEM = "G1 Z0.3 F1000.0";
+    static const char cmd_intro_mmu_7[] PROGMEM = "G92 E0.0";
+    static const char cmd_intro_mmu_8[] PROGMEM = "G1 X240.0 E25.0  F2200.0";
+    static const char cmd_intro_mmu_9[] PROGMEM = "G1 Y-2.0 F1000.0";
+    static const char cmd_intro_mmu_10[] PROGMEM = "G1 X55.0 E25 F1400.0";
+    static const char cmd_intro_mmu_11[] PROGMEM = "G1 Z0.20 F1000.0";
+    static const char cmd_intro_mmu_12[] PROGMEM = "G1 X5.0 E4.0 F1000.0";
+
+    static const char * const intro_mmu_cmd[] PROGMEM =
+    {
+        cmd_intro_mmu_3,
+        cmd_intro_mmu_4,
+        cmd_intro_mmu_5,
+        cmd_intro_mmu_6,
+        cmd_intro_mmu_7,
+        cmd_intro_mmu_8,
+        cmd_intro_mmu_9,
+        cmd_intro_mmu_10,
+        cmd_intro_mmu_11,
+        cmd_intro_mmu_12,
+    };
+
+    if (mmu_enabled)
+    {
+        for (uint8_t i = 0; i < (sizeof(intro_mmu_cmd)/sizeof(intro_mmu_cmd[0])); ++i)
+        {
+            enquecommand_P(static_cast<char*>(pgm_read_ptr(&intro_mmu_cmd[i])));
+        }
+    }
+    else
+    {
+        enquecommand_P(PSTR("G1 X60.0 E9.0 F1000.0"));
+        enquecommand_P(PSTR("G1 X100.0 E12.5 F1000.0"));
+    }
+}
+
+//! @brief Setup for printing meander
+void lay1cal_before_meander()
+{
+    static const char cmd_pre_meander_0[] PROGMEM = "G92 E0.0";
+    static const char cmd_pre_meander_1[] PROGMEM = "G21"; //set units to millimeters TODO unsupported command
+    static const char cmd_pre_meander_2[] PROGMEM = "G90"; //use absolute coordinates
+    static const char cmd_pre_meander_3[] PROGMEM = "M83"; //use relative distances for extrusion TODO: duplicate
+    static const char cmd_pre_meander_4[] PROGMEM = "G1 E-1.50000 F2100.00000";
+    static const char cmd_pre_meander_5[] PROGMEM = "G1 Z5 F7200.000";
+    static const char cmd_pre_meander_6[] PROGMEM = "M204 S1000"; //set acceleration
+    static const char cmd_pre_meander_7[] PROGMEM = "G1 F4000";
+
+    static const char * const cmd_pre_meander[] PROGMEM =
+    {
+            cmd_pre_meander_0,
+            cmd_pre_meander_1,
+            cmd_pre_meander_2,
+            cmd_pre_meander_3,
+            cmd_pre_meander_4,
+            cmd_pre_meander_5,
+            cmd_pre_meander_6,
+            cmd_pre_meander_7,
+    };
+
+    for (uint8_t i = 0; i < (sizeof(cmd_pre_meander)/sizeof(cmd_pre_meander[0])); ++i)
+    {
+        enquecommand_P(static_cast<char*>(pgm_read_ptr(&cmd_pre_meander[i])));
+    }
+}
+

 //! @brief Count extrude length
 //!
@ -18,247 +131,83 @@
 //! @param extrusion_width extrusion width in mm
 //! @param extrusion_length extrusion length in mm
 //! @return filament length in mm which needs to be extruded to form line
-static constexpr float __attribute__((noinline)) count_e(float layer_height, float extrusion_width, float extrusion_length, float filament_diameter=1.75f)
+static constexpr float count_e(float layer_height, float extrusion_width, float extrusion_length)
 {
-    return (extrusion_length * ((M_PI * pow(layer_height, 2)) / 4 + layer_height * (extrusion_width - layer_height))) / ((M_PI * pow(filament_diameter, 2)) / 4);
+    return (extrusion_length * layer_height * extrusion_width / (M_PI * pow(1.75, 2) / 4));
 }

-//! @brief Extrusion spacing
-//!
-//! @param layer_height layer height in mm
-//! @param extrusion_width extrusion width in mm
-//! @return filament length in mm which needs to be extruded to form line
-static constexpr float spacing(float layer_height, float extrusion_width, float overlap_factor=1.f)
-{
-    return extrusion_width - layer_height * (overlap_factor - M_PI/4);
-}
-
-// Common code extracted into one function to reduce code size
-static void lay1cal_common_enqueue_loop(const char * const * cmd_sequence, const uint8_t steps) {
-    for (uint8_t i = 0; i < steps; ++i)
-    {
-        void * const pgm_ptr = pgm_read_ptr(cmd_sequence + i);
-
-        // M702 is currently only used with MMU enabled
-        if (pgm_ptr == MSG_M702 && !MMU2::mmu2.Enabled()) {
-            continue;
-        }
-
-        enquecommand_P(static_cast<char*>(pgm_ptr));
-    }
-}
-
-static const char extrude_fmt_X[] PROGMEM = "G1X%.4fE%.4f";
-static const char extrude_fmt_Y[] PROGMEM = "G1Y%.4fE%.4f";
-static const char zero_extrusion[] PROGMEM = "G92E0";
-static const char feedrate_F1080[] PROGMEM = "G1F1080";
-#ifndef NEW_FIRST_LAYER_CAL
-static constexpr int8_t invert = 1;
-static constexpr float short_length = 20;
-static constexpr float square_width = short_length;
-#else
-static constexpr int8_t invert = -1;
-static constexpr float short_length = 13.2812; //max_pos[1]/2 / meander * 2
-static constexpr float square_width = short_length*2;
-#endif //NEW_FIRST_LAYER_CAL
-static constexpr float long_length = 150;
-
-//! @brief Wait for preheat
-void lay1cal_wait_preheat()
-{
-    static const char preheat_cmd_2[] PROGMEM = "M190";
-    static const char preheat_cmd_3[] PROGMEM = "M109";
-    static const char preheat_cmd_4[] PROGMEM = "G28";
-
-    static const char * const preheat_cmd[] PROGMEM =
-    {
-        MSG_M107,
-        preheat_cmd_2,
-        preheat_cmd_3,
-        preheat_cmd_4,
-        zero_extrusion
-    };
-
-    lay1cal_common_enqueue_loop(preheat_cmd, sizeof(preheat_cmd)/sizeof(preheat_cmd[0]));
-}
-
-//! @brief Load filament
-//! @param cmd_buffer character buffer needed to format gcodes
-//! @param filament filament to use (applies for MMU only)
-//! @returns true if extra purge distance is needed in case of MMU prints (after a toolchange), otherwise false
-bool lay1cal_load_filament(uint8_t filament)
-{
-    if (MMU2::mmu2.Enabled())
-    {
-        enquecommand_P(MSG_M83);
-        enquecommand_P(PSTR("G1Y-3F1000"));
-        enquecommand_P(PSTR("G1Z0.4"));
-
-        uint8_t currentTool = MMU2::mmu2.get_current_tool();
-        if(currentTool == filament ){
-            // already have the correct tool loaded - do nothing
-            return false;
-        } else if( currentTool != (uint8_t)MMU2::FILAMENT_UNKNOWN){
-            // some other slot is loaded, perform an unload first
-            enquecommand_P(MSG_M702);
-        }
-        // perform a toolchange
-        enquecommandf_P(PSTR("T%d"), filament);
-        return true;
-    }
-    return false;
-}
-
-//! @brief Print intro line
-//! @param extraPurgeNeeded false if the first MMU-related "G1 E29" have to be skipped because the nozzle is already full of filament
-//! @param layer_height the height of the calibration layer
-//! @param extrusion_width the width of the extrusion layer
-void lay1cal_intro_line(bool extraPurgeNeeded, float layer_height, float extrusion_width)
-{
-    static const char cmd_intro_mmu_3[] PROGMEM = "G1X55E29F1073";
-    static const char cmd_intro_mmu_4[] PROGMEM = "G1X5E29F1800";
-    static const char cmd_intro_mmu_5[] PROGMEM = "G1X55E8F2000";
-    static const char cmd_intro_mmu_6[] PROGMEM = "G1Z0.3F1000";
-    static const char cmd_intro_mmu_8[] PROGMEM = "G1X240E25F2200";
-    static const char cmd_intro_mmu_9[] PROGMEM = "G1Y-2F1000";
-    static const char cmd_intro_mmu_10[] PROGMEM = "G1X202.5E8F1400";
-    static const char cmd_intro_mmu_11[] PROGMEM = "G1Z0.2";
-    static const char * const cmd_intro_mmu[] PROGMEM =
-    {
-        // first 2 items are only relevant if filament was not loaded - i.e. extraPurgeNeeded == true
-        cmd_intro_mmu_3,
-        cmd_intro_mmu_4,
-        cmd_intro_mmu_5,
-        cmd_intro_mmu_6,
-        zero_extrusion,
-        cmd_intro_mmu_8,
-        cmd_intro_mmu_9,
-        cmd_intro_mmu_10,
-        cmd_intro_mmu_11,
-    };
-
-    if (MMU2::mmu2.Enabled())
-    {
-        for (uint8_t i = (extraPurgeNeeded ? 0 : 2); i < (sizeof(cmd_intro_mmu)/sizeof(cmd_intro_mmu[0])); ++i)
-        {
-            enquecommand_P(static_cast<char*>(pgm_read_ptr(&cmd_intro_mmu[i])));
-        }
-    }
-    else
-    {
-        enquecommand_P(feedrate_F1080); //fixed velocity for the intro line
-        enquecommandf_P(extrude_fmt_X, 60.f, count_e(layer_height, extrusion_width * 4.f, 60));
-        enquecommandf_P(extrude_fmt_X, 202.5f, count_e(layer_height, extrusion_width * 8.f, 142.5));
-    }
-}
-
-//! @brief Setup for printing meander
-void lay1cal_before_meander()
-{
-#ifndef NEW_FIRST_LAYER_CAL
-    static const char cmd_pre_meander_4[] PROGMEM = "G1E-1.5F2100";
-    static const char cmd_pre_meander_5[] PROGMEM = "G1Z5F7200";
-#endif //NEW_FIRST_LAYER_CAL
-    static const char cmd_pre_meander_6[] PROGMEM = "M204S1000"; //set acceleration
-
-    static const char * const cmd_pre_meander[] PROGMEM =
-    {
-            zero_extrusion,
-            MSG_G90,
-            MSG_M83, // use relative distances for extrusion
-#ifndef NEW_FIRST_LAYER_CAL
-            cmd_pre_meander_4,
-            cmd_pre_meander_5,
-#endif //NEW_FIRST_LAYER_CAL
-            cmd_pre_meander_6,
-    };
-
-    lay1cal_common_enqueue_loop(cmd_pre_meander, (sizeof(cmd_pre_meander)/sizeof(cmd_pre_meander[0])));
-}
-
-//! @brief Print meander start
-void lay1cal_meander_start(float layer_height, float extrusion_width)
-{
-#ifndef NEW_FIRST_LAYER_CAL
-    enquecommand_P(PSTR("G1X50Y155"));
-#endif //_NEW_FIRST_LAYER_CAL
-    static const char fmt1[] PROGMEM = "G1Z%.2f";
-    enquecommandf_P(fmt1, layer_height);
-    enquecommand_P(feedrate_F1080);
-    enquecommand_P(MSG_G91); //enable relative XYZ
-#ifdef NEW_FIRST_LAYER_CAL
-    enquecommandf_P(extrude_fmt_Y, short_length, count_e(layer_height, extrusion_width, short_length));
-    enquecommandf_P(extrude_fmt_X, long_length*invert, count_e(layer_height, extrusion_width, long_length));
-    enquecommandf_P(extrude_fmt_Y, -short_length*invert, count_e(layer_height, extrusion_width, short_length));
-#else
-    enquecommandf_P(extrude_fmt_X, 25.f*invert, count_e(layer_height, extrusion_width * 4.f, 25));
-    enquecommandf_P(extrude_fmt_X, 25.f*invert, count_e(layer_height, extrusion_width * 2.f, 25));
-    enquecommandf_P(extrude_fmt_X, 100.f*invert, count_e(layer_height, extrusion_width, 100));
-    enquecommandf_P(extrude_fmt_Y, -20.f*invert, count_e(layer_height, extrusion_width, 20));
-#endif //_NEW_FIRST_LAYER_CAL
-}
+static const float width = 0.4; //!< line width
+static const float length = 20 - width; //!< line length
+static const float height = 0.2; //!< layer height TODO This is wrong, as current Z height is 0.15 mm
+static const float extr = count_e(height, width, length); //!< E axis movement needed to print line

 //! @brief Print meander
 //! @param cmd_buffer character buffer needed to format gcodes
-void lay1cal_meander(float layer_height, float extrusion_width)
+void lay1cal_meander(char *cmd_buffer)
 {
-    const float long_extrusion = count_e(layer_height, extrusion_width, long_length);
-    const float short_extrusion = count_e(layer_height, extrusion_width, short_length);
+    static const char cmd_meander_0[] PROGMEM = "G1 X50 Y155";
+    static const char cmd_meander_1[] PROGMEM = "G1 Z0.150 F7200.000";
+    static const char cmd_meander_2[] PROGMEM = "G1 F1080";
+    static const char cmd_meander_3[] PROGMEM = "G1 X75 Y155 E2.5";
+    static const char cmd_meander_4[] PROGMEM = "G1 X100 Y155 E2";
+    static const char cmd_meander_5[] PROGMEM = "G1 X200 Y155 E2.62773";
+    static const char cmd_meander_6[] PROGMEM = "G1 X200 Y135 E0.66174";
+    static const char cmd_meander_7[] PROGMEM = "G1 X50 Y135 E3.62773";
+    static const char cmd_meander_8[] PROGMEM = "G1 X50 Y115 E0.49386";
+    static const char cmd_meander_9[] PROGMEM = "G1 X200 Y115 E3.62773";
+    static const char cmd_meander_10[] PROGMEM = "G1 X200 Y95 E0.49386";
+    static const char cmd_meander_11[] PROGMEM = "G1 X50 Y95 E3.62773";
+    static const char cmd_meander_12[] PROGMEM = "G1 X50 Y75 E0.49386";
+    static const char cmd_meander_13[] PROGMEM = "G1 X200 Y75 E3.62773";
+    static const char cmd_meander_14[] PROGMEM = "G1 X200 Y55 E0.49386";
+    static const char cmd_meander_15[] PROGMEM = "G1 X50 Y55 E3.62773";

-    for(int8_t i = 0, xdir = -invert; i <= 4; i++, xdir = -xdir)
+    static const char * const cmd_meander[] PROGMEM =
    {
-        enquecommandf_P(extrude_fmt_X, xdir * long_length, long_extrusion);
-        enquecommandf_P(extrude_fmt_Y, invert * -short_length, short_extrusion);
-    }
-#ifdef NEW_FIRST_LAYER_CAL
-    constexpr float mid_length = 0.5f * long_length - 0.5f * square_width;
-    const float mid_extrusion = count_e(layer_height, extrusion_width, mid_length);
-    enquecommandf_P(extrude_fmt_X, -mid_length, mid_extrusion); //~Middle of bed X125
-    enquecommandf_P(extrude_fmt_Y, short_length, short_extrusion); //~Middle of bed Y105
-#endif //NEW_FIRST_LAYER_CAL
+        cmd_meander_0,
+        cmd_meander_1,
+        cmd_meander_2,
+        cmd_meander_3,
+        cmd_meander_4,
+        cmd_meander_5,
+        cmd_meander_6,
+        cmd_meander_7,
+        cmd_meander_8,
+        cmd_meander_9,
+        cmd_meander_10,
+        cmd_meander_11,
+        cmd_meander_12,
+        cmd_meander_13,
+        cmd_meander_14,
+        cmd_meander_15,
+    };

+    for (uint8_t i = 0; i < (sizeof(cmd_meander)/sizeof(cmd_meander[0])); ++i)
+    {
+        enquecommand_P(static_cast<char*>(pgm_read_ptr(&cmd_meander[i])));
+    }
+    sprintf_P(cmd_buffer, PSTR("G1 X50 Y35 E%-.3f"), extr);
+    enquecommand(cmd_buffer);
 }

 //! @brief Print square
 //!
-//! This function enqueues 4 lines of the square, so it needs to be called multiple times
+//! This function needs to be called 16 times for i from 0 to 15.
 //!
 //! @param cmd_buffer character buffer needed to format gcodes
-void lay1cal_square(float layer_height, float extrusion_width)
+//! @param i iteration
+void lay1cal_square(char *cmd_buffer, uint8_t i)
 {
-    const float Y_spacing = spacing(layer_height, extrusion_width);
-    const float long_extrusion = count_e(layer_height, extrusion_width, square_width);
-    const float short_extrusion = count_e(layer_height, extrusion_width, Y_spacing);
+    const float extr_short_segment = count_e(height, width, width);

-    for (uint8_t i = 0; i < 4; i++)
-    {
-        enquecommandf_P(extrude_fmt_X, square_width*invert, long_extrusion);
-        enquecommandf_P(extrude_fmt_Y, -Y_spacing*invert, short_extrusion);
-        enquecommandf_P(extrude_fmt_X, -square_width*invert, long_extrusion);
-        enquecommandf_P(extrude_fmt_Y, -Y_spacing*invert, short_extrusion);
-    }
-}
-
-void lay1cal_finish()
-{
-    static const char cmd_cal_finish_3[] PROGMEM = "G1E-0.075F2100"; // Retract
-    static const char cmd_cal_finish_4[] PROGMEM = "M140S0";         // Turn off bed heater
-    static const char cmd_cal_finish_5[] PROGMEM = "G1Z10F1300";     // Lift Z
-    static const char cmd_cal_finish_6[] PROGMEM = "G1X10Y180F4000"; // Go to parking position
-    static const char cmd_cal_finish_8[] PROGMEM = "M104S0";         // Turn off hotend heater
-
-    static const char * const cmd_cal_finish[] PROGMEM =
-    {
-        MSG_G90,          // Set to Absolute Positioning
-        MSG_M107,         // Turn off printer fan
-        cmd_cal_finish_3, // Retract
-        cmd_cal_finish_4, // Turn off bed heater
-        cmd_cal_finish_5, // Lift Z
-        cmd_cal_finish_6, // Go to parking position
-        MSG_M702,         // Unload filament (MMU only)
-        cmd_cal_finish_8, // Turn off hotend heater
-        MSG_M84           // Disable stepper motors
-    };
-
-    lay1cal_common_enqueue_loop(cmd_cal_finish, (sizeof(cmd_cal_finish)/sizeof(cmd_cal_finish[0])));
+    static const char fmt1[] PROGMEM = "G1 X%d Y%-.2f E%-.3f";
+    static const char fmt2[] PROGMEM = "G1 Y%-.2f E%-.3f";
+    sprintf_P(cmd_buffer, fmt1, 70, (35 - i*width * 2), extr);
+    enquecommand(cmd_buffer);
+    sprintf_P(cmd_buffer, fmt2, (35 - (2 * i + 1)*width), extr_short_segment);
+    enquecommand(cmd_buffer);
+    sprintf_P(cmd_buffer, fmt1, 50, (35 - (2 * i + 1)*width), extr);
+    enquecommand(cmd_buffer);
+    sprintf_P(cmd_buffer, fmt2, (35 - (i + 1)*width * 2), extr_short_segment);
+    enquecommand(cmd_buffer);
 }
--- a/Firmware/first_lay_cal.h
+++ b/Firmware/first_lay_cal.h
@ -7,12 +7,10 @@
 #include <stdint.h>

 void lay1cal_wait_preheat();
-[[nodiscard]] bool lay1cal_load_filament(uint8_t filament);
-void lay1cal_intro_line(bool skipExtraPurge, float layer_height, float extrusion_width);
+void lay1cal_load_filament(char *cmd_buffer, uint8_t filament);
+void lay1cal_intro_line();
 void lay1cal_before_meander();
-void lay1cal_meander_start(float layer_height, float extrusion_width);
-void lay1cal_meander(float layer_height, float extrusion_width);
-void lay1cal_square(float layer_height, float extrusion_width);
-void lay1cal_finish();
+void lay1cal_meander(char *cmd_buffer);
+void lay1cal_square(char *cmd_buffer, uint8_t i);

 #endif /* FIRMWARE_FIRST_LAY_CAL_H_ */
--- a/Firmware/fsensor.cpp
+++ b/Firmware/fsensor.cpp
@ -0,0 +1,789 @@
+//! @file
+
+#include "Marlin.h"
+
+#include "fsensor.h"
+#include <avr/pgmspace.h>
+#include "pat9125.h"
+#include "stepper.h"
+#include "io_atmega2560.h"
+#include "cmdqueue.h"
+#include "ultralcd.h"
+#include "mmu.h"
+#include "cardreader.h"
+
+#include "adc.h"
+#include "temperature.h"
+#include "config.h"
+
+//! @name Basic parameters
+//! @{
+#define FSENSOR_CHUNK_LEN      1.25 //!< filament sensor chunk length (mm)
+#define FSENSOR_ERR_MAX           4 //!< filament sensor maximum error/chunk count for runout detection
+
+#define FSENSOR_SOFTERR_CMAX      3 //!< number of contiguous soft failures before a triggering a runout
+#define FSENSOR_SOFTERR_DELTA 30000 //!< maximum interval (ms) to consider soft failures contiguous
+//! @}
+
+//! @name Optical quality measurement parameters
+//! @{
+#define FSENSOR_OQ_MAX_ES      2    //!< maximum sum of error blocks during filament recheck
+#define FSENSOR_OQ_MIN_YD      2    //!< minimum yd sum during filament check (counts per inch)
+#define FSENSOR_OQ_MIN_BR      80   //!< minimum brightness value
+#define FSENSOR_OQ_MAX_SH      10   //!< maximum shutter value
+//! @}
+
+const char ERRMSG_PAT9125_NOT_RESP[] PROGMEM = "PAT9125 not responding (%d)!\n";
+
+// PJ7 can not be used (does not have PinChangeInterrupt possibility)
+#define FSENSOR_INT_PIN          75 //!< filament sensor interrupt pin PJ4
+#define FSENSOR_INT_PIN_MASK   0x10 //!< filament sensor interrupt pin mask (bit4)
+#define FSENSOR_INT_PIN_PIN_REG PINJ              // PIN register @ PJ4
+#define FSENSOR_INT_PIN_VECT PCINT1_vect          // PinChange ISR @ PJ4
+#define FSENSOR_INT_PIN_PCMSK_REG PCMSK1          // PinChangeMaskRegister @ PJ4
+#define FSENSOR_INT_PIN_PCMSK_BIT PCINT13         // PinChange Interrupt / PinChange Enable Mask @ PJ4
+#define FSENSOR_INT_PIN_PCICR_BIT PCIE1           // PinChange Interrupt Enable / Flag @ PJ4
+
+//! enabled = initialized and sampled every chunk event
+bool fsensor_enabled = true;
+//! runout watching is done in fsensor_update (called from main loop)
+bool fsensor_watch_runout = true;
+//! not responding - is set if any communication error occurred during initialization or readout
+bool fsensor_not_responding = false;
+
+#ifdef PAT9125
+uint8_t fsensor_int_pin_old = 0;
+//! optical checking "chunk lenght" (already in steps)
+int16_t fsensor_chunk_len = 0;
+//! enable/disable quality meassurement
+bool fsensor_oq_meassure_enabled = false;
+//! number of errors, updated in ISR
+uint8_t fsensor_err_cnt = 0;
+//! variable for accumulating step count (updated callbacks from stepper and ISR)
+int16_t fsensor_st_cnt = 0;
+//! count of total sensor "soft" failures (filament status checks)
+uint8_t fsensor_softfail = 0;
+//! timestamp of last soft failure
+unsigned long fsensor_softfail_last = 0;
+//! count of soft failures within the configured time
+uint8_t fsensor_softfail_ccnt = 0;
+#endif
+
+#ifdef DEBUG_FSENSOR_LOG
+//! log flag: 0=log disabled, 1=log enabled
+uint8_t fsensor_log = 1;
+#endif //DEBUG_FSENSOR_LOG
+
+
+//! @name filament autoload variables
+//! @{
+
+//! autoload feature enabled
+bool fsensor_autoload_enabled = true;
+//! autoload watching enable/disable flag
+bool fsensor_watch_autoload = false;
+
+#ifdef PAT9125
+//
+uint16_t fsensor_autoload_y;
+//
+uint8_t fsensor_autoload_c;
+//
+uint32_t fsensor_autoload_last_millis;
+//
+uint8_t fsensor_autoload_sum;
+//! @}
+#endif
+
+
+//! @name filament optical quality measurement variables
+//! @{
+
+//! Measurement enable/disable flag
+bool fsensor_oq_meassure = false;
+//! skip-chunk counter, for accurate measurement is necessary to skip first chunk...
+uint8_t  fsensor_oq_skipchunk;
+//! number of samples from start of measurement
+uint8_t fsensor_oq_samples;
+//! sum of steps in positive direction movements
+uint16_t fsensor_oq_st_sum;
+//! sum of deltas in positive direction movements
+uint16_t fsensor_oq_yd_sum;
+//! sum of errors during measurement
+uint16_t fsensor_oq_er_sum;
+//! max error counter value during measurement
+uint8_t  fsensor_oq_er_max;
+//! minimum delta value
+int16_t fsensor_oq_yd_min;
+//! maximum delta value
+int16_t fsensor_oq_yd_max;
+//! sum of shutter value
+uint16_t fsensor_oq_sh_sum;
+//! @}
+
+#ifdef IR_SENSOR_ANALOG
+ClFsensorPCB oFsensorPCB;
+ClFsensorActionNA oFsensorActionNA;
+bool bIRsensorStateFlag=false;
+unsigned long nIRsensorLastTime;
+#endif //IR_SENSOR_ANALOG
+
+void fsensor_stop_and_save_print(void)
+{
+    printf_P(PSTR("fsensor_stop_and_save_print\n"));
+    stop_and_save_print_to_ram(0, 0);
+    fsensor_watch_runout = false;
+}
+
+#ifdef PAT9125
+// Reset all internal counters to zero, including stepper callbacks
+void fsensor_reset_err_cnt()
+{
+    fsensor_err_cnt = 0;
+    pat9125_y = 0;
+    st_reset_fsensor();
+}
+
+void fsensor_set_axis_steps_per_unit(float u)
+{
+    fsensor_chunk_len = (int16_t)(FSENSOR_CHUNK_LEN * u);
+}
+#endif
+
+
+void fsensor_restore_print_and_continue(void)
+{
+    printf_P(PSTR("fsensor_restore_print_and_continue\n"));
+    fsensor_watch_runout = true;
+#ifdef PAT9125
+    fsensor_reset_err_cnt();
+#endif
+    restore_print_from_ram_and_continue(0);
+}
+
+// fsensor_checkpoint_print cuts the current print job at the current position,
+// allowing new instructions to be inserted in the middle
+void fsensor_checkpoint_print(void)
+{
+    printf_P(PSTR("fsensor_checkpoint_print\n"));
+    stop_and_save_print_to_ram(0, 0);
+    restore_print_from_ram_and_continue(0);
+}
+
+#ifdef IR_SENSOR_ANALOG
+const char* FsensorIRVersionText()
+{
+	switch(oFsensorPCB)
+	{
+		case ClFsensorPCB::_Old:
+			return _T(MSG_IR_03_OR_OLDER);
+		case ClFsensorPCB::_Rev04:
+			return _T(MSG_IR_04_OR_NEWER);
+		default:
+			return _T(MSG_IR_UNKNOWN);
+	}
+}
+#endif //IR_SENSOR_ANALOG
+
+void fsensor_init(void)
+{
+#ifdef PAT9125
+	uint8_t pat9125 = pat9125_init();
+	printf_P(PSTR("PAT9125_init:%hhu\n"), pat9125);
+#endif //PAT9125
+	uint8_t fsensor_enabled = eeprom_read_byte((uint8_t*)EEPROM_FSENSOR);
+	fsensor_autoload_enabled=eeprom_read_byte((uint8_t*)EEPROM_FSENS_AUTOLOAD_ENABLED);
+	fsensor_not_responding = false;
+#ifdef PAT9125
+	uint8_t oq_meassure_enabled = eeprom_read_byte((uint8_t*)EEPROM_FSENS_OQ_MEASS_ENABLED);
+	fsensor_oq_meassure_enabled = (oq_meassure_enabled == 1)?true:false;
+	fsensor_set_axis_steps_per_unit(cs.axis_steps_per_unit[E_AXIS]);
+	
+	if (!pat9125){
+		fsensor_enabled = 0; //disable sensor
+		fsensor_not_responding = true;
+	}
+#endif //PAT9125
+#ifdef IR_SENSOR_ANALOG
+	bIRsensorStateFlag=false;
+	oFsensorPCB = (ClFsensorPCB)eeprom_read_byte((uint8_t*)EEPROM_FSENSOR_PCB);
+	oFsensorActionNA = (ClFsensorActionNA)eeprom_read_byte((uint8_t*)EEPROM_FSENSOR_ACTION_NA);
+
+	// If the fsensor is not responding even at the start of the printer,
+	// set this flag accordingly to show N/A in Settings->Filament sensor.
+	// This is even valid for both fsensor board revisions (0.3 or older and 0.4).
+	// Must be done after reading what type of fsensor board we have
+	fsensor_not_responding = ! fsensor_IR_check();
+#endif //IR_SENSOR_ANALOG
+	if (fsensor_enabled){
+		fsensor_enable(false);                  // (in this case) EEPROM update is not necessary
+	} else {
+		fsensor_disable(false);                 // (in this case) EEPROM update is not necessary
+	}
+	printf_P(PSTR("FSensor %S"), (fsensor_enabled?PSTR("ENABLED"):PSTR("DISABLED")));
+#ifdef IR_SENSOR_ANALOG
+	printf_P(PSTR(" (sensor board revision:%S)\n"), FsensorIRVersionText());
+#else //IR_SENSOR_ANALOG
+	MYSERIAL.println();
+#endif //IR_SENSOR_ANALOG
+	if (check_for_ir_sensor()){
+		ir_sensor_detected = true;
+	}
+}
+
+bool fsensor_enable(bool bUpdateEEPROM)
+{
+#ifdef PAT9125
+	if (mmu_enabled == false) { //filament sensor is pat9125, enable only if it is working
+		uint8_t pat9125 = pat9125_init();
+		printf_P(PSTR("PAT9125_init:%hhu\n"), pat9125);
+		if (pat9125)
+			fsensor_not_responding = false;
+		else
+			fsensor_not_responding = true;
+		fsensor_enabled = pat9125 ? true : false;
+		fsensor_watch_runout = true;
+		fsensor_oq_meassure = false;
+        fsensor_reset_err_cnt();
+		eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, fsensor_enabled ? 0x01 : 0x00);
+		FSensorStateMenu = fsensor_enabled ? 1 : 0;
+	}
+	else //filament sensor is FINDA, always enable 
+	{
+		fsensor_enabled = true;
+		eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, 0x01);
+		FSensorStateMenu = 1;
+	}
+#else // PAT9125
+#ifdef IR_SENSOR_ANALOG
+     if(!fsensor_IR_check())
+          {
+          bUpdateEEPROM=true;
+          fsensor_enabled=false;
+          fsensor_not_responding=true;
+          FSensorStateMenu=0;
+          }
+     else {
+#endif //IR_SENSOR_ANALOG
+     fsensor_enabled=true;
+     fsensor_not_responding=false;
+     FSensorStateMenu=1;
+#ifdef IR_SENSOR_ANALOG
+          }
+#endif //IR_SENSOR_ANALOG
+     if(bUpdateEEPROM)
+          eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, FSensorStateMenu);
+#endif //PAT9125
+	return fsensor_enabled;
+}
+
+void fsensor_disable(bool bUpdateEEPROM)
+{ 
+	fsensor_enabled = false;
+	FSensorStateMenu = 0;
+     if(bUpdateEEPROM)
+          eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, 0x00); 
+}
+
+void fsensor_autoload_set(bool State)
+{
+#ifdef PAT9125
+	if (!State) fsensor_autoload_check_stop();
+#endif //PAT9125
+	fsensor_autoload_enabled = State;
+	eeprom_update_byte((unsigned char *)EEPROM_FSENS_AUTOLOAD_ENABLED, fsensor_autoload_enabled);
+}
+
+void pciSetup(byte pin)
+{
+// !!! "digitalPinTo?????bit()" does not provide the correct results for some MCU pins
+	*digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin)); // enable pin
+	PCIFR |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
+	PCICR |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group 
+}
+
+#ifdef PAT9125
+void fsensor_autoload_check_start(void)
+{
+//	puts_P(_N("fsensor_autoload_check_start\n"));
+	if (!fsensor_enabled) return;
+	if (!fsensor_autoload_enabled) return;
+	if (fsensor_watch_autoload) return;
+	if (!pat9125_update()) //update sensor
+	{
+		fsensor_disable();
+		fsensor_not_responding = true;
+		fsensor_watch_autoload = false;
+		printf_P(ERRMSG_PAT9125_NOT_RESP, 3);
+		return;
+	}
+	puts_P(_N("fsensor_autoload_check_start - autoload ENABLED\n"));
+	fsensor_autoload_y = pat9125_y; //save current y value
+	fsensor_autoload_c = 0; //reset number of changes counter
+	fsensor_autoload_sum = 0;
+	fsensor_autoload_last_millis = _millis();
+	fsensor_watch_runout = false;
+	fsensor_watch_autoload = true;
+}
+
+
+void fsensor_autoload_check_stop(void)
+{
+//	puts_P(_N("fsensor_autoload_check_stop\n"));
+	if (!fsensor_enabled) return;
+//	puts_P(_N("fsensor_autoload_check_stop 1\n"));
+	if (!fsensor_autoload_enabled) return;
+//	puts_P(_N("fsensor_autoload_check_stop 2\n"));
+	if (!fsensor_watch_autoload) return;
+	puts_P(_N("fsensor_autoload_check_stop - autoload DISABLED\n"));
+	fsensor_autoload_sum = 0;
+	fsensor_watch_autoload = false;
+	fsensor_watch_runout = true;
+    fsensor_reset_err_cnt();
+}
+#endif //PAT9125
+
+bool fsensor_check_autoload(void)
+{
+	if (!fsensor_enabled) return false;
+	if (!fsensor_autoload_enabled) return false;
+	if (ir_sensor_detected) {
+		if (digitalRead(IR_SENSOR_PIN) == 1) {
+			fsensor_watch_autoload = true;
+		}
+		else if (fsensor_watch_autoload == true) {
+			fsensor_watch_autoload = false;
+			return true;
+		}
+	}
+#ifdef PAT9125
+	if (!fsensor_watch_autoload)
+	{
+		fsensor_autoload_check_start();
+		return false;
+	}
+#if 0
+	uint8_t fsensor_autoload_c_old = fsensor_autoload_c;
+#endif
+	if ((_millis() - fsensor_autoload_last_millis) < 25) return false;
+	fsensor_autoload_last_millis = _millis();
+	if (!pat9125_update_y()) //update sensor
+	{
+		fsensor_disable();
+		fsensor_not_responding = true;
+		printf_P(ERRMSG_PAT9125_NOT_RESP, 2);
+		return false;
+	}
+	int16_t dy = pat9125_y - fsensor_autoload_y;
+	if (dy) //? dy value is nonzero
+	{
+		if (dy > 0) //? delta-y value is positive (inserting)
+		{
+			fsensor_autoload_sum += dy;
+			fsensor_autoload_c += 3; //increment change counter by 3
+		}
+		else if (fsensor_autoload_c > 1)
+			fsensor_autoload_c -= 2; //decrement change counter by 2 
+		fsensor_autoload_y = pat9125_y; //save current value
+	}
+	else if (fsensor_autoload_c > 0)
+		fsensor_autoload_c--;
+	if (fsensor_autoload_c == 0) fsensor_autoload_sum = 0;
+#if 0
+  	puts_P(_N("fsensor_check_autoload\n"));
+  	if (fsensor_autoload_c != fsensor_autoload_c_old)
+  		printf_P(PSTR("fsensor_check_autoload dy=%d c=%d sum=%d\n"), dy, fsensor_autoload_c, fsensor_autoload_sum);
+#endif
+//	if ((fsensor_autoload_c >= 15) && (fsensor_autoload_sum > 30))
+	if ((fsensor_autoload_c >= 12) && (fsensor_autoload_sum > 20))
+	{
+//		puts_P(_N("fsensor_check_autoload = true !!!\n"));
+		return true;
+	}
+#endif //PAT9125
+	return false;
+}
+
+#ifdef PAT9125
+void fsensor_oq_meassure_set(bool State)
+{
+	fsensor_oq_meassure_enabled = State;
+	eeprom_update_byte((unsigned char *)EEPROM_FSENS_OQ_MEASS_ENABLED, fsensor_oq_meassure_enabled);
+}
+
+void fsensor_oq_meassure_start(uint8_t skip)
+{
+	if (!fsensor_enabled) return;
+	if (!fsensor_oq_meassure_enabled) return;
+	printf_P(PSTR("fsensor_oq_meassure_start\n"));
+	fsensor_oq_skipchunk = skip;
+	fsensor_oq_samples = 0;
+	fsensor_oq_st_sum = 0;
+	fsensor_oq_yd_sum = 0;
+	fsensor_oq_er_sum = 0;
+	fsensor_oq_er_max = 0;
+	fsensor_oq_yd_min = INT16_MAX;
+	fsensor_oq_yd_max = 0;
+	fsensor_oq_sh_sum = 0;
+	pat9125_update();
+	pat9125_y = 0;
+	fsensor_oq_meassure = true;
+}
+
+void fsensor_oq_meassure_stop(void)
+{
+	if (!fsensor_enabled) return;
+	if (!fsensor_oq_meassure_enabled) return;
+	printf_P(PSTR("fsensor_oq_meassure_stop, %hhu samples\n"), fsensor_oq_samples);
+	printf_P(_N(" st_sum=%u yd_sum=%u er_sum=%u er_max=%hhu\n"), fsensor_oq_st_sum, fsensor_oq_yd_sum, fsensor_oq_er_sum, fsensor_oq_er_max);
+	printf_P(_N(" yd_min=%u yd_max=%u yd_avg=%u sh_avg=%u\n"), fsensor_oq_yd_min, fsensor_oq_yd_max, (uint16_t)((uint32_t)fsensor_oq_yd_sum * fsensor_chunk_len / fsensor_oq_st_sum), (uint16_t)(fsensor_oq_sh_sum / fsensor_oq_samples));
+	fsensor_oq_meassure = false;
+}
+
+#ifdef FSENSOR_QUALITY
+const char _OK[] PROGMEM = "OK";
+const char _NG[] PROGMEM = "NG!";
+
+bool fsensor_oq_result(void)
+{
+	if (!fsensor_enabled) return true;
+	if (!fsensor_oq_meassure_enabled) return true;
+	printf_P(_N("fsensor_oq_result\n"));
+	bool res_er_sum = (fsensor_oq_er_sum <= FSENSOR_OQ_MAX_ES);
+	printf_P(_N(" er_sum = %u %S\n"), fsensor_oq_er_sum, (res_er_sum?_OK:_NG));
+	bool res_er_max = (fsensor_oq_er_max <= FSENSOR_OQ_MAX_EM);
+	printf_P(_N(" er_max = %hhu %S\n"), fsensor_oq_er_max, (res_er_max?_OK:_NG));
+	uint8_t yd_avg = ((uint32_t)fsensor_oq_yd_sum * fsensor_chunk_len / fsensor_oq_st_sum);
+	bool res_yd_avg = (yd_avg >= FSENSOR_OQ_MIN_YD) && (yd_avg <= FSENSOR_OQ_MAX_YD);
+	printf_P(_N(" yd_avg = %hhu %S\n"), yd_avg, (res_yd_avg?_OK:_NG));
+	bool res_yd_max = (fsensor_oq_yd_max <= (yd_avg * FSENSOR_OQ_MAX_PD));
+	printf_P(_N(" yd_max = %u %S\n"), fsensor_oq_yd_max, (res_yd_max?_OK:_NG));
+	bool res_yd_min = (fsensor_oq_yd_min >= (yd_avg / FSENSOR_OQ_MAX_ND));
+	printf_P(_N(" yd_min = %u %S\n"), fsensor_oq_yd_min, (res_yd_min?_OK:_NG));
+
+	uint16_t yd_dev = (fsensor_oq_yd_max - yd_avg) + (yd_avg - fsensor_oq_yd_min);
+	printf_P(_N(" yd_dev = %u\n"), yd_dev);
+
+	uint16_t yd_qua = 10 * yd_avg / (yd_dev + 1);
+	printf_P(_N(" yd_qua = %u %S\n"), yd_qua, ((yd_qua >= 8)?_OK:_NG));
+
+	uint8_t sh_avg = (fsensor_oq_sh_sum / fsensor_oq_samples);
+	bool res_sh_avg = (sh_avg <= FSENSOR_OQ_MAX_SH);
+	if (yd_qua >= 8) res_sh_avg = true;
+
+	printf_P(_N(" sh_avg = %hhu %S\n"), sh_avg, (res_sh_avg?_OK:_NG));
+	bool res = res_er_sum && res_er_max && res_yd_avg && res_yd_max && res_yd_min && res_sh_avg;
+	printf_P(_N("fsensor_oq_result %S\n"), (res?_OK:_NG));
+	return res;
+}
+#endif //FSENSOR_QUALITY
+
+FORCE_INLINE static void fsensor_isr(int st_cnt)
+{
+	uint8_t old_err_cnt = fsensor_err_cnt;
+	uint8_t pat9125_res = fsensor_oq_meassure?pat9125_update():pat9125_update_y();
+	if (!pat9125_res)
+	{
+		fsensor_disable();
+		fsensor_not_responding = true;
+		printf_P(ERRMSG_PAT9125_NOT_RESP, 1);
+	}
+
+	if (st_cnt != 0)
+	{
+        // movement was planned, check for sensor movement
+        int8_t st_dir = st_cnt >= 0;
+        int8_t pat9125_dir = pat9125_y >= 0;
+
+        if (pat9125_y == 0)
+        {
+            if (st_dir)
+            {
+                // no movement detected: we might be within a blind sensor range,
+                // update the frame and shutter parameters we didn't earlier
+                if (!fsensor_oq_meassure)
+                    pat9125_update_bs();
+
+                // increment the error count only if underexposed: filament likely missing
+                if ((pat9125_b < FSENSOR_OQ_MIN_BR) && (pat9125_s > FSENSOR_OQ_MAX_SH))
+                {
+                    // check for a dark frame (<30% avg brightness) with long exposure
+                    ++fsensor_err_cnt;
+                }
+                else
+                {
+                    // good frame, filament likely present
+                    if(fsensor_err_cnt) --fsensor_err_cnt;
+                }
+            }
+        }
+        else if (pat9125_dir != st_dir)
+        {
+            // detected direction opposite of motor movement
+            if (st_dir) ++fsensor_err_cnt;
+        }
+        else if (pat9125_dir == st_dir)
+        {
+            // direction agreeing with planned movement
+            if (fsensor_err_cnt) --fsensor_err_cnt;
+        }
+
+        if (st_dir && fsensor_oq_meassure)
+        {
+            // extruding with quality assessment
+            if (fsensor_oq_skipchunk)
+            {
+                fsensor_oq_skipchunk--;
+                fsensor_err_cnt = 0;
+            }
+            else
+            {
+                if (st_cnt == fsensor_chunk_len)
+                {
+                    if (pat9125_y > 0) if (fsensor_oq_yd_min > pat9125_y) fsensor_oq_yd_min = (fsensor_oq_yd_min + pat9125_y) / 2;
+                    if (pat9125_y >= 0) if (fsensor_oq_yd_max < pat9125_y) fsensor_oq_yd_max = (fsensor_oq_yd_max + pat9125_y) / 2;
+                }
+                fsensor_oq_samples++;
+                fsensor_oq_st_sum += st_cnt;
+                if (pat9125_y > 0) fsensor_oq_yd_sum += pat9125_y;
+                if (fsensor_err_cnt > old_err_cnt)
+                    fsensor_oq_er_sum += (fsensor_err_cnt - old_err_cnt);
+                if (fsensor_oq_er_max < fsensor_err_cnt)
+                    fsensor_oq_er_max = fsensor_err_cnt;
+                fsensor_oq_sh_sum += pat9125_s;
+            }
+        }
+	}
+
+#ifdef DEBUG_FSENSOR_LOG
+	if (fsensor_log)
+	{
+		printf_P(_N("FSENSOR cnt=%d dy=%d err=%hhu %S\n"), st_cnt, pat9125_y, fsensor_err_cnt, (fsensor_err_cnt > old_err_cnt)?_N("NG!"):_N("OK"));
+		if (fsensor_oq_meassure) printf_P(_N("FSENSOR st_sum=%u yd_sum=%u er_sum=%u er_max=%hhu yd_max=%u\n"), fsensor_oq_st_sum, fsensor_oq_yd_sum, fsensor_oq_er_sum, fsensor_oq_er_max, fsensor_oq_yd_max);
+	}
+#endif //DEBUG_FSENSOR_LOG
+
+	pat9125_y = 0;
+}
+
+ISR(FSENSOR_INT_PIN_VECT)
+{
+    if (mmu_enabled || ir_sensor_detected) return;
+    if (!((fsensor_int_pin_old ^ FSENSOR_INT_PIN_PIN_REG) & FSENSOR_INT_PIN_MASK)) return;
+    fsensor_int_pin_old = FSENSOR_INT_PIN_PIN_REG;
+
+    // prevent isr re-entry
+    static bool _lock = false;
+    if (!_lock)
+    {
+        // fetch fsensor_st_cnt atomically
+        int st_cnt = fsensor_st_cnt;
+        fsensor_st_cnt = 0;
+
+        _lock = true;
+        sei();
+        fsensor_isr(st_cnt);
+        cli();
+        _lock = false;
+    }
+}
+
+void fsensor_setup_interrupt(void)
+{
+
+	pinMode(FSENSOR_INT_PIN, OUTPUT);
+	digitalWrite(FSENSOR_INT_PIN, LOW);
+	fsensor_int_pin_old = 0;
+
+	//pciSetup(FSENSOR_INT_PIN);
+// !!! "pciSetup()" does not provide the correct results for some MCU pins
+// so interrupt registers settings:
+     FSENSOR_INT_PIN_PCMSK_REG |= bit(FSENSOR_INT_PIN_PCMSK_BIT); // enable corresponding PinChangeInterrupt (individual pin)
+     PCIFR |= bit(FSENSOR_INT_PIN_PCICR_BIT);     // clear previous occasional interrupt (set of pins)
+     PCICR |= bit(FSENSOR_INT_PIN_PCICR_BIT);     // enable corresponding PinChangeInterrupt (set of pins)
+}
+
+void fsensor_st_block_chunk(int cnt)
+{
+	if (!fsensor_enabled) return;
+	fsensor_st_cnt += cnt;
+
+    // !!! bit toggling (PINxn <- 1) (for PinChangeInterrupt) does not work for some MCU pins
+    if (PIN_GET(FSENSOR_INT_PIN)) {PIN_VAL(FSENSOR_INT_PIN, LOW);}
+    else {PIN_VAL(FSENSOR_INT_PIN, HIGH);}
+}
+#endif //PAT9125
+
+
+//! Common code for enqueing M600 and supplemental codes into the command queue.
+//! Used both for the IR sensor and the PAT9125
+void fsensor_enque_M600(){
+	printf_P(PSTR("fsensor_update - M600\n"));
+	eeprom_update_byte((uint8_t*)EEPROM_FERROR_COUNT, eeprom_read_byte((uint8_t*)EEPROM_FERROR_COUNT) + 1);
+	eeprom_update_word((uint16_t*)EEPROM_FERROR_COUNT_TOT, eeprom_read_word((uint16_t*)EEPROM_FERROR_COUNT_TOT) + 1);
+	enquecommand_front_P((PSTR("M600")));
+}
+
+//! @brief filament sensor update (perform M600 on filament runout)
+//!
+//! Works only if filament sensor is enabled.
+//! When the filament sensor error count is larger then FSENSOR_ERR_MAX, pauses print, tries to move filament back and forth.
+//! If there is still no plausible signal from filament sensor plans M600 (Filament change).
+void fsensor_update(void)
+{
+#ifdef PAT9125
+    if (fsensor_watch_runout && (fsensor_err_cnt > FSENSOR_ERR_MAX))
+        {
+            fsensor_stop_and_save_print();
+            KEEPALIVE_STATE(IN_HANDLER);
+
+            bool autoload_enabled_tmp = fsensor_autoload_enabled;
+            fsensor_autoload_enabled = false;
+            bool oq_meassure_enabled_tmp = fsensor_oq_meassure_enabled;
+            fsensor_oq_meassure_enabled = true;
+
+            // move the nozzle away while checking the filament
+            current_position[Z_AXIS] += 0.8;
+            if(current_position[Z_AXIS] > Z_MAX_POS) current_position[Z_AXIS] = Z_MAX_POS;
+            plan_buffer_line_curposXYZE(max_feedrate[Z_AXIS]);
+            st_synchronize();
+
+            // check the filament in isolation
+            fsensor_reset_err_cnt();
+            fsensor_oq_meassure_start(0);
+            float e_tmp = current_position[E_AXIS];
+            current_position[E_AXIS] -= 3;
+            plan_buffer_line_curposXYZE(250/60);
+            current_position[E_AXIS] = e_tmp;
+            plan_buffer_line_curposXYZE(200/60);
+            st_synchronize();
+            fsensor_oq_meassure_stop();
+
+            bool err = false;
+            err |= (fsensor_err_cnt > 0);                   // final error count is non-zero
+            err |= (fsensor_oq_er_sum > FSENSOR_OQ_MAX_ES); // total error count is above limit
+            err |= (fsensor_oq_yd_sum < FSENSOR_OQ_MIN_YD); // total measured distance is below limit
+
+            fsensor_restore_print_and_continue();
+            fsensor_autoload_enabled = autoload_enabled_tmp;
+            fsensor_oq_meassure_enabled = oq_meassure_enabled_tmp;
+            unsigned long now = _millis();
+            if (!err && (now - fsensor_softfail_last) > FSENSOR_SOFTERR_DELTA)
+                fsensor_softfail_ccnt = 0;
+            if (!err && fsensor_softfail_ccnt <= FSENSOR_SOFTERR_CMAX)
+            {
+                printf_P(PSTR("fsensor_err_cnt = 0\n"));
+                ++fsensor_softfail;
+                ++fsensor_softfail_ccnt;
+                fsensor_softfail_last = now;
+            }
+            else
+            {
+                fsensor_softfail_ccnt = 0;
+                fsensor_softfail_last = 0;
+                fsensor_enque_M600();
+            }
+        }
+#else //PAT9125
+        if (CHECK_FSENSOR && ir_sensor_detected)
+        {
+            if(digitalRead(IR_SENSOR_PIN))
+            {                                  // IR_SENSOR_PIN ~ H
+#ifdef IR_SENSOR_ANALOG
+                if(!bIRsensorStateFlag)
+                {
+                    bIRsensorStateFlag=true;
+                    nIRsensorLastTime=_millis();
+                }
+                else
+                {
+                    if((_millis()-nIRsensorLastTime)>IR_SENSOR_STEADY)
+                    {
+                        uint8_t nMUX1,nMUX2;
+                        uint16_t nADC;
+                        bIRsensorStateFlag=false;
+                        // sequence for direct data reading from AD converter
+                        DISABLE_TEMPERATURE_INTERRUPT();
+                        nMUX1=ADMUX;        // ADMUX saving
+                        nMUX2=ADCSRB;
+                        adc_setmux(VOLT_IR_PIN);
+                        ADCSRA|=(1<<ADSC);  // first conversion after ADMUX change discarded (preventively)
+                        while(ADCSRA&(1<<ADSC))
+                            ;
+                        ADCSRA|=(1<<ADSC);  // second conversion used
+                        while(ADCSRA&(1<<ADSC))
+                            ;
+                        nADC=ADC;
+                        ADMUX=nMUX1;        // ADMUX restoring
+                        ADCSRB=nMUX2;
+                        ENABLE_TEMPERATURE_INTERRUPT();
+                        // end of sequence for ...
+                        // Detection of correct function of fsensor v04 - it must NOT read >4.6V
+                        // If it does, it means a disconnected cables or faulty board
+                        if( (oFsensorPCB == ClFsensorPCB::_Rev04) && ( (nADC*OVERSAMPLENR) > IRsensor_Hopen_TRESHOLD ) )
+                        {
+                            fsensor_disable();
+                            fsensor_not_responding = true;
+                            printf_P(PSTR("IR sensor not responding (%d)!\n"),1);
+                            if((ClFsensorActionNA)eeprom_read_byte((uint8_t*)EEPROM_FSENSOR_ACTION_NA)==ClFsensorActionNA::_Pause)
+
+                            // if we are printing and FS action is set to "Pause", force pause the print
+                            if(oFsensorActionNA==ClFsensorActionNA::_Pause)
+                                lcd_pause_print();
+                        }
+                        else
+                        {
+#endif //IR_SENSOR_ANALOG
+                            fsensor_checkpoint_print();
+                            fsensor_enque_M600();
+#ifdef IR_SENSOR_ANALOG
+                        }
+                    }
+                }
+                   }
+                   else
+                   {                                  // IR_SENSOR_PIN ~ L
+                        bIRsensorStateFlag=false;
+#endif //IR_SENSOR_ANALOG
+            }
+        }
+#endif //PAT9125
+}
+
+#ifdef IR_SENSOR_ANALOG
+/// This is called only upon start of the printer or when switching the fsensor ON in the menu
+/// We cannot do temporal window checks here (aka the voltage has been in some range for a period of time)
+bool fsensor_IR_check(){
+    if( IRsensor_Lmax_TRESHOLD <= current_voltage_raw_IR && current_voltage_raw_IR <= IRsensor_Hmin_TRESHOLD ){
+        /// If the voltage is in forbidden range, the fsensor is ok, but the lever is mounted improperly.
+        /// Or the user is so creative so that he can hold a piece of fillament in the hole in such a genius way,
+        /// that the IR fsensor reading is within 1.5 and 3V ... this would have been highly unusual
+        /// and would have been considered more like a sabotage than normal printer operation
+        printf_P(PSTR("fsensor in forbidden range 1.5-3V - check sensor\n"));
+        return false; 
+    }
+    if( oFsensorPCB == ClFsensorPCB::_Rev04 ){
+        /// newer IR sensor cannot normally produce 4.6-5V, this is considered a failure/bad mount
+        if( IRsensor_Hopen_TRESHOLD <= current_voltage_raw_IR && current_voltage_raw_IR <= IRsensor_VMax_TRESHOLD ){
+            printf_P(PSTR("fsensor v0.4 in fault range 4.6-5V - unconnected\n"));
+            return false;
+        }
+        /// newer IR sensor cannot normally produce 0-0.3V, this is considered a failure 
+#if 0	//Disabled as it has to be decided if we gonna use this or not.
+        if( IRsensor_Hopen_TRESHOLD <= current_voltage_raw_IR && current_voltage_raw_IR <= IRsensor_VMax_TRESHOLD ){
+            printf_P(PSTR("fsensor v0.4 in fault range 0.0-0.3V - wrong IR sensor\n"));
+            return false;
+        }
+#endif
+    }
+    /// If IR sensor is "uknown state" and filament is not loaded > 1.5V return false
+#if 0
+    if( (oFsensorPCB == ClFsensorPCB::_Undef) && ( current_voltage_raw_IR > IRsensor_Lmax_TRESHOLD ) ){
+        printf_P(PSTR("Unknown IR sensor version and no filament loaded detected.\n"));
+        return false;
+    }
+#endif
+    // otherwise the IR fsensor is considered working correctly
+    return true;
+}
+#endif //IR_SENSOR_ANALOG
--- a/Firmware/fsensor.h
+++ b/Firmware/fsensor.h
@ -0,0 +1,124 @@
+//! @file
+#ifndef FSENSOR_H
+#define FSENSOR_H
+
+#include <inttypes.h>
+#include "config.h"
+
+
+// enable/disable flag
+extern bool fsensor_enabled;
+// not responding flag
+extern bool fsensor_not_responding;
+#ifdef PAT9125
+// optical checking "chunk lenght" (already in steps)
+extern int16_t fsensor_chunk_len;
+// count of soft failures
+extern uint8_t fsensor_softfail;
+#endif
+
+//! @name save restore printing
+//! @{
+extern void fsensor_stop_and_save_print(void);
+//! restore print - restore position and heatup to original temperature
+extern void fsensor_restore_print_and_continue(void);
+//! split the current gcode stream to insert new instructions
+extern void fsensor_checkpoint_print(void);
+//! @}
+
+//! initialize
+extern void fsensor_init(void);
+
+#ifdef PAT9125
+//! update axis resolution
+extern void fsensor_set_axis_steps_per_unit(float u);
+#endif
+
+//! @name enable/disable
+//! @{
+extern bool fsensor_enable(bool bUpdateEEPROM=true);
+extern void fsensor_disable(bool bUpdateEEPROM=true);
+//! @}
+
+//autoload feature enabled
+extern bool fsensor_autoload_enabled;
+extern void fsensor_autoload_set(bool State);
+
+extern void fsensor_update(void);
+#ifdef PAT9125
+//! setup pin-change interrupt
+extern void fsensor_setup_interrupt(void);
+
+//! @name autoload support
+//! @{
+
+extern void fsensor_autoload_check_start(void);
+extern void fsensor_autoload_check_stop(void);
+#endif //PAT9125
+extern bool fsensor_check_autoload(void);
+//! @}
+
+#ifdef PAT9125
+//! @name optical quality measurement support
+//! @{
+extern bool fsensor_oq_meassure_enabled;
+extern void fsensor_oq_meassure_set(bool State);
+extern void fsensor_oq_meassure_start(uint8_t skip);
+extern void fsensor_oq_meassure_stop(void);
+extern bool fsensor_oq_result(void);
+//! @}
+
+//! @name callbacks from stepper
+//! @{
+extern void fsensor_st_block_chunk(int cnt);
+
+// debugging
+extern uint8_t fsensor_log;
+
+// There's really nothing to do in block_begin: the stepper ISR likely has
+// called us already at the end of the last block, making this integration
+// redundant. LA1.5 might not always do that during a coasting move, so attempt
+// to drain fsensor_st_cnt anyway at the beginning of the new block.
+#define fsensor_st_block_begin(rev) fsensor_st_block_chunk(0)
+//! @}
+#endif //PAT9125
+
+#define VOLT_DIV_REF 5
+
+#ifdef IR_SENSOR_ANALOG
+#define IR_SENSOR_STEADY 10                       // [ms]
+
+enum class ClFsensorPCB:uint_least8_t
+{
+    _Old=0,
+    _Rev04=1,
+    _Undef=EEPROM_EMPTY_VALUE
+};
+
+enum class ClFsensorActionNA:uint_least8_t
+{
+    _Continue=0,
+    _Pause=1,
+    _Undef=EEPROM_EMPTY_VALUE
+};
+
+extern ClFsensorPCB oFsensorPCB;
+extern ClFsensorActionNA oFsensorActionNA;
+extern const char* FsensorIRVersionText();
+
+extern bool fsensor_IR_check();
+constexpr uint16_t Voltage2Raw(float V){
+	return ( V * 1023 * OVERSAMPLENR / VOLT_DIV_REF ) + 0.5F;
+}
+constexpr float Raw2Voltage(uint16_t raw){
+	return VOLT_DIV_REF*(raw / (1023.F * OVERSAMPLENR) );
+}
+constexpr uint16_t IRsensor_Ldiode_TRESHOLD = Voltage2Raw(0.3F); // ~0.3V, raw value=982
+constexpr uint16_t IRsensor_Lmax_TRESHOLD = Voltage2Raw(1.5F); // ~1.5V (0.3*Vcc), raw value=4910
+constexpr uint16_t IRsensor_Hmin_TRESHOLD = Voltage2Raw(3.0F); // ~3.0V (0.6*Vcc), raw value=9821
+constexpr uint16_t IRsensor_Hopen_TRESHOLD = Voltage2Raw(4.6F); // ~4.6V (N.C. @ Ru~20-50k, Rd'=56k, Ru'=10k), raw value=15059
+constexpr uint16_t IRsensor_VMax_TRESHOLD = Voltage2Raw(5.F); // ~5V, raw value=16368
+
+#endif //IR_SENSOR_ANALOG
+
+#endif //FSENSOR_H
--- a/Firmware/heatbed_pwm.cpp
+++ b/Firmware/heatbed_pwm.cpp
@ -1,5 +1,6 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
+#include "io_atmega2560.h"

 // All this is about silencing the heat bed, as it behaves like a loudspeaker.
 // Basically, we want the PWM heating switched at 30Hz (or so) which is a well ballanced
@ -11,7 +12,7 @@
 // Doing this at higher frequency than the bed "loudspeaker" can handle makes the click barely audible.
 // Technically:
 // timer0 is set to fast PWM mode at 62.5kHz (timer0 is linked to the bed heating pin) (zero prescaler)
-// To keep the bed switching at 30Hz - we don't want the PWM running at 62kHz all the time
+// To keep the bed switching at 30Hz - we don't want the PWM running at 62kHz all the time 
 // since it would burn the heatbed's MOSFET:
 // 16MHz/256 levels of PWM duty gives us 62.5kHz
 // 62.5kHz/256 gives ~244Hz, that is still too fast - 244/8 gives ~30Hz, that's what we need
@ -19,9 +20,9 @@
 // The finite automaton is running in the ISR(TIMER0_OVF_vect)

 // 2019-08-14 update: the original algorithm worked very well, however there were 2 regressions:
-// 1. 62kHz ISR requires considerable amount of processing power,
+// 1. 62kHz ISR requires considerable amount of processing power, 
 //    USB transfer speed dropped by 20%, which was most notable when doing short G-code segments.
-// 2. Some users reported TLed PSU started clicking when running at 120V/60Hz.
+// 2. Some users reported TLed PSU started clicking when running at 120V/60Hz. 
 //    This looks like the original algorithm didn't maintain base PWM 30Hz, but only 15Hz
 // To address both issues, there is an improved approach based on the idea of leveraging
 // different CLK prescalers in some automaton states - i.e. when holding LOW or HIGH on the output pin,
@ -40,7 +41,7 @@
 // - it can toggle unnoticed, which will result in bed clicking again.
 // That's why there are special transition states ZERO_TO_RISE and ONE_TO_FALL, which enable the
 // counter change its operation atomically and without artefacts on the output pin.
-// The resulting signal on the output pin was checked with an osciloscope.
+// The resulting signal on the output pin was checked with an osciloscope. 
 // If there are any change requirements in the future, the signal must be checked with an osciloscope again,
 // ad-hoc changes may completely screw things up!

@ -57,7 +58,7 @@ enum class States : uint8_t {
 	ZERO_TO_RISE,  ///< metastate allowing the timer change its state atomically without artefacts on the output pin
 	RISE,          ///< 16 fast PWM cycles with increasing duty up to steady ON
 	RISE_TO_ONE,   ///< metastate allowing the timer change its state atomically without artefacts on the output pin
-	ONE,           ///< steady 1 (ON), no change for the whole period
+	ONE,           ///< steady 1 (ON), no change for the whole period 
 	FALL,          ///< 16 fast PWM cycles with decreasing duty down to steady OFF
 	FALL_TO_ZERO   ///< metastate allowing the timer change its state atomically without artefacts on the output pin
 };
@ -92,7 +93,7 @@ static const uint8_t fastShift = 4;
 /// increment slowCounter by 1
 /// but use less bits of soft PWM - something like soft_pwm_bed >> 2
 /// that may further reduce the CPU cycles required by the bed heating automaton
-/// Due to the nature of bed heating the reduced PID precision may not be a major issue, however doing 8x less ISR(timer0_ovf) may significantly improve the performance
+/// Due to the nature of bed heating the reduced PID precision may not be a major issue, however doing 8x less ISR(timer0_ovf) may significantly improve the performance 
 static const uint8_t slowInc = 1;

 ISR(TIMER0_OVF_vect)          // timer compare interrupt service routine
@ -153,6 +154,7 @@ ISR(TIMER0_OVF_vect)          // timer compare interrupt service routine
 			return;           // want full duty for the next ONE cycle again - so keep on heating and just wait for the next timer ovf
 		}
 		// otherwise moving towards FALL
+		state = States::ONE;//_TO_FALL;
 		state=States::FALL;
 		fastCounter = fastMax - 1;// we'll do 16-1 cycles of RISE
 		TCNT0 = 255;              // force overflow on the next clock cycle
@ -177,6 +179,6 @@ ISR(TIMER0_OVF_vect)          // timer compare interrupt service routine
 		TCNT0 = 128;
 		OCR0B = 255;
 		TCCR0B = (1 << CS01); // change prescaler to 8, i.e. 7.8kHz
-		break;
+		break;		
    }
 }
--- a/Firmware/host.cpp
+++ b/Firmware/host.cpp
@ -1,32 +0,0 @@
-#include <string.h>
-#include "Configuration_adv.h"
-#include "host.h"
-#include "Timer.h"
-
-static LongTimer M79_timer;
-static char host_status_screen_name[3];
-
-void SetHostStatusScreenName(const char * name) {
-    strncpy(host_status_screen_name, name, 2);
-    host_status_screen_name[2] = '\0';
-}
-
-char * GetHostStatusScreenName() {
-    return host_status_screen_name;
-}
-
-void ResetHostStatusScreenName() {
-    memset(host_status_screen_name, 0, sizeof(host_status_screen_name));
-}
-
-void M79_timer_restart() {
-    M79_timer.start();
-}
-
-bool M79_timer_get_status() {
-    return M79_timer.running();
-}
-
-void M79_timer_update_status() {
-    M79_timer.expired(M79_TIMEOUT);
-}
--- a/Firmware/host.h
+++ b/Firmware/host.h
@ -1,22 +0,0 @@
-#pragma once
-
-/// Assigns host name with up to two characters which will be shown on
-/// the UI when printing. The function forces the third byte to be null delimiter.
-void SetHostStatusScreenName(const char * name);
-
-/// Returns a pointer to the host name
-char * GetHostStatusScreenName();
-
-/// Reset the memory to NULL when the host name should not be used
-void ResetHostStatusScreenName();
-
-/// Restart the M79 timer
-void M79_timer_restart();
-
-/// Get the current M79 timer status
-/// @returns true if running, false otherwise
-bool M79_timer_get_status();
-
-/// Checks if the timer period has expired. If the timer
-/// has expired, the timer is stopped
-void M79_timer_update_status();
--- a/Firmware/io_atmega2560.h
+++ b/Firmware/io_atmega2560.h
@ -0,0 +1,374 @@
+//io_atmega2560.h
+#ifndef _IO_ATMEGA2560
+#define _IO_ATMEGA2560
+
+
+#define __PIN_P0  PINE
+#define __PIN_P1  PINE
+#define __PIN_P2  PINE
+#define __PIN_P3  PINE
+#define __PIN_P4  PING
+#define __PIN_P5  PINE
+#define __PIN_P6  PINH
+#define __PIN_P7  PINH
+#define __PIN_P8  PINH
+#define __PIN_P9  PINH
+#define __PIN_P10 PINB
+#define __PIN_P11 PINB
+#define __PIN_P12 PINB
+#define __PIN_P13 PINB
+#define __PIN_P14 PINJ
+#define __PIN_P15 PINJ
+#define __PIN_P16 PINH
+#define __PIN_P17 PINH
+#define __PIN_P18 PIND
+#define __PIN_P19 PIND
+#define __PIN_P20 PIND
+#define __PIN_P21 PIND
+#define __PIN_P22 PINA
+#define __PIN_P23 PINA
+#define __PIN_P24 PINA
+#define __PIN_P25 PINA
+#define __PIN_P26 PINA
+#define __PIN_P27 PINA
+#define __PIN_P28 PINA
+#define __PIN_P29 PINA
+#define __PIN_P30 PINC
+#define __PIN_P31 PINC
+#define __PIN_P32 PINC
+#define __PIN_P33 PINC
+#define __PIN_P34 PINC
+#define __PIN_P35 PINC
+#define __PIN_P36 PINC
+#define __PIN_P37 PINC
+#define __PIN_P38 PIND
+#define __PIN_P39 PING
+#define __PIN_P40 PING
+#define __PIN_P41 PING
+#define __PIN_P42 PINL
+#define __PIN_P43 PINL
+#define __PIN_P44 PINL
+#define __PIN_P45 PINL
+#define __PIN_P46 PINL
+#define __PIN_P47 PINL
+#define __PIN_P48 PINL
+#define __PIN_P49 PINL
+#define __PIN_P50 PINB
+#define __PIN_P51 PINB
+#define __PIN_P52 PINB
+#define __PIN_P53 PINB
+#define __PIN_P54 PINF
+#define __PIN_P55 PINF
+#define __PIN_P56 PINF
+#define __PIN_P57 PINF
+#define __PIN_P58 PINF
+#define __PIN_P59 PINF
+#define __PIN_P60 PINF
+#define __PIN_P61 PINF
+#define __PIN_P62 PINK
+#define __PIN_P63 PINK
+#define __PIN_P64 PINK
+#define __PIN_P65 PINK
+#define __PIN_P66 PINK
+#define __PIN_P67 PINK
+#define __PIN_P68 PINK
+#define __PIN_P69 PINK
+#define __PIN_P70 PING
+#define __PIN_P71 PING
+#define __PIN_P72 PINJ
+#define __PIN_P73 PINJ
+#define __PIN_P74 PINJ
+#define __PIN_P75 PINJ
+#define __PIN_P76 PINJ
+#define __PIN_P77 PINJ
+#define __PIN_P78 PINE
+#define __PIN_P79 PINE
+#define __PIN_P80 PINE
+#define __PIN_P81 PIND
+#define __PIN_P82 PIND
+#define __PIN_P83 PIND
+#define __PIN_P84 PINH
+#define __PIN_P85 PINH
+
+#define __PORT_P0  PORTE
+#define __PORT_P1  PORTE
+#define __PORT_P2  PORTE
+#define __PORT_P3  PORTE
+#define __PORT_P4  PORTG
+#define __PORT_P5  PORTE
+#define __PORT_P6  PORTH
+#define __PORT_P7  PORTH
+#define __PORT_P8  PORTH
+#define __PORT_P9  PORTH
+#define __PORT_P10 PORTB
+#define __PORT_P11 PORTB
+#define __PORT_P12 PORTB
+#define __PORT_P13 PORTB
+#define __PORT_P14 PORTJ
+#define __PORT_P15 PORTJ
+#define __PORT_P16 PORTH
+#define __PORT_P17 PORTH
+#define __PORT_P18 PORTD
+#define __PORT_P19 PORTD
+#define __PORT_P20 PORTD
+#define __PORT_P21 PORTD
+#define __PORT_P22 PORTA
+#define __PORT_P23 PORTA
+#define __PORT_P24 PORTA
+#define __PORT_P25 PORTA
+#define __PORT_P26 PORTA
+#define __PORT_P27 PORTA
+#define __PORT_P28 PORTA
+#define __PORT_P29 PORTA
+#define __PORT_P30 PORTC
+#define __PORT_P31 PORTC
+#define __PORT_P32 PORTC
+#define __PORT_P33 PORTC
+#define __PORT_P34 PORTC
+#define __PORT_P35 PORTC
+#define __PORT_P36 PORTC
+#define __PORT_P37 PORTC
+#define __PORT_P38 PORTD
+#define __PORT_P39 PORTG
+#define __PORT_P40 PORTG
+#define __PORT_P41 PORTG
+#define __PORT_P42 PORTL
+#define __PORT_P43 PORTL
+#define __PORT_P44 PORTL
+#define __PORT_P45 PORTL
+#define __PORT_P46 PORTL
+#define __PORT_P47 PORTL
+#define __PORT_P48 PORTL
+#define __PORT_P49 PORTL
+#define __PORT_P50 PORTB
+#define __PORT_P51 PORTB
+#define __PORT_P52 PORTB
+#define __PORT_P53 PORTB
+#define __PORT_P54 PORTF
+#define __PORT_P55 PORTF
+#define __PORT_P56 PORTF
+#define __PORT_P57 PORTF
+#define __PORT_P58 PORTF
+#define __PORT_P59 PORTF
+#define __PORT_P60 PORTF
+#define __PORT_P61 PORTF
+#define __PORT_P62 PORTK
+#define __PORT_P63 PORTK
+#define __PORT_P64 PORTK
+#define __PORT_P65 PORTK
+#define __PORT_P66 PORTK
+#define __PORT_P67 PORTK
+#define __PORT_P68 PORTK
+#define __PORT_P69 PORTK
+#define __PORT_P70 PORTG
+#define __PORT_P71 PORTG
+#define __PORT_P72 PORTJ
+#define __PORT_P73 PORTJ
+#define __PORT_P74 PORTJ
+#define __PORT_P75 PORTJ
+#define __PORT_P76 PORTJ
+#define __PORT_P77 PORTJ
+#define __PORT_P78 PORTE
+#define __PORT_P79 PORTE
+#define __PORT_P80 PORTE
+#define __PORT_P81 PORTD
+#define __PORT_P82 PORTD
+#define __PORT_P83 PORTD
+#define __PORT_P84 PORTH
+#define __PORT_P85 PORTH
+
+#define __DDR_P0  DDRE
+#define __DDR_P1  DDRE
+#define __DDR_P2  DDRE
+#define __DDR_P3  DDRE
+#define __DDR_P4  DDRG
+#define __DDR_P5  DDRE
+#define __DDR_P6  DDRH
+#define __DDR_P7  DDRH
+#define __DDR_P8  DDRH
+#define __DDR_P9  DDRH
+#define __DDR_P10 DDRB
+#define __DDR_P11 DDRB
+#define __DDR_P12 DDRB
+#define __DDR_P13 DDRB
+#define __DDR_P14 DDRJ
+#define __DDR_P15 DDRJ
+#define __DDR_P16 DDRH
+#define __DDR_P17 DDRH
+#define __DDR_P18 DDRD
+#define __DDR_P19 DDRD
+#define __DDR_P20 DDRD
+#define __DDR_P21 DDRD
+#define __DDR_P22 DDRA
+#define __DDR_P23 DDRA
+#define __DDR_P24 DDRA
+#define __DDR_P25 DDRA
+#define __DDR_P26 DDRA
+#define __DDR_P27 DDRA
+#define __DDR_P28 DDRA
+#define __DDR_P29 DDRA
+#define __DDR_P30 DDRC
+#define __DDR_P31 DDRC
+#define __DDR_P32 DDRC
+#define __DDR_P33 DDRC
+#define __DDR_P34 DDRC
+#define __DDR_P35 DDRC
+#define __DDR_P36 DDRC
+#define __DDR_P37 DDRC
+#define __DDR_P38 DDRD
+#define __DDR_P39 DDRG
+#define __DDR_P40 DDRG
+#define __DDR_P41 DDRG
+#define __DDR_P42 DDRL
+#define __DDR_P43 DDRL
+#define __DDR_P44 DDRL
+#define __DDR_P45 DDRL
+#define __DDR_P46 DDRL
+#define __DDR_P47 DDRL
+#define __DDR_P48 DDRL
+#define __DDR_P49 DDRL
+#define __DDR_P50 DDRB
+#define __DDR_P51 DDRB
+#define __DDR_P52 DDRB
+#define __DDR_P53 DDRB
+#define __DDR_P54 DDRF
+#define __DDR_P55 DDRF
+#define __DDR_P56 DDRF
+#define __DDR_P57 DDRF
+#define __DDR_P58 DDRF
+#define __DDR_P59 DDRF
+#define __DDR_P60 DDRF
+#define __DDR_P61 DDRF
+#define __DDR_P62 DDRK
+#define __DDR_P63 DDRK
+#define __DDR_P64 DDRK
+#define __DDR_P65 DDRK
+#define __DDR_P66 DDRK
+#define __DDR_P67 DDRK
+#define __DDR_P68 DDRK
+#define __DDR_P69 DDRK
+#define __DDR_P70 DDRG
+#define __DDR_P71 DDRG
+#define __DDR_P72 DDRJ
+#define __DDR_P73 DDRJ
+#define __DDR_P74 DDRJ
+#define __DDR_P75 DDRJ
+#define __DDR_P76 DDRJ
+#define __DDR_P77 DDRJ
+#define __DDR_P78 DDRE
+#define __DDR_P79 DDRE
+#define __DDR_P80 DDRE
+#define __DDR_P81 DDRD
+#define __DDR_P82 DDRD
+#define __DDR_P83 DDRD
+#define __DDR_P84 DDRH
+#define __DDR_P85 DDRH
+
+#define __BIT_P0  0
+#define __BIT_P1  1
+#define __BIT_P2  4
+#define __BIT_P3  5
+#define __BIT_P4  5
+#define __BIT_P5  3
+#define __BIT_P6  3
+#define __BIT_P7  4
+#define __BIT_P8  5
+#define __BIT_P9  6
+#define __BIT_P10 4
+#define __BIT_P11 5
+#define __BIT_P12 6
+#define __BIT_P13 7
+#define __BIT_P14 1
+#define __BIT_P15 0
+#define __BIT_P16 0
+#define __BIT_P17 1
+#define __BIT_P18 3
+#define __BIT_P19 2
+#define __BIT_P20 1
+#define __BIT_P21 0
+#define __BIT_P22 0
+#define __BIT_P23 1
+#define __BIT_P24 2
+#define __BIT_P25 3
+#define __BIT_P26 4
+#define __BIT_P27 5
+#define __BIT_P28 6
+#define __BIT_P29 7
+#define __BIT_P30 7
+#define __BIT_P31 6
+#define __BIT_P32 5
+#define __BIT_P33 4
+#define __BIT_P34 3
+#define __BIT_P35 2
+#define __BIT_P36 1
+#define __BIT_P37 0
+#define __BIT_P38 7
+#define __BIT_P39 2
+#define __BIT_P40 1
+#define __BIT_P41 0
+#define __BIT_P42 7
+#define __BIT_P43 6
+#define __BIT_P44 5
+#define __BIT_P45 4
+#define __BIT_P46 3
+#define __BIT_P47 2
+#define __BIT_P48 1
+#define __BIT_P49 0
+#define __BIT_P50 3
+#define __BIT_P51 2
+#define __BIT_P52 1
+#define __BIT_P53 0
+#define __BIT_P54 0
+#define __BIT_P55 1
+#define __BIT_P56 2
+#define __BIT_P57 3
+#define __BIT_P58 4
+#define __BIT_P59 5
+#define __BIT_P60 6
+#define __BIT_P61 7
+#define __BIT_P62 0
+#define __BIT_P63 1
+#define __BIT_P64 2
+#define __BIT_P65 3
+#define __BIT_P66 4
+#define __BIT_P67 5
+#define __BIT_P68 6
+#define __BIT_P69 7
+#define __BIT_P70 4
+#define __BIT_P71 3
+#define __BIT_P72 2
+#define __BIT_P73 3
+#define __BIT_P74 7
+#define __BIT_P75 4
+#define __BIT_P76 5
+#define __BIT_P77 6
+#define __BIT_P78 2
+#define __BIT_P79 6
+#define __BIT_P80 7
+#define __BIT_P81 4
+#define __BIT_P82 5
+#define __BIT_P83 6
+#define __BIT_P84 2
+#define __BIT_P85 7
+
+#define __BIT(pin) __BIT_P##pin
+#define __MSK(pin) (1 << __BIT(pin))
+
+#define __PIN(pin) __PIN_P##pin
+#define __PORT(pin) __PORT_P##pin
+#define __DDR(pin) __DDR_P##pin
+
+#define PIN(pin) __PIN(pin)
+#define PORT(pin) __PORT(pin)
+#define DDR(pin) __DDR(pin)
+
+#define PIN_INP(pin) DDR(pin) &= ~__MSK(pin)
+#define PIN_OUT(pin) DDR(pin) |= __MSK(pin)
+#define PIN_CLR(pin) PORT(pin) &= ~__MSK(pin)
+#define PIN_SET(pin) PORT(pin) |= __MSK(pin)
+#define PIN_VAL(pin, val) if (val) PIN_SET(pin); else PIN_CLR(pin);
+#define PIN_GET(pin) (PIN(pin) & __MSK(pin))
+#define PIN_INQ(pin) (PORT(pin) & __MSK(pin))
+
+
+#endif //_IO_ATMEGA2560
--- a/Firmware/la10compat.cpp
+++ b/Firmware/la10compat.cpp
@ -74,7 +74,7 @@ float la10c_jerk(float j)

    // check for a compatible range of values prior to convert (be sure that
    // a higher E-jerk would still be compatible wrt the E accell range)
-    if(j < 4.5 && cs.max_acceleration_mm_per_s2_normal[E_AXIS] < 2000)
+    if(j < 4.5 && cs.max_acceleration_units_per_sq_second_normal[E_AXIS] < 2000)
        return j;

    // bring low E-jerk values into equivalent LA 1.5 values by
--- a/Firmware/language.c
+++ b/Firmware/language.c
@ -1,17 +1,15 @@
 //language.c
 #include "language.h"
 #include <avr/pgmspace.h>
-#include <avr/io.h>
 #include <avr/eeprom.h>
 #include "bootapp.h"

 #include "Configuration.h"
 #include "pins.h"

-#ifdef XFLASH
-#include "xflash.h"
-#include "xflash_layout.h"
-#endif //XFLASH
+#ifdef W25X20CL
+#include "w25x20cl.h"
+#endif //W25X20CL

 // Currently active language selection.
 uint8_t lang_selected = 0;
@ -19,17 +17,17 @@ uint8_t lang_selected = 0;

 #if (LANG_MODE == 0) //primary language only

-uint8_t lang_select(_UNUSED uint8_t lang) { return 0; }
+uint8_t lang_select(__attribute__((unused)) uint8_t lang) { return 0; }
 uint8_t lang_get_count() { return 1; }
-uint16_t lang_get_code(_UNUSED uint8_t lang) { return LANG_CODE_EN; }
-const char* lang_get_name_by_code(_UNUSED uint16_t code) { return _n("English"); }
+uint16_t lang_get_code(__attribute__((unused)) uint8_t lang) { return LANG_CODE_EN; }
+const char* lang_get_name_by_code(__attribute__((unused)) uint16_t code) { return _n("English"); }
 void lang_reset(void) { }
 uint8_t lang_is_selected(void) { return 1; }

 #else //(LANG_MODE == 0) //secondary languages in progmem or xflash

 //reserved xx kbytes for secondary language table
-const char _SEC_LANG[LANG_SIZE_RESERVED] __attribute__((aligned(SPM_PAGESIZE))) PROGMEM_I2 = "_SEC_LANG";
+const char _SEC_LANG[LANG_SIZE_RESERVED] PROGMEM_I2 = "_SEC_LANG";

 //primary language signature
 const uint32_t _PRI_LANG_SIGNATURE[1] __attribute__((section(".progmem0"))) = {0xffffffff};
@ -42,7 +40,7 @@ const char* lang_get_translation(const char* s)
 	if (lang_selected == 0) return s + 2; //primary language selected, return orig. str.
 	if (lang_table == 0) return s + 2; //sec. lang table not found, return orig. str.
 	uint16_t ui = pgm_read_word(((uint16_t*)s)); //read string id
-	if (ui == 0xffff) return s + 2; //id not assigned, return orig. str.
+	if (ui == 0xffff) return s + 2; //translation not found, return orig. str.
 	ui = pgm_read_word(((uint16_t*)(((char*)lang_table + 16 + ui*2)))); //read relative offset
 	if (pgm_read_byte(((uint8_t*)((char*)lang_table + ui))) == 0) //read first character
 		return s + 2;//zero length string == not translated, return orig. str.
@ -56,7 +54,7 @@ uint8_t lang_select(uint8_t lang)
 		lang_table = 0;
 		lang_selected = lang;
 	}
-#ifdef XFLASH
+#ifdef W25X20CL
 	if (lang_get_code(lang) == lang_get_code(LANG_ID_SEC)) lang = LANG_ID_SEC;
 	if (lang == LANG_ID_SEC) //current secondary language
 	{
@ -70,7 +68,7 @@ uint8_t lang_select(uint8_t lang)
 				}
 		}
 	}
-#else //XFLASH
+#else //W25X20CL
 	if (lang == LANG_ID_SEC)
 	{
 		uint16_t table = _SEC_LANG_TABLE;
@ -84,7 +82,7 @@ uint8_t lang_select(uint8_t lang)
 				}
 		}
 	}
-#endif //XFLASH
+#endif //W25X20CL
 	if (lang_selected == lang)
 	{
 		eeprom_update_byte((unsigned char*)EEPROM_LANG, lang_selected);
@ -109,19 +107,19 @@ uint8_t lang_get_count()
 {
 	if (pgm_read_dword(((uint32_t*)(_PRI_LANG_SIGNATURE))) == 0xffffffff)
 		return 1; //signature not set - only primary language will be available
-#ifdef XFLASH
-	XFLASH_SPI_ENTER();
+#ifdef W25X20CL
+	W25X20CL_SPI_ENTER();
 	uint8_t count = 2; //count = 1+n (primary + secondary + all in xflash)
-	uint32_t addr = LANG_OFFSET;
+	uint32_t addr = 0x00000; //start of xflash
 	lang_table_header_t header; //table header structure
 	while (1)
 	{
-		xflash_rd_data(addr, (uint8_t*)&header, sizeof(lang_table_header_t)); //read table header from xflash
+		w25x20cl_rd_data(addr, (uint8_t*)&header, sizeof(lang_table_header_t)); //read table header from xflash
 		if (header.magic != LANG_MAGIC) break; //break if magic not valid
 		addr += header.size; //calc address of next table
 		count++; //inc counter
 	}
-#else //XFLASH
+#else //W25X20CL
 	uint16_t table = _SEC_LANG_TABLE;
 	uint8_t count = 1; //count = 1 (primary)
 	while (pgm_read_dword(((uint32_t*)table)) == LANG_MAGIC) //magic valid
@ -129,14 +127,14 @@ uint8_t lang_get_count()
 		table += pgm_read_word((uint16_t*)(table + 4));
 		count++;
 	}
-#endif //XFLASH
+#endif //W25X20CL
 	return count;
 }

 uint8_t lang_get_header(uint8_t lang, lang_table_header_t* header, uint32_t* offset)
 {
 	if (lang == LANG_ID_PRI) return 0; //primary lang not supported for this function
-#ifdef XFLASH
+#ifdef W25X20CL
 	if (lang == LANG_ID_SEC)
 	{
 		uint16_t ui = _SEC_LANG_TABLE; //table pointer
@ -144,18 +142,18 @@ uint8_t lang_get_header(uint8_t lang, lang_table_header_t* header, uint32_t* off
 		if (offset) *offset = ui;
 		return (header->magic == LANG_MAGIC)?1:0; //return 1 if magic valid
 	}
-	XFLASH_SPI_ENTER();
-	uint32_t addr = LANG_OFFSET;
+	W25X20CL_SPI_ENTER();
+	uint32_t addr = 0x00000; //start of xflash
 	lang--;
 	while (1)
 	{
-		xflash_rd_data(addr, (uint8_t*)(header), sizeof(lang_table_header_t)); //read table header from xflash
+		w25x20cl_rd_data(addr, (uint8_t*)(header), sizeof(lang_table_header_t)); //read table header from xflash
 		if (header->magic != LANG_MAGIC) break; //break if not valid
 		if (offset) *offset = addr;
 		if (--lang == 0) return 1;
 		addr += header->size; //calc address of next table
 	}
-#else //XFLASH
+#else //W25X20CL
 	if (lang == LANG_ID_SEC)
 	{
 		uint16_t ui = _SEC_LANG_TABLE; //table pointer
@ -163,32 +161,32 @@ uint8_t lang_get_header(uint8_t lang, lang_table_header_t* header, uint32_t* off
 		if (offset) *offset = ui;
 		return (header->magic == LANG_MAGIC)?1:0; //return 1 if magic valid
 	}
-#endif //XFLASH
+#endif //W25X20CL
 	return 0;
 }

 uint16_t lang_get_code(uint8_t lang)
 {
 	if (lang == LANG_ID_PRI) return LANG_CODE_EN; //primary lang = EN
-#ifdef XFLASH
+#ifdef W25X20CL
 	if (lang == LANG_ID_SEC)
 	{
 		uint16_t ui = _SEC_LANG_TABLE; //table pointer
 		if (pgm_read_dword(((uint32_t*)(ui + 0))) != LANG_MAGIC) return LANG_CODE_XX; //magic not valid
 		return pgm_read_word(((uint32_t*)(ui + 10))); //return lang code from progmem
 	}
-	XFLASH_SPI_ENTER();
-	uint32_t addr = LANG_OFFSET;
+	W25X20CL_SPI_ENTER();
+	uint32_t addr = 0x00000; //start of xflash
 	lang_table_header_t header; //table header structure
 	lang--;
 	while (1)
 	{
-		xflash_rd_data(addr, (uint8_t*)&header, sizeof(lang_table_header_t)); //read table header from xflash
+		w25x20cl_rd_data(addr, (uint8_t*)&header, sizeof(lang_table_header_t)); //read table header from xflash
 		if (header.magic != LANG_MAGIC) break; //break if not valid
 		if (--lang == 0) return header.code;
 		addr += header.size; //calc address of next table
 	}
-#else //XFLASH
+#else //W25X20CL
 	uint16_t table = _SEC_LANG_TABLE;
 	uint8_t count = 1; //count = 1 (primary)
 	while (pgm_read_dword((uint32_t*)table) == LANG_MAGIC) //magic valid
@ -197,7 +195,7 @@ uint16_t lang_get_code(uint8_t lang)
 		table += pgm_read_word((uint16_t*)(table + 4));
 		count++;
 	}
-#endif //XFLASH
+#endif //W25X20CL
 	return LANG_CODE_XX;
 }

@ -212,46 +210,6 @@ const char* lang_get_name_by_code(uint16_t code)
 	case LANG_CODE_FR: return _n("Francais");
 	case LANG_CODE_IT: return _n("Italiano");
 	case LANG_CODE_PL: return _n("Polski");
-#ifdef COMMUNITY_LANGUAGE_SUPPORT //Community language support
-#ifdef COMMUNITY_LANG_GROUP1_NL
-	case LANG_CODE_NL: return _n("Nederlands"); //community Dutch contribution
-#endif // COMMUNITY_LANG_GROUP1_NL
-#ifdef COMMUNITY_LANG_GROUP1_SV
-	case LANG_CODE_SV: return _n("Svenska"); //community Swedish contribution
-#endif // COMMUNITY_LANG_GROUP1_SV
-#ifdef COMMUNITY_LANG_GROUP1_NO
-	case LANG_CODE_NO: return _n("Norsk"); //community Swedish contribution
-#endif // COMMUNITY_LANG_GROUP1_NO
-#ifdef COMMUNITY_LANG_GROUP1_DA
-	case LANG_CODE_DA: return _n("Dansk"); //community Danish contribution
-#endif // COMMUNITY_LANG_GROUP1_DA
-#ifdef COMMUNITY_LANG_GROUP1_SK
-	case LANG_CODE_SK: return _n("Slovencina"); //community Slovak contribution
-#endif // COMMUNITY_LANG_GROUP1_SK
-#ifdef COMMUNITY_LANG_GROUP1_SL
-	case LANG_CODE_SL: return _n("Slovenscina"); //community Slovanian contribution
-#endif // COMMUNITY_LANG_GROUP1_SL
-#ifdef COMMUNITY_LANG_GROUP1_HU
-	case LANG_CODE_HU: return _n("Magyar"); //community Hungarian contribution
-#endif // COMMUNITY_LANG_GROUP1_HU
-#ifdef COMMUNITY_LANG_GROUP1_LB
-	case LANG_CODE_LB: return _n("Letzebuergesch"); //community Luxembourgish contribution
-#endif // COMMUNITY_LANG_GROUP1_LB
-#ifdef COMMUNITY_LANG_GROUP1_HR
-	case LANG_CODE_HR: return _n("Hrvatski"); //community Croatian contribution
-#endif // COMMUNITY_LANG_GROUP1_HR
-#ifdef COMMUNITY_LANG_GROUP2_LT
-	case LANG_CODE_LT: return _n("Lietuviu"); //community Lithuanian contribution
-#endif // COMMUNITY_LANG_GROUP2_LT
-#ifdef COMMUNITY_LANG_GROUP1_RO
-	case LANG_CODE_RO: return _n("Romana"); //community Romanian contribution
-#endif // COMMUNITY_LANG_GROUP1_RO
-
-//Use the 3 lines below as a template and replace 'QR' and 'New language'
-//#ifdef COMMUNITY_LANG_GROUP1_QR
-//	case LANG_CODE_QR: return _n("New language"); //community contribution
-//#endif // COMMUNITY_LANG_GROUP1_QR
-#endif // COMMUNITY_LANGUAGE_SUPPORT
 	}
 	return _n("??");
 }
@ -276,7 +234,7 @@ const char* lang_get_sec_lang_str_by_id(uint16_t id)
 	return ui + pgm_read_word(((uint16_t*)(ui + 16 + id * 2))); //read relative offset and return calculated pointer
 }

-uint16_t lang_print_sec_lang()
+uint16_t lang_print_sec_lang(FILE* out)
 {
 	printf_P(_n("&_SEC_LANG        = 0x%04x\n"), &_SEC_LANG);
 	printf_P(_n("sizeof(_SEC_LANG) = 0x%04x\n"), sizeof(_SEC_LANG));
@ -298,7 +256,7 @@ uint16_t lang_print_sec_lang()
 	puts_P(_n(" strings:\n"));
 	uint16_t ui = _SEC_LANG_TABLE; //table pointer
 	for (ui = 0; ui < _lt_count; ui++)
-		printf_P(_n("  %3d %S\n"), ui, lang_get_sec_lang_str_by_id(ui));
+		fprintf_P(out, _n("  %3d %S\n"), ui, lang_get_sec_lang_str_by_id(ui));
 	return _lt_count;
 }
 #endif //DEBUG_SEC_LANG
@ -310,5 +268,5 @@ void lang_boot_update_start(uint8_t lang)
 {
 	uint8_t cnt = lang_get_count();
 	if ((lang < 2) || (lang > cnt)) return; //only languages from xflash can be selected
-	bootapp_reboot_user0(lang << 3);
+	bootapp_reboot_user0(lang << 4);
 }
--- a/Firmware/language.h
+++ b/Firmware/language.h
@ -5,7 +5,6 @@


 #include "config.h"
-#include "macros.h"
 #include <inttypes.h>
 #ifdef DEBUG_SEC_LANG
    #include <stdio.h>
@ -13,15 +12,27 @@

 #define PROTOCOL_VERSION "1.0"

+#ifndef CUSTOM_MENDEL_NAME
+    #define MACHINE_NAME "Mendel"
+#endif
+
+#ifndef MACHINE_UUID
+   #define MACHINE_UUID "00000000-0000-0000-0000-000000000000"
+#endif
+
+#define MSG_FW_VERSION                   "Firmware"
+
+#define STRINGIFY_(n) #n
+#define STRINGIFY(n) STRINGIFY_(n)
+
 #if (LANG_MODE == 0) //primary language only
 #define PROGMEM_I2 __attribute__((section(".progmem0")))
 #define PROGMEM_I1 __attribute__((section(".progmem1")))
 #define PROGMEM_N1 __attribute__((section(".progmem2")))
 #define _I(s) (__extension__({static const char __c[] PROGMEM_I1 = s; &__c[0];}))
-#define ISTR(s) (s) // declare a translatable string
-#define _i(s) _I(s) // declare a translatable string and return the translated form
-#define _T(s) (s)   // return translated string from reference
-#define _O(s) (s)   // return original (untranslated) string from reference
+#define ISTR(s) s
+#define _i(s) _I(s)
+#define _T(s) s
 #else //(LANG_MODE == 0)
 // section .loc_sec (originaly .progmem0) will be used for localized translated strings
 #define PROGMEM_I2 __attribute__((section(".loc_sec")))
@ -33,17 +44,14 @@
 #define ISTR(s) "\xff\xff" s
 #define _i(s) lang_get_translation(_I(s))
 #define _T(s) lang_get_translation(s)
-#define _O(s) (s + 2)
 #endif //(LANG_MODE == 0)
-
 #define _N(s) (__extension__({static const char __c[] PROGMEM_N1 = s; &__c[0];}))
-#define _n(s) _N(s) // declare and return untranslated string
-#define _R(s) (s)   // return reference to translatable string (for warning suppression)
+#define _n(s) _N(s)

 /** @brief lang_table_header_t structure - (size= 16byte) */
 typedef struct
 {
-	uint32_t magic;      //+0
+	uint32_t magic;      //+0 
 	uint16_t size;       //+4
 	uint16_t count;      //+6
 	uint16_t checksum;   //+8
@ -90,45 +98,6 @@ typedef struct
 #define LANG_CODE_FR 0x6672 //!<'fr'
 #define LANG_CODE_IT 0x6974 //!<'it'
 #define LANG_CODE_PL 0x706c //!<'pl'
-#ifdef COMMUNITY_LANGUAGE_SUPPORT //Community language support
-#ifdef COMMUNITY_LANG_GROUP1_NL
-#define LANG_CODE_NL 0x6e6c //!<'nl'
-#endif // COMMUNITY_LANG_GROUP1_NL
-#ifdef COMMUNITY_LANG_GROUP1_SV
-#define LANG_CODE_SV 0x7376 //!<'sv'
-#endif // COMMUNITY_LANG_GROUP1_SV
-#ifdef COMMUNITY_LANG_GROUP1_NO
-#define LANG_CODE_NO 0x6E6F //!<'no'
-#endif // COMMUNITY_LANG_GROUP1_NO
-#ifdef COMMUNITY_LANG_GROUP1_DA
-#define LANG_CODE_DA 0x6461 //!<'da'
-#endif // COMMUNITY_LANG_GROUP1_DA
-#ifdef COMMUNITY_LANG_GROUP1_SL
-#define LANG_CODE_SL 0x736C //!<'sl'
-#endif // COMMUNITY_LANG_GROUP1_SL
-#ifdef COMMUNITY_LANG_GROUP1_HU
-#define LANG_CODE_HU 0x6875 //!<'hu'
-#endif // COMMUNITY_LANG_GROUP1_HU
-#ifdef COMMUNITY_LANG_GROUP1_LB
-#define LANG_CODE_LB 0x6C62 //!<'lb'
-#endif // COMMUNITY_LANG_GROUP1_LB
-#ifdef COMMUNITY_LANG_GROUP1_HR
-#define LANG_CODE_HR 0x6872 //!<'hr'
-#endif // COMMUNITY_LANG_GROUP1_HR
-#ifdef COMMUNITY_LANG_GROUP2_LT
-#define LANG_CODE_LT 0x6C74 //!<'lt'
-#endif // COMMUNITY_LANG_GROUP2_LT
-#ifdef COMMUNITY_LANG_GROUP1_SK
-#define LANG_CODE_SK 0x736b //!<'sk'
-#endif // COMMUNITY_LANG_GROUP1_SK
-#ifdef COMMUNITY_LANG_GROUP1_RO
-#define LANG_CODE_RO 0x726F //!<'ro'
-#endif // COMMUNITY_LANG_GROUP1_RO
-//Use the 3 lines below as a template and replace 'QR', '0X7172' and 'qr'
-//#ifdef COMMUNITY_LANG_GROUP1_QR
-//#define LANG_CODE_QR 0x7172 //!<'qr'
-//#endif // COMMUNITY_LANG_GROUP1_QR
-#endif // COMMUNITY_LANGUAGE_SUPPORT
 ///@}

 #if defined(__cplusplus)
@ -141,7 +110,9 @@ extern uint8_t lang_selected;
 #if (LANG_MODE != 0)
 extern const char _SEC_LANG[LANG_SIZE_RESERVED];
 extern const char* lang_get_translation(const char* s);
-#define _SEC_LANG_TABLE ((uint16_t)&_SEC_LANG)
+/** @def _SEC_LANG_TABLE
+ *  @brief Align table to start of 256 byte page */
+#define _SEC_LANG_TABLE ((((uint16_t)&_SEC_LANG) + 0x00ff) & 0xff00)
 #endif //(LANG_MODE != 0)

 /** @brief selects language, eeprom is updated in case of success */
@ -163,7 +134,7 @@ extern uint8_t lang_is_selected(void);

 #ifdef DEBUG_SEC_LANG
 extern const char* lang_get_sec_lang_str_by_id(uint16_t id);
-extern uint16_t lang_print_sec_lang();
+extern uint16_t lang_print_sec_lang(FILE* out);
 #endif //DEBUG_SEC_LANG

 extern void lang_boot_update_start(uint8_t lang);
--- a/Firmware/lcd.cpp
+++ b/Firmware/lcd.cpp
--- a/Firmware/lcd.h
+++ b/Firmware/lcd.h
@ -22,40 +22,54 @@ extern void lcd_clear(void);

 extern void lcd_home(void);

-extern void lcd_set_cursor(uint8_t col, uint8_t row);
+/*extern void lcd_no_display(void);
+extern void lcd_display(void);
+extern void lcd_no_blink(void);
+extern void lcd_blink(void);
+extern void lcd_no_cursor(void);
+extern void lcd_cursor(void);
+extern void lcd_scrollDisplayLeft(void);
+extern void lcd_scrollDisplayRight(void);
+extern void lcd_leftToRight(void);
+extern void lcd_rightToLeft(void);
+extern void lcd_autoscroll(void);
+extern void lcd_no_autoscroll(void);*/

-/// @brief Change the cursor column position while preserving the current row position
-/// @param col column number, ranges from 0 to LCD_WIDTH - 1
-void lcd_set_cursor_column(uint8_t col);
+extern void lcd_set_cursor(uint8_t col, uint8_t row);

 extern void lcd_createChar_P(uint8_t, const uint8_t*);


-// char c is non-standard, however it saves 1B on stack
-extern int lcd_putc(char c);
-extern int lcd_putc_at(uint8_t c, uint8_t r, char ch);
-
+extern int lcd_putc(int c);
 extern int lcd_puts_P(const char* str);
 extern int lcd_puts_at_P(uint8_t c, uint8_t r, const char* str);
 extern int lcd_printf_P(const char* format, ...);
 extern void lcd_space(uint8_t n);

 extern void lcd_printNumber(unsigned long n, uint8_t base);
+extern void lcd_printFloat(double number, uint8_t digits);

 extern void lcd_print(const char*);
-extern uint8_t lcd_print_pad(const char* s, uint8_t len);
-
-/// @brief print a string from PROGMEM with left-adjusted padding
-/// @param s string from PROGMEM.
-/// @param len maximum number of characters to print, including padding. Ranges from 0 to LCD_WIDTH.
-/// @return number of padded bytes. 0 means there was no padding.
-uint8_t lcd_print_pad_P(const char* s, uint8_t len);
 extern void lcd_print(char, int = 0);
 extern void lcd_print(unsigned char, int = 0);
 extern void lcd_print(int, int = 10);
 extern void lcd_print(unsigned int, int = 10);
 extern void lcd_print(long, int = 10);
 extern void lcd_print(unsigned long, int = 10);
+extern void lcd_print(double, int = 2);
+
+//! @brief Clear screen
+#define ESC_2J     "\x1b[2J"
+//! @brief Show cursor
+#define ESC_25h    "\x1b[?25h"
+//! @brief Hide cursor
+#define ESC_25l    "\x1b[?25l"
+//! @brief Set cursor to
+//! @param c column
+//! @param r row
+#define ESC_H(c,r) "\x1b["#r";"#c"H"
+
+

 #define LCD_UPDATE_INTERVAL    100
 #define LCD_TIMEOUT_TO_STATUS 30000ul //!< Generic timeout to status screen in ms, when no user action.
@ -72,9 +86,17 @@ typedef void (*lcd_lcdupdate_func_t)(void);
 //Set to none-zero when the LCD needs to draw, decreased after every draw. Set to 2 in LCD routines so the LCD gets at least 1 full redraw (first redraw is partial)
 extern uint8_t lcd_draw_update;

-extern int16_t lcd_encoder;
+extern int32_t lcd_encoder;

-extern uint8_t lcd_click_trigger;
+extern uint8_t lcd_encoder_bits;
+
+// lcd_encoder_diff is updated from interrupt context and added to lcd_encoder every LCD update
+extern int8_t lcd_encoder_diff;
+
+//the last checked lcd_buttons in a bit array.
+extern uint8_t lcd_buttons;
+
+extern uint8_t lcd_button_pressed;

 extern uint8_t lcd_update_enabled;

@ -82,8 +104,11 @@ extern LongTimer lcd_timeoutToStatus;

 extern uint32_t lcd_next_update_millis;

+extern uint8_t lcd_status_update_delay;
+
 extern lcd_longpress_func_t lcd_longpress_func;
-extern bool lcd_longpress_trigger;
+
+extern lcd_charsetup_func_t lcd_charsetup_func;

 extern lcd_lcdupdate_func_t lcd_lcdupdate_func;

@ -96,10 +121,6 @@ extern void lcd_beeper_quick_feedback(void);
 //Cause an LCD refresh, and give the user visual or audible feedback that something has happened
 extern void lcd_quick_feedback(void);

-/// @brief Check whether knob is rotated or clicked and update relevant
-///variables. Flags are set by lcd_buttons_update in ISR context.
-extern void lcd_knob_update();
-
 extern void lcd_update(uint8_t lcdDrawUpdateOverride);

 extern void lcd_update_enable(uint8_t enabled);
@ -111,25 +132,38 @@ extern void lcd_buttons_update(void);
 //! When constructed (on stack), original state state of lcd_update_enabled is stored
 //! and LCD updates are disabled.
 //! When destroyed (gone out of scope), original state of LCD update is restored.
-//! Do not call lcd_update_enable() to prevent calling lcd_update() in sensitive code.
-//! in certain scenarios it will cause recursion e.g. in the menus.
+//! It has zero overhead compared to storing bool saved = lcd_update_enabled
+//! and calling lcd_update_enable(false) and lcd_update_enable(saved).
 class LcdUpdateDisabler
 {
 public:
    LcdUpdateDisabler(): m_updateEnabled(lcd_update_enabled)
    {
-        lcd_update_enabled = false;
+        lcd_update_enable(false);
    }
    ~LcdUpdateDisabler()
    {
-        lcd_update_enabled = m_updateEnabled;
+        lcd_update_enable(m_updateEnabled);
    }

 private:
    bool m_updateEnabled;
 };

+
 ////////////////////////////////////
+// Setup button and encode mappings for each panel (into 'lcd_buttons' variable
+//
+// This is just to map common functions (across different panels) onto the same 
+// macro name. The mapping is independent of whether the button is directly connected or 
+// via a shift/i2c register.
+
+#define BLEN_B 1
+#define BLEN_A 0
+#define EN_B (1<<BLEN_B) // The two encoder pins are connected through BTN_EN1 and BTN_EN2
+#define EN_A (1<<BLEN_A)
+#define BLEN_C 2 
+#define EN_C (1<<BLEN_C) 

 //! @brief Was button clicked?
 //!
@ -140,32 +174,42 @@ private:
 //!
 //! @retval 0 button was not clicked
 //! @retval 1 button was clicked
-#define LCD_CLICKED (lcd_click_trigger)
+#define LCD_CLICKED (lcd_buttons&EN_C)
+
+////////////////////////
+// Setup Rotary Encoder Bit Values (for two pin encoders to indicate movement)
+// These values are independent of which pins are used for EN_A and EN_B indications
+// The rotary encoder part is also independent to the chipset used for the LCD
+#define encrot0 0
+#define encrot1 2
+#define encrot2 3
+#define encrot3 1

-////////////////////////////////////

 //Custom characters defined in the first 8 characters of the LCD
-#define LCD_STR_ARROW_RIGHT  "\x7E" //from the default character set
-#define LCD_STR_ARROW_LEFT   "\x7F" //from the default character set
-#define LCD_STR_BEDTEMP      "\x80"
-#define LCD_STR_DEGREE       "\x81"
-#define LCD_STR_THERMOMETER  "\x82"
-#define LCD_STR_UPLEVEL      "\x83"
-#define LCD_STR_REFRESH      "\x84"
-#define LCD_STR_FOLDER       "\x85"
-#define LCD_STR_FEEDRATE     "\x86"
-#define LCD_STR_CLOCK        "\x87"
-#define LCD_STR_ARROW_2_DOWN "\x88"
-#define LCD_STR_CONFIRM      "\x89"
-#define LCD_STR_SOLID_BLOCK  "\xFF"  //from the default character set
+#define LCD_STR_BEDTEMP     "\x00"
+#define LCD_STR_DEGREE      "\x01"
+#define LCD_STR_THERMOMETER "\x02"
+#define LCD_STR_UPLEVEL     "\x03"
+#define LCD_STR_REFRESH     "\x04"
+#define LCD_STR_FOLDER      "\x05"
+#define LCD_STR_FEEDRATE    "\x06"
+#define LCD_STR_CLOCK       "\x07"
+#define LCD_STR_ARROW_UP    "\x0B"
+#define LCD_STR_ARROW_DOWN  "\x01"
+#define LCD_STR_ARROW_RIGHT "\x7E" //from the default character set

-extern void lcd_frame_start();
+extern void lcd_set_custom_characters(void);
+extern void lcd_set_custom_characters_arrows(void);
+extern void lcd_set_custom_characters_progress(void);
+extern void lcd_set_custom_characters_nextpage(void);
+extern void lcd_set_custom_characters_degree(void);

-//! @brief Consume click and longpress event
+//! @brief Consume click event
 inline void lcd_consume_click()
 {
-    lcd_click_trigger = 0;
-    lcd_longpress_trigger = 0;
+    lcd_button_pressed = 0;
+    lcd_buttons &= 0xff^EN_C;
 }


--- a/Firmware/macros.h
+++ b/Firmware/macros.h
@ -1,93 +0,0 @@
-#ifndef MACROS_H
-#define MACROS_H
-
-#include <avr/interrupt.h> //for cli() and sei()
-
-#define  FORCE_INLINE __attribute__((always_inline)) inline
-#define _UNUSED __attribute__((unused))
-
-#ifndef CRITICAL_SECTION_START
-  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
-  #define CRITICAL_SECTION_END    SREG = _sreg;
-#endif //CRITICAL_SECTION_START
-
-#define _REGNAME(registerbase,number,suffix) registerbase##number##suffix
-#define _REGNAME_SHORT(registerbase,suffix) registerbase##suffix
-
-// Macros to make a string from a macro
-#define STRINGIFY_(M) #M
-#define STRINGIFY(M) STRINGIFY_(M)
-
-// Macros for bit masks
-#undef _BV
-#define _BV(n) (1<<(n))
-#define TEST(n,b) (!!((n)&_BV(b)))
-#define SET_BIT_TO(N,B,TF) do{ if (TF) SBI(N,B); else CBI(N,B); }while(0)
-
-#ifndef SBI
-  #define SBI(A,B) (A |= (1 << (B)))
-#endif
-
-#ifndef CBI
-  #define CBI(A,B) (A &= ~(1 << (B)))
-#endif
-
-#define TBI(N,B) (N ^= _BV(B))
-
-
-// Macros to chain up to 12 conditions
-#define _DO_1(W,C,A)       (_##W##_1(A))
-#define _DO_2(W,C,A,B)     (_##W##_1(A) C _##W##_1(B))
-#define _DO_3(W,C,A,V...)  (_##W##_1(A) C _DO_2(W,C,V))
-#define _DO_4(W,C,A,V...)  (_##W##_1(A) C _DO_3(W,C,V))
-#define _DO_5(W,C,A,V...)  (_##W##_1(A) C _DO_4(W,C,V))
-#define _DO_6(W,C,A,V...)  (_##W##_1(A) C _DO_5(W,C,V))
-#define _DO_7(W,C,A,V...)  (_##W##_1(A) C _DO_6(W,C,V))
-#define _DO_8(W,C,A,V...)  (_##W##_1(A) C _DO_7(W,C,V))
-#define _DO_9(W,C,A,V...)  (_##W##_1(A) C _DO_8(W,C,V))
-#define _DO_10(W,C,A,V...) (_##W##_1(A) C _DO_9(W,C,V))
-#define _DO_11(W,C,A,V...) (_##W##_1(A) C _DO_10(W,C,V))
-#define _DO_12(W,C,A,V...) (_##W##_1(A) C _DO_11(W,C,V))
-#define __DO_N(W,C,N,V...) _DO_##N(W,C,V)
-#define _DO_N(W,C,N,V...)  __DO_N(W,C,N,V)
-#define DO(W,C,V...)       _DO_N(W,C,NUM_ARGS(V),V)
-
-// Macros to support option testing
-#define _CAT(a,V...) a##V
-#define CAT(a,V...) _CAT(a,V)
-
-#define _ISENA_     ~,1
-#define _ISENA_1    ~,1
-#define _ISENA_0x1  ~,1
-#define _ISENA_true ~,1
-#define _ISENA(V...)        IS_PROBE(V)
-
-#define _ENA_1(O)           _ISENA(CAT(_IS,CAT(ENA_, O)))
-#define _DIS_1(O)           NOT(_ENA_1(O))
-#define ENABLED(V...)       DO(ENA,&&,V)
-#define DISABLED(V...)      DO(DIS,&&,V)
-
-#define TERN(O,A,B)         _TERN(_ENA_1(O),B,A)    // OPTION converted to '0' or '1'
-#define TERN0(O,A)          _TERN(_ENA_1(O),0,A)    // OPTION converted to A or '0'
-#define TERN1(O,A)          _TERN(_ENA_1(O),1,A)    // OPTION converted to A or '1'
-#define TERN_(O,A)          _TERN(_ENA_1(O),,A)     // OPTION converted to A or '<nul>'
-#define _TERN(E,V...)       __TERN(_CAT(T_,E),V)    // Prepend 'T_' to get 'T_0' or 'T_1'
-#define __TERN(T,V...)      ___TERN(_CAT(_NO,T),V)  // Prepend '_NO' to get '_NOT_0' or '_NOT_1'
-#define ___TERN(P,V...)     THIRD(P,V)              // If first argument has a comma, A. Else B.
-
-
-// Use NUM_ARGS(__VA_ARGS__) to get the number of variadic arguments
-#define _NUM_ARGS(_,Z,Y,X,W,V,U,T,S,R,Q,P,O,N,M,L,K,J,I,H,G,F,E,D,C,B,A,OUT,...) OUT
-#define NUM_ARGS(V...) _NUM_ARGS(0,V,26,25,24,23,22,21,20,19,18,17,16,15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0)
-
-//
-// Primitives supporting precompiler REPEAT
-//
-#define FIRST(a,...)     a
-#define SECOND(a,b,...)  b
-#define THIRD(a,b,c,...) c
-
-#define IS_PROBE(V...) SECOND(V, 0)     // Get the second item passed, or 0
-#define NOT(x) IS_PROBE(_CAT(_NOT_, x)) // NOT('0') gets '1'. Anything else gets '0'.
-
-#endif //MACROS_H
--- a/Firmware/meatpack.cpp
+++ b/Firmware/meatpack.cpp
@ -1,379 +0,0 @@
-/*
-* MeatPack G-Code Compression
-*
-* Algorithm & Implementation: Scott Mudge - mail@scottmudge.com
-* Date: Dec. 2020
-*/
-
-#include "meatpack.h"
-
-#ifdef ENABLE_MEATPACK
-
-#include "language.h"
-#include "Marlin.h"
-
-//#define MP_DEBUG
-
-// Utility definitions
-#define MeatPack_CommandByte		0b11111111
-#define MeatPack_NextPackedFirst	0b00000001
-#define MeatPack_NextPackedSecond	0b00000010
-
-#define MeatPack_SpaceCharIdx       11U
-#define MeatPack_SpaceCharReplace   'E'
-
-#define MeatPack_ProtocolVersion     "PV01"
-
-/*
-
-    Character Frequencies from ~30 MB of comment-stripped gcode:
-
-     '1' -> 4451136
-     '0' -> 4253577
-     ' ' -> 3053297
-     '.' -> 3035310
-     '2' -> 1523296
-     '8' -> 1366812
-     '4' -> 1353273
-     '9' -> 1352147
-     '3' -> 1262929
-     '5' -> 1189871
-     '6' -> 1127900
-     '7' -> 1112908
-    '\n' -> 1087683
-     'G' -> 1075806
-     'X' ->  975742
-     'E' ->  965275
-     'Y' ->  965274
-     'F' ->   99416
-     '-' ->   90242
-     'Z' ->   34109
-     'M' ->   11879
-     'S' ->    9910
-
-     If spaces are omitted, we add 'E'
-
-*/
-
-// Note:
-// I've tried both a switch/case method and a lookup table. The disassembly is exactly the same after compilation, byte-to-byte.
-// Thus, performance is identical.
-#define USE_LOOKUP_TABLE
-
-// State variables
-// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-enum MeatPack_ConfigStateFlags {
-    MPConfig_None = 0,
-    MPConfig_Active = (1 << 0),
-    MPConfig_NoSpaces = (1 << 1)
-};
-
-uint8_t mp_config = MPConfig_None; // Configuration state
-uint8_t mp_cmd_active = 0;         // Is a command is pending
-uint8_t mp_char_buf = 0;           // Buffers a character if dealing with out-of-sequence pairs
-uint8_t mp_cmd_count = 0;          // Counts how many command bytes are received (need 2)
-uint8_t mp_full_char_queue = 0;    // Counts how many full-width characters are to be received
-uint8_t mp_char_out_buf[2];        // Output buffer for caching up to 2 characters
-uint8_t mp_char_out_count = 0;     // Stores number of characters to be read out.
-
-
-#ifdef USE_LOOKUP_TABLE
-// The 15 most-common characters used in G-code, ~90-95% of all g-code uses these characters
-// NOT storing this with PROGMEM, given how frequently this table will be accessed.
-uint8_t MeatPackLookupTbl[16] = {
-    '0',	// 0000
-    '1',	// 0001
-    '2',	// 0010
-    '3',	// 0011
-    '4',	// 0100
-    '5',	// 0101
-    '6',	// 0110
-    '7',	// 0111
-    '8',	// 1000
-    '9',	// 1001
-    '.',	// 1010
-    ' ',	// 1011
-    '\n',	// 1100
-    'G',	// 1101
-    'X',	// 1110
-    '\0' // never used, 0b1111 is used to indicate next 8-bits is a full character
-};
-#else
-inline uint8_t get_char(const uint8_t in) {
-    switch (in) {
-    case 0b0000:
-        return '0';
-        break;
-    case 0b0001:
-        return '1';
-        break;
-    case 0b0010:
-        return '2';
-        break;
-    case 0b0011:
-        return '3';
-        break;
-    case 0b0100:
-        return '4';
-        break;
-    case 0b0101:
-        return '5';
-        break;
-    case 0b0110:
-        return '6';
-        break;
-    case 0b0111:
-        return '7';
-        break;
-    case 0b1000:
-        return '8';
-        break;
-    case 0b1001:
-        return '9';
-        break;
-    case 0b1010:
-        return '.';
-        break;
-    case 0b1011:
-        return (mp_config & MPConfig_NoSpaces) ? MeatPack_SpaceCharReplace : ' ';
-        break;
-    case 0b1100:
-        return '\n';
-        break;
-    case 0b1101:
-        return 'G';
-        break;
-    case 0b1110:
-        return 'X';
-        break;
-    }
-    return 0;
-}
-#endif
-
-// #DEBUGGING
-#ifdef MP_DEBUG
-uint32_t mp_chars_decoded = 0;
-#endif
-
-void FORCE_INLINE mp_handle_output_char(const uint8_t c) {
-    mp_char_out_buf[mp_char_out_count++] = c;
-
-#ifdef MP_DEBUG
-    if (mp_chars_decoded < 4096) {
-        ++mp_chars_decoded;
-        SERIAL_ECHOPGM("RB: ");
-        MYSERIAL.print((char)c);
-        SERIAL_ECHOLNPGM("");
-    }
-#endif
-}
-
-// Storing
-// packed = ((low & 0xF) << 4) | (high & 0xF);
-
-// Unpacking
-// low = (packed >> 4) & 0xF;
-// high = (packed & 0xF);
-
-//==========================================================================
-uint8_t FORCE_INLINE mp_unpack_chars(const uint8_t pk, uint8_t* __restrict const chars_out) {
-    uint8_t out = 0;
-
-#ifdef USE_LOOKUP_TABLE
-    // If lower 4 bytes is 0b1111, the higher 4 are unused, and next char is full.
-    if ((pk & MeatPack_FirstNotPacked) == MeatPack_FirstNotPacked) out |= MeatPack_NextPackedFirst;
-    else chars_out[0] = MeatPackLookupTbl[(pk & 0xF)]; // Assign lower char
-
-    // Check if upper 4 bytes is 0b1111... if so, we don't need the second char.
-    if ((pk & MeatPack_SecondNotPacked) == MeatPack_SecondNotPacked) out |= MeatPack_NextPackedSecond;
-    else chars_out[1] = MeatPackLookupTbl[((pk >> 4) & 0xf)]; // Assign upper char
-#else
-    // If lower 4 bytes is 0b1111, the higher 4 are unused, and next char is full.
-    if ((pk & MeatPack_FirstNotPacked) == MeatPack_FirstNotPacked) out |= MeatPack_NextPackedFirst;
-    else chars_out[0] = get_char(pk & 0xF); // Assign lower char
-
-    // Check if upper 4 bytes is 0b1111... if so, we don't need the second char.
-    if ((pk & MeatPack_SecondNotPacked) == MeatPack_SecondNotPacked) out |= MeatPack_NextPackedSecond;
-    else chars_out[1] = get_char((pk >> 4) & 0xf); // Assign upper char
-#endif
-
-    return out;
-}
-
-//==============================================================================
-void FORCE_INLINE mp_reset_state() {
-    mp_char_out_count = 0;
-    mp_cmd_active = MPCommand_None;
-    mp_config = MPConfig_None;
-    mp_char_buf = 0;
-    mp_cmd_count = 0;
-    mp_cmd_active = 0;
-    mp_full_char_queue = 0;
-
-#ifdef MP_DEBUG
-    mp_chars_decoded = 0;
-    SERIAL_ECHOLNPGM("MP Reset");
-#endif
-}
-
-//==========================================================================
-void FORCE_INLINE mp_handle_rx_char_inner(const uint8_t c) {
-
-    // Packing enabled, handle character and re-arrange them appropriately.
-    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-    if (mp_config & MPConfig_Active) {
-        if (mp_full_char_queue > 0) {
-            mp_handle_output_char(c);
-                if (mp_char_buf > 0) {
-                    mp_handle_output_char(mp_char_buf);
-                        mp_char_buf = 0;
-                }
-            --mp_full_char_queue;
-        }
-        else {
-            uint8_t buf[2] = { 0,0 };
-            const uint8_t res = mp_unpack_chars(c, buf);
-
-            if (res & MeatPack_NextPackedFirst) {
-                ++mp_full_char_queue;
-                if (res & MeatPack_NextPackedSecond) ++mp_full_char_queue;
-                else mp_char_buf = buf[1];
-            }
-            else {
-                mp_handle_output_char(buf[0]);
-                if (buf[0] != '\n') {
-                    if (res & MeatPack_NextPackedSecond) ++mp_full_char_queue;
-                    else mp_handle_output_char(buf[1]);
-                }
-            }
-        }
-    }
-    else // Packing not enabled, just copy character to output
-        mp_handle_output_char(c);
-}
-
-//==========================================================================
-void FORCE_INLINE mp_echo_config_state() {
-    SERIAL_ECHOPGM(" [MP] "); // Add space at idx 0 just in case first character is dropped due to timing/sync issues.
-
-    // NOTE: if any configuration vars are added below, the outgoing sync text for host plugin
-    // should not contain the "PV' substring, as this is used to indicate protocol version
-    SERIAL_ECHOPGM(MeatPack_ProtocolVersion);
-
-    // Echo current state
-    if (mp_config & MPConfig_Active)
-        SERIAL_ECHOPGM(" ON");
-    else
-        SERIAL_ECHOPGM(" OFF");
-
-    if (mp_config & MPConfig_NoSpaces)
-        SERIAL_ECHOPGM(" NSP"); // [N]o [SP]aces
-    else
-        SERIAL_ECHOPGM(" ESP"); // [E]nabled [SP]aces
-
-    SERIAL_ECHOLNPGM("");
-
-    // Validate config vars
-#ifdef USE_LOOKUP_TABLE
-    if (mp_config & MPConfig_NoSpaces)
-        MeatPackLookupTbl[MeatPack_SpaceCharIdx] = (uint8_t)(MeatPack_SpaceCharReplace);
-    else
-        MeatPackLookupTbl[MeatPack_SpaceCharIdx] = ' ';
-#endif
-
-}
-
-//==========================================================================
-void FORCE_INLINE mp_handle_cmd(const MeatPack_Command c) {
-    switch (c) {
-    case MPCommand_EnablePacking: {
-        mp_config |= MPConfig_Active;
-#ifdef MP_DEBUG
-        SERIAL_ECHOLNPGM("[MPDBG] ENABL REC");
-#endif
-    } break;
-    case MPCommand_DisablePacking: {
-        mp_config &= ~(MPConfig_Active);
-#ifdef MP_DEBUG
-        SERIAL_ECHOLNPGM("[MPDBG] DISBL REC");
-#endif
-    } break;
-    case MPCommand_ResetAll: {
-        mp_reset_state();
-#ifdef MP_DEBUG
-        SERIAL_ECHOLNPGM("[MPDBG] RESET REC");
-#endif
-    } break;
-    case MPCommand_EnableNoSpaces: {
-        mp_config |= MPConfig_NoSpaces;
-#ifdef MP_DEBUG
-        SERIAL_ECHOLNPGM("[MPDBG] ENABL NSP");
-#endif
-    } break;
-    case MPCommand_DisableNoSpaces: {
-        mp_config &= ~(MPConfig_NoSpaces);
-#ifdef MP_DEBUG
-        SERIAL_ECHOLNPGM("[MPDBG] DISBL NSP");
-#endif
-    } break;
-    default: {
-#ifdef MP_DEBUG
-        SERIAL_ECHOLN("[MPDBG] UNK CMD REC");
-#endif
-    }
-    case MPCommand_QueryConfig:
-        break;
-    }
-
-    mp_echo_config_state();
-}
-
-//==========================================================================
-void mp_handle_rx_char(const uint8_t c) {
-
-    // Check for commit complete
-    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-    if (c == (uint8_t)(MeatPack_CommandByte)) {
-        if (mp_cmd_count > 0) {
-            mp_cmd_active = 1;
-            mp_cmd_count = 0;
-        }
-        else
-            ++mp_cmd_count;
-        return;
-    }
-
-    if (mp_cmd_active > 0) {
-        mp_handle_cmd((MeatPack_Command)c);
-        mp_cmd_active = 0;
-        return;
-    }
-
-    if (mp_cmd_count > 0) {
-        mp_handle_rx_char_inner((uint8_t)(MeatPack_CommandByte));
-        mp_cmd_count = 0;
-    }
-
-    mp_handle_rx_char_inner(c);
-}
-
-//==========================================================================
-uint8_t mp_get_result_char(char* const __restrict out) {
-    if (mp_char_out_count > 0) {
-        const uint8_t res = mp_char_out_count;
-        for (uint8_t i = 0; i < mp_char_out_count; ++i)
-            out[i] = (char)mp_char_out_buf[i];
-        mp_char_out_count = 0;
-        return res;
-    }
-    return 0;
-}
-
-//==============================================================================
-void mp_trigger_cmd(const MeatPack_Command cmd)
-{
-    mp_handle_cmd(cmd);
-}
-
-#endif
--- a/Firmware/meatpack.h
+++ b/Firmware/meatpack.h
@ -1,70 +0,0 @@
-/*
-* MeatPack G-Code Compression
-*
-* Algorithm & Implementation: Scott Mudge - mail@scottmudge.com
-* Date: Dec. 2020
-*
-* Specifically optimized for 3D printing G-Code, this is a zero-cost data compression method
-* which packs ~180-190% more data into the same amount of bytes going to the CNC controller.
-* As a majority of G-Code can be represented by a restricted alphabet, I performed histogram
-* analysis on a wide variety of 3D printing gcode samples, and found ~93% of all gcode could
-* be represented by the same 15-character alphabet.
-*
-* This allowed me to design a system of packing 2 8-bit characters into a single byte, assuming
-* they fall within this limited 15-character alphabet. Using a 4-bit lookup table, these 8-bit
-* characters can be represented by a 4-bit index.
-*
-* Combined with some logic to allow commingling of full-width characters outside of this 15-
-* character alphabet (at the cost of an extra 8-bits per full-width character), and by stripping
-* out unnecessary comments, the end result is gcode which is roughly half the original size.
-*
-* Why did I do this? I noticed micro-stuttering and other data-bottleneck issues while printing
-* objects with high curvature, especially at high speeds. There is also the issue of the limited
-* baud rate provided by Prusa's Atmega2560-based boards, over the USB serial connection. So soft-
-* ware like OctoPrint would also suffer this same micro-stuttering and poor print quality issue.
-*
-*/
-#include <stdint.h>
-#include "Configuration.h"
-
-#ifndef MEATPACK_H_
-#define MEATPACK_H_
-
-#ifdef ENABLE_MEATPACK
-
-#define MeatPack_SecondNotPacked    0b11110000
-#define MeatPack_FirstNotPacked     0b00001111
-
-// These are commands sent to MeatPack to control its behavior.
-// They are sent by first sending 2x MeatPack_CommandByte (0xFF) in sequence,
-//      followed by one of the command bytes below.
-// Provided that 0xFF is an exceedingly rare character that is virtually never
-// present in g-code naturally, it is safe to assume 2 in sequence should never
-// happen naturally, and so it is used as a signal here.
-//
-// 0xFF *IS* used in "packed" g-code (used to denote that the next 2 characters are
-// full-width), however 2 in a row will never occur, as the next 2 bytes will always
-// some non-0xFF character.
-enum MeatPack_Command {
-    MPCommand_None            = 0U,
-    // MPCommand_TogglePacking   = 253U, -- Unused, byte 253 can be re-used later.
-    MPCommand_EnablePacking   = 251U,
-    MPCommand_DisablePacking  = 250U,
-    MPCommand_ResetAll        = 249U,
-    MPCommand_QueryConfig     = 248U,
-    MPCommand_EnableNoSpaces  = 247U,
-    MPCommand_DisableNoSpaces = 246U
-};
-
-// Pass in a character rx'd by SD card or serial. Automatically parses command/ctrl sequences,
-// and will control state internally.
-extern void mp_handle_rx_char(const uint8_t c);
-
-// After passing in rx'd char using above method, call this to get characters out. Can return
-// from 0 to 2 characters at once.
-// @param out [in] Output pointer for unpacked/processed data.
-// @return Number of characters returned. Range from 0 to 2.
-extern uint8_t mp_get_result_char(char* const __restrict out);
-#endif
-
-#endif // MEATPACK_H_
--- a/Show More
+++ b/Show More