# VanAlign Pro – Neigungsmessung über BLE
#
# ESP32-S3 mit MPU6050. Liefert Pitch und Roll als 32-Bit-Float
# (little-endian) und lässt sich über Bluetooth auf die aktuelle Lage
# kalibrieren.
#
# Die UUIDs sind gegenüber Version 1.0 unverändert, damit die vorhandene
# WebApp weiterläuft. Neu sind:
#   * notify auf Pitch und Roll – die Werte werden gesendet, sobald sie sich
#     ändern, statt vom Client im Sekundentakt abgefragt zu werden.
#   * eine funktionierende Kalibrierung über Bluetooth. Bisher war die
#     Charakteristik zwar beschreibbar, hatte aber keine Aktion hinterlegt,
#     und der Kalibrier-Knopf war nur über den Webserver erreichbar, der
#     auskommentiert ist. Kalibrieren war damit gar nicht möglich.
#   * die gespeicherten Offsets sind auslesbar, sodass ein Client zeigen
#     kann, ob überhaupt schon kalibriert wurde.

esphome:
  name: vanalign
  friendly_name: "VanAlign Pro"
  on_boot:
    priority: -100
    then:
      - script.execute: publish_offsets

esp32:
  board: esp32-s3-devkitc-1
  framework:
    type: esp-idf

logger:
  level: WARN

i2c:
  sda: GPIO8
  scl: GPIO9
  scan: true

globals:
  - id: pitch_offset
    type: float
    restore_value: yes
    initial_value: '0.0'
  - id: roll_offset
    type: float
    restore_value: yes
    initial_value: '0.0'

sensor:
  - platform: mpu6050
    #address: 0x69
    accel_x:
      id: accel_x
      internal: true
    accel_y:
      id: accel_y
      internal: true
    accel_z:
      id: accel_z
      internal: true
    gyro_x:
      id: mpu_gyro_x
      internal: true
    gyro_y:
      id: mpu_gyro_y
      internal: true
    gyro_z:
      id: mpu_gyro_z
      internal: true
    update_interval: 0.1s

  - platform: template
    name: "Neigung Pitch"
    id: pitch
    icon: mdi:caravan
    unit_of_measurement: "°"
    accuracy_decimals: 1
    update_interval: 0.1s
    lambda: |-
      if (isnan(id(accel_x).state) || isnan(id(accel_y).state) || isnan(id(accel_z).state)) {
        return NAN;
      }
      float raw = atan2(id(accel_y).state,
                        sqrt(pow(id(accel_x).state, 2) + pow(id(accel_z).state, 2)))
                  * (180.0 / 3.14159265);
      return raw - id(pitch_offset);
    filters:
      - sliding_window_moving_average:
          window_size: 8
          send_every: 1
      - exponential_moving_average:
          alpha: 0.2
    on_value:
      # Ohne dieses Senden müsste der Client die Werte abfragen; die Anzeige
      # ruckelt dann im Takt der Abfrage statt der Messung.
      - ble_server.characteristic.set_value:
          id: pitch_ble
          value: !lambda |-
            std::vector<uint8_t> v(sizeof(float));
            float val = x;
            memcpy(v.data(), &val, sizeof(float));
            return v;

  - platform: template
    name: "Neigung Roll"
    id: roll
    icon: mdi:axis-x-rotate-clockwise
    unit_of_measurement: "°"
    accuracy_decimals: 1
    update_interval: 0.1s
    lambda: |-
      if (isnan(id(accel_x).state) || isnan(id(accel_z).state)) {
        return NAN;
      }
      float raw = atan2(-id(accel_x).state, id(accel_z).state) * (180.0 / 3.14159265);
      return raw - id(roll_offset);
    filters:
      - sliding_window_moving_average:
          window_size: 8
          send_every: 1
      - exponential_moving_average:
          alpha: 0.2
    on_value:
      - ble_server.characteristic.set_value:
          id: roll_ble
          value: !lambda |-
            std::vector<uint8_t> v(sizeof(float));
            float val = x;
            memcpy(v.data(), &val, sizeof(float));
            return v;

script:
  # Die aktuelle Lage wird zur neuen Null. Beide Wege – Knopf und Bluetooth –
  # laufen hier zusammen, damit sie nicht auseinanderdriften.
  - id: calibrate_level
    then:
      - lambda: |-
          if (isnan(id(accel_x).state) || isnan(id(accel_y).state) || isnan(id(accel_z).state)) {
            ESP_LOGW("vanalign", "Kalibrierung abgebrochen: keine Sensorwerte");
            return;
          }
          id(pitch_offset) = atan2(id(accel_y).state,
                                   sqrt(pow(id(accel_x).state, 2) + pow(id(accel_z).state, 2)))
                             * (180.0 / 3.14159265);
          id(roll_offset) = atan2(-id(accel_x).state, id(accel_z).state) * (180.0 / 3.14159265);
          ESP_LOGI("vanalign", "Kalibriert: pitch_offset=%.2f roll_offset=%.2f",
                   id(pitch_offset), id(roll_offset));

      - script.execute: publish_offsets

  - id: reset_calibration
    then:
      - lambda: |-
          id(pitch_offset) = 0.0f;
          id(roll_offset) = 0.0f;
          ESP_LOGI("vanalign", "Kalibrierung zurückgesetzt");
      - script.execute: publish_offsets

  # Die gespeicherten Offsets für lesende Clients bereitstellen.
  - id: publish_offsets
    then:
      - ble_server.characteristic.set_value:
          id: offsets_ble
          value: !lambda |-
            std::vector<uint8_t> v(2 * sizeof(float));
            float values[2] = { id(pitch_offset), id(roll_offset) };
            memcpy(v.data(), values, sizeof(values));
            return v;

esp32_ble_server:
  manufacturer: "VanAlign"
  model: "Pro"
  services:
    - uuid: 2a24b789-7aab-4535-af3e-ee76a35cc42d
      advertise: true
      characteristics:
        - id: pitch_ble
          uuid: cad48e28-7fbe-41cf-bae9-d77a6c233424
          description: "Pitch"
          read: true
          notify: true

        - id: roll_ble
          uuid: cad48e28-7fbe-41cf-bae9-d77a6c233425
          description: "Roll"
          read: true
          notify: true

        # Zwei Floats: die gespeicherten Offsets. Damit kann ein Client
        # anzeigen, ob und worauf kalibriert wurde.
        - id: offsets_ble
          uuid: cad48e28-7fbe-41cf-bae9-d77a6c233426
          description: "Kalibrier-Offsets"
          read: true
          notify: true

        # Ein Byte: 0 setzt die Kalibrierung zurück, alles andere kalibriert
        # auf die aktuelle Lage.
        - id: calib_ble
          uuid: cad48e28-7fbe-41cf-bae9-d77a6c233427
          description: "Kalibriere Neigung"
          write: true
          on_write:
            then:
              - lambda: |-
                  bool reset = !x.empty() && (x[0] == 0x00 || x[0] == '0');
                  if (reset) {
                    id(reset_calibration).execute();
                  } else {
                    id(calibrate_level).execute();
                  }

button:
  - platform: template
    name: "Kalibriere Neigung"
    id: calib_button
    on_press:
      - script.execute: calibrate_level

  - platform: template
    name: "Kalibrierung zurücksetzen"
    id: calib_reset_button
    on_press:
      - script.execute: reset_calibration

  - platform: restart
    name: "ESP Restart"

text_sensor:
  - platform: template
    name: "Firmware Version"
    id: firmware_version
    icon: mdi:tag
    lambda: |-
      return {"v1.1.0"};