mmwave

# This is a custom firmware for Human Presence Sensor 2A
# https://docs.screek.io/2a
# Current version synchronized with public version: V230907_1_beta

esphome:
  name: ld2450-sensor-esp32c3
  comment: LD2450 Presence Sensor
  friendly_name: LD2450 Presence Sensor esp32c3
  name_add_mac_suffix: false
  platformio_options:
    board_build.flash_mode: dio
    # board_build.f_cpu: 80000000L
  project:
    name: Screek.Human_Presence_Sensor
    version: 2A
  on_boot:
    - priority: 100
      then:
        lambda: |-
          id(cpu_speed) = ESP.getCpuFreqMHz();
    - priority: -200
      then:
        lambda: |-
          id(zone1_target_exsits).publish_state(false);
          id(zone2_target_exsits).publish_state(false);
          id(zone3_target_exsits).publish_state(false);
          id(zone_ex1_target_exsits).publish_state(false);

preferences:
    flash_write_interval: 5s

external_components:
  - source:
      type: git
      url: https://github.com/screekworkshop/custom_components_support_for_screek_2a
      ref: main
    components: [esp32, uart]

esp32:
  board: esp32-c3-devkitm-1
  framework:
    type: arduino

globals:
  - id: cpu_speed
    type: int
    restore_value: no
    initial_value: '0'
  - id: last_update_ld2450
    type: unsigned long
    restore_value: no
    initial_value: '0'
  - id: init_zone_publish
    type: bool
    restore_value: no
    initial_value: "false"
  - id: last_illuminance
    type: float
    restore_value: no
    initial_value: "-1"

logger:

debug:
  update_interval: 30s

api:

ota:

wifi:
  ssid: !secret wifi_ssid
  password: !secret wifi_password

  ap:
    ssid: "HUMAN-SENSOR 2A Store"

captive_portal:

web_server:
  port: 80


text_sensor:
  - platform: debug
    reset_reason:
      name: "ESP Reset Reason"
      icon: mdi:anchor
      disabled_by_default: True
  - platform: wifi_info
    ip_address:
      name: ESP IP Address
      entity_category: "diagnostic"
      disabled_by_default: True
      icon: mdi:ip-network
    mac_address:
      name: ESP MAC
      entity_category: "diagnostic"
      icon: mdi:ip-network
      disabled_by_default: True
  - platform: template
    name: "Zone1 Info"
    id: tips_zone1_conf
    icon: mdi:information-outline
    entity_category: config
    lambda: |-
      return {"Configure below" };
    update_interval: 1000s
  - platform: template
    name: "Zone2 Info"
    id: tips_zone2_conf
    icon: mdi:information-outline
    entity_category: config
    lambda: |-
      return {"Configure below" };
    update_interval: 1000s
  - platform: template
    name: "Zone3 Info"
    id: tips_zone3_conf
    icon: mdi:information-outline
    entity_category: config
    lambda: |-
      return {"Configure below" };
    update_interval: 1000s
  - platform: template
    name: "Zout1 Info"
    id: tips_zone_ex1_conf
    icon: mdi:information-outline
    entity_category: config
    lambda: |-
      return {"Zone Exclusion 1" };
    update_interval: 1000s
  - platform: template
    name: "Any-Presence Info"
    id: tips_any_presence_conf
    icon: mdi:information-outline
    entity_category: config
    lambda: |-
      return {"Any Presence Config" };
    update_interval: 1000s
  - platform: template
    name: "Target1 Direction"
    id: target1_direction
    icon: mdi:directions
  - platform: template
    name: "Target2 Direction"
    id: target2_direction
    icon: mdi:directions
  - platform: template
    name: "Target3 Direction"
    id: target3_direction
    icon: mdi:directions
  - platform: template
    name: "Target1 Position"
    id: target1_position
    icon: mdi:directions
  - platform: template
    name: "Target2 Position"
    id: target2_position
    icon: mdi:directions
  - platform: template
    name: "Target3 Position"
    id: target3_position
    icon: mdi:directions

number:
  - platform: template
    name: "Any Presence Timeout"
    id: any_presence_timeout
    min_value: 0
    max_value: 600
    mode: box
    device_class: duration
    entity_category: config
    unit_of_measurement: s
    icon: mdi:timer-off
    step: 1
    optimistic: True
    initial_value: 0
    restore_value: True
  - platform: template
    name: "Zone1 Timeout"
    id: zone1_x_timeout
    min_value: 0
    max_value: 600
    mode: box
    device_class: duration
    entity_category: config
    unit_of_measurement: s
    icon: mdi:timer-off
    step: 1
    optimistic: True
    initial_value: 0
    restore_value: True
  - platform: template
    name: "Zone2 Timeout"
    id: zone2_x_timeout
    min_value: 0
    max_value: 600
    mode: box
    device_class: duration
    entity_category: config
    unit_of_measurement: s
    icon: mdi:timer-off
    step: 1
    optimistic: True
    initial_value: 0
    restore_value: True
  - platform: template
    name: "Zone3 Timeout"
    id: zone3_x_timeout
    min_value: 0
    max_value: 600
    mode: box
    device_class: duration
    entity_category: config
    unit_of_measurement: s
    icon: mdi:timer-off
    step: 1
    optimistic: True
    initial_value: 0
    restore_value: True
  # Zone 1
  - platform: template
    name: Zone1 X-Begin
    id: zone1_x_begin
    min_value: -4000
    max_value: 4000
    mode: box
    device_class: distance
    entity_category: config
    unit_of_measurement: mm
    icon: mdi:arrow-left-bold
    step: 10
    optimistic: True
    initial_value: 0
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone1_vaild
  - platform: template
    name: Zone1 X-End
    id: zone1_x_end
    mode: box
    min_value: -4000
    max_value: 4000
    device_class: distance
    unit_of_measurement: mm
    entity_category: config
    icon: mdi:arrow-right-bold
    step: 10
    initial_value: 0
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone1_vaild
  - platform: template
    name: Zone1 Y-Begin
    id: zone1_y_begin
    mode: box
    min_value: 0
    max_value: 6000
    device_class: distance
    entity_category: config
    icon: mdi:arrow-up-bold
    unit_of_measurement: mm
    step: 10
    initial_value: 0
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone1_vaild
  - platform: template
    name: Zone1 Y-End
    id: zone1_y_end
    icon: mdi:arrow-down-bold
    mode: box
    min_value: 0
    max_value: 6000
    initial_value: 0
    entity_category: config
    device_class: distance
    unit_of_measurement: mm
    step: 10
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone1_vaild

  # Zone 2
  - platform: template
    name: Zone2 X-Begin
    id: zone2_x_begin
    min_value: -4000
    max_value: 4000
    mode: box
    device_class: distance
    entity_category: config
    unit_of_measurement: mm
    icon: mdi:arrow-left-bold
    step: 10
    optimistic: True
    initial_value: 0
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone2_vaild
  - platform: template
    name: Zone2 X-End
    id: zone2_x_end
    mode: box
    min_value: -4000
    max_value: 4000
    device_class: distance
    unit_of_measurement: mm
    entity_category: config
    icon: mdi:arrow-right-bold
    step: 10
    initial_value: 0
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone2_vaild
  - platform: template
    name: Zone2 Y-Begin
    id: zone2_y_begin
    mode: box
    min_value: 0
    max_value: 6000
    device_class: distance
    entity_category: config
    icon: mdi:arrow-up-bold
    unit_of_measurement: mm
    step: 10
    initial_value: 0
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone2_vaild
  - platform: template
    name: Zone2 Y-End
    id: zone2_y_end
    icon: mdi:arrow-down-bold
    mode: box
    min_value: 0
    max_value: 6000
    initial_value: 0
    entity_category: config
    device_class: distance
    unit_of_measurement: mm
    step: 10
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone2_vaild

  # Zone 3
  - platform: template
    name: Zone3 X-Begin
    id: zone3_x_begin
    min_value: -4000
    max_value: 4000
    mode: box
    device_class: distance
    entity_category: config
    unit_of_measurement: mm
    icon: mdi:arrow-left-bold
    step: 10
    optimistic: True
    initial_value: 0
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone3_vaild
  - platform: template
    name: Zone3 X-End
    id: zone3_x_end
    mode: box
    min_value: -4000
    max_value: 4000
    device_class: distance
    unit_of_measurement: mm
    entity_category: config
    icon: mdi:arrow-right-bold
    step: 10
    initial_value: 0
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone3_vaild
  - platform: template
    name: Zone3 Y-Begin
    id: zone3_y_begin
    mode: box
    min_value: 0
    max_value: 6000
    device_class: distance
    entity_category: config
    icon: mdi:arrow-up-bold
    unit_of_measurement: mm
    step: 10
    initial_value: 0
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone3_vaild
  - platform: template
    name: Zone3 Y-End
    id: zone3_y_end
    icon: mdi:arrow-down-bold
    mode: box
    min_value: 0
    max_value: 6000
    initial_value: 0
    entity_category: config
    device_class: distance
    unit_of_measurement: mm
    step: 10
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zone3_vaild

  # Zout1
  - platform: template
    name: Zout1 X-Begin
    id: zone_ex1_x_begin
    min_value: -4000
    max_value: 4000
    mode: box
    device_class: distance
    entity_category: config
    unit_of_measurement: mm
    icon: mdi:arrow-left-bold
    step: 10
    optimistic: True
    initial_value: 0
    restore_value: True
    on_value:
      then:
        - script.execute: check_zout1_vaild
  - platform: template
    name: Zout1 X-End
    id: zone_ex1_x_end
    mode: box
    min_value: -4000
    max_value: 4000
    device_class: distance
    unit_of_measurement: mm
    entity_category: config
    icon: mdi:arrow-right-bold
    step: 10
    initial_value: 0
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zout1_vaild
  - platform: template
    name: Zout1 Y-Begin
    id: zone_ex1_y_begin
    mode: box
    min_value: 0
    max_value: 6000
    device_class: distance
    entity_category: config
    icon: mdi:arrow-up-bold
    unit_of_measurement: mm
    step: 10
    initial_value: 0
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zout1_vaild
  - platform: template
    name: Zout1 Y-End
    id: zone_ex1_y_end
    icon: mdi:arrow-down-bold
    mode: box
    min_value: 0
    max_value: 6000
    initial_value: 0
    entity_category: config
    device_class: distance
    unit_of_measurement: mm
    step: 10
    optimistic: True
    restore_value: True
    on_value:
      then:
        - script.execute: check_zout1_vaild

binary_sensor:
  - platform: status
    name: Online
    id: ink_ha_connected
  - platform: template
    name: "Any Presence"
    id: any_target_exsits
    device_class: occupancy
    filters:
      - delayed_off: !lambda |-
          if (!id(init_zone_publish) || !id(zone_fn_enable).state) {
            return 0;
          };
          return id(any_presence_timeout).state * 1000.0;
  - platform: template
    name: "Zone1 Presence"
    id: zone1_target_exsits
    device_class: occupancy
    filters:
      - delayed_off: !lambda |-
          if (!id(init_zone_publish) || !id(zone_fn_enable).state) {
            return 0;
          }
          return id(zone1_x_timeout).state * 1000.0;
  - platform: template
    name: "Zone2 Presence"
    id: zone2_target_exsits
    device_class: occupancy
    filters:
      - delayed_off: !lambda |-
          if (!id(init_zone_publish) || !id(zone_fn_enable).state) {
            return 0;
          }
          return id(zone2_x_timeout).state * 1000.0;
  - platform: template
    name: "Zone3 Presence"
    id: zone3_target_exsits
    device_class: occupancy
    filters:
      - delayed_off: !lambda |-
          if (!id(init_zone_publish) || !id(zone_fn_enable).state) {
            return 0;
          }
          return id(zone3_x_timeout).state * 1000.0;
  - platform: template
    name: "Zout1 Presence"
    id: zone_ex1_target_exsits
    icon: mdi:account-multiple-remove
    device_class: occupancy

i2c:
  sda: GPIO19
  scl: GPIO18
  scan: true
  id: bus_a

script:
  - id: check_zone1_vaild
    then:
      - lambda: |-
          if (id(zone1_x_begin).state > id(zone1_x_end).state){
            id(tips_zone1_conf).publish_state("Err: X-Begin > X-End");
            return;
          }
          if (id(zone1_y_begin).state > id(zone1_y_end).state){
            id(tips_zone1_conf).publish_state("Err: Y-Begin > Y-End");
            return;
          }
          if (id(zone1_x_begin).state == 0, id(zone1_x_end).state == 0, id(zone1_y_begin).state == 0, id(zone1_y_end).state == 0){
            id(tips_zone1_conf).publish_state("Configure below");
            return;
          }

          int x_size = id(zone1_x_end).state - id(zone1_x_begin).state;
          int y_size = id(zone1_y_end).state - id(zone1_y_begin).state;

          char combined[80];
          sprintf(combined, "Curr Size: %d x %d", x_size, y_size);
          id(tips_zone1_conf).publish_state(combined);
  - id: check_zone2_vaild
    then:
      - lambda: |-
          if (id(zone2_x_begin).state > id(zone2_x_end).state){
            id(tips_zone2_conf).publish_state("Err: X-Begin > X-End");
            return;
          }
          if (id(zone2_y_begin).state > id(zone2_y_end).state){
            id(tips_zone2_conf).publish_state("Err: Y-Begin > Y-End");
            return;
          }
          if (id(zone2_x_begin).state == 0, id(zone2_x_end).state == 0, id(zone2_y_begin).state == 0, id(zone2_y_end).state == 0){
            id(tips_zone2_conf).publish_state("Configure below");
            return;
          }

          int x_size = id(zone2_x_end).state - id(zone2_x_begin).state;
          int y_size = id(zone2_y_end).state - id(zone2_y_begin).state;

          char combined[80];
          sprintf(combined, "Curr Size: %d x %d", x_size, y_size);
          id(tips_zone2_conf).publish_state(combined);
  - id: check_zone3_vaild
    then:
      - lambda: |-
          if (id(zone3_x_begin).state > id(zone3_x_end).state){
            id(tips_zone3_conf).publish_state("Err: X-Begin > X-End");
            return;
          }
          if (id(zone3_y_begin).state > id(zone3_y_end).state){
            id(tips_zone3_conf).publish_state("Err: Y-Begin > Y-End");
            return;
          }
          if (id(zone3_x_begin).state == 0, id(zone3_x_end).state == 0, id(zone3_y_begin).state == 0, id(zone3_y_end).state == 0){
            id(tips_zone3_conf).publish_state("Configure below");
            return;
          }

          int x_size = id(zone3_x_end).state - id(zone3_x_begin).state;
          int y_size = id(zone3_y_end).state - id(zone3_y_begin).state;

          char combined[80];
          sprintf(combined, "Curr Size: %d x %d", x_size, y_size);
          id(tips_zone3_conf).publish_state(combined);
  - id: check_zout1_vaild
    then:
      - lambda: |-
          if (id(zone_ex1_x_begin).state > id(zone_ex1_x_end).state){
            id(tips_zone_ex1_conf).publish_state("Err: X-Begin > X-End");
            return;
          }
          if (id(zone_ex1_y_begin).state > id(zone_ex1_y_end).state){
            id(tips_zone_ex1_conf).publish_state("Err: Y-Begin > Y-End");
            return;
          }
          id(tips_zone_ex1_conf).publish_state("Zone Exclusion 1");

sensor:
  - platform: template
    name: "ESP CPU Speed"
    accuracy_decimals: 0
    icon: mdi:cpu-32-bit
    unit_of_measurement: Mhz
    disabled_by_default: True
    lambda: |-
      return (id(cpu_speed));
    entity_category: "diagnostic"
    update_interval: 600s
  - platform: template
    id: sys_esp_temperature
    name: ESP Temperature
    lambda: return temperatureRead();
    unit_of_measurement: °C
    device_class: TEMPERATURE
    update_interval: 45s
    entity_category: "diagnostic"
  - platform: uptime
    name: ESP Uptime
    id: sys_uptime
    update_interval: 60s
  - platform: wifi_signal
    name: RSSI
    id: wifi_signal_db
    update_interval: 60s
    entity_category: "diagnostic"
  - platform: template
    id: esp_memory
    icon: mdi:memory
    name: ESP Free Memory
    lambda: return heap_caps_get_free_size(MALLOC_CAP_INTERNAL) / 1024;
    unit_of_measurement: 'kB'
    state_class: measurement
    entity_category: "diagnostic"
    update_interval: 60s
  - platform: bh1750
    name: "Illuminance"
    accuracy_decimals: 1
    id: bh1750_light
    update_interval: 1s
    filters:
      - lambda: !lambda |-
          if (id(last_illuminance) == x){
            return {};
          }
          if (id(bh1750_fast_update).state){
            id(last_illuminance) = x;
            // ESP_LOGD("custom", "Fast Update BH1850");
            return x;
          }
          if (abs(id(last_illuminance) - x) > 1){

              id(last_illuminance) = x;
              return x;
          }
          return {};

  #-------------------------------------#
  # 高级雷达数据
  - platform: template
    name: "All Target Counts"
    id: all_target_count
    accuracy_decimals: 0
    icon: "mdi:counter"
    unit_of_measurement: "targets"
  - platform: template
    name: "Zone1 Target Counts"
    id: zone1_target_count
    accuracy_decimals: 0
    icon: "mdi:counter"
    unit_of_measurement: "targets"
  - platform: template
    name: "Zone2 Target Counts"
    id: zone2_target_count
    accuracy_decimals: 0
    icon: "mdi:counter"
    unit_of_measurement: "targets"
  - platform: template
    name: "Zone3 Target Counts"
    id: zone3_target_count
    accuracy_decimals: 0
    icon: "mdi:counter"
    unit_of_measurement: "targets"
  - platform: template
    name: "Zout1 Target Counts"
    id: zone_ex1_target_count
    accuracy_decimals: 0
    icon: mdi:account-multiple-minus-outline
    unit_of_measurement: "targets"

  # Target 1
  - platform: template
    name: "Target1 X"
    id: target1_x
    accuracy_decimals: 0
    unit_of_measurement: 'mm'
    state_class: measurement
    icon: mdi:focus-field-horizontal
    device_class: distance
  - platform: template
    name: "Target1 Y"
    id: target1_y
    accuracy_decimals: 0
    unit_of_measurement: 'mm'
    state_class: measurement
    device_class: distance
    icon: mdi:focus-field-vertical
  - platform: template
    name: "Target1 Speed"
    id: target1_speed
    accuracy_decimals: 2
    unit_of_measurement: 'm/s'
    state_class: measurement
    device_class: speed
  - platform: template
    name: "Target1 Resolution"
    id: target1_resolution
    accuracy_decimals: 0
    unit_of_measurement: 'mm'
    state_class: measurement
    device_class: distance

  # Target 2
  - platform: template
    name: "Target2 X"
    id: target2_x
    accuracy_decimals: 0
    unit_of_measurement: 'mm'
    state_class: measurement
    device_class: distance
    icon: mdi:focus-field-horizontal
  - platform: template
    name: "Target2 Y"
    id: target2_y
    accuracy_decimals: 0
    unit_of_measurement: 'mm'
    state_class: measurement
    device_class: distance
    icon: mdi:focus-field-vertical
  - platform: template
    name: "Target2 Speed"
    id: target2_speed
    accuracy_decimals: 0
    unit_of_measurement: 'm/s'
    state_class: measurement
    device_class: speed
  - platform: template
    name: "Target2 Resolution"
    id: target2_resolution
    accuracy_decimals: 0
    unit_of_measurement: 'mm'
    state_class: measurement
    device_class: distance

  # Target 3
  - platform: template
    name: "Target3 X"
    id: target3_x
    accuracy_decimals: 0
    unit_of_measurement: 'mm'
    state_class: measurement
    device_class: distance
    icon: mdi:focus-field-horizontal
  - platform: template
    name: "Target3 Y"
    id: target3_y
    accuracy_decimals: 0
    unit_of_measurement: 'mm'
    state_class: measurement
    device_class: distance
    icon: mdi:focus-field-vertical
  - platform: template
    name: "Target3 Speed"
    id: target3_speed
    accuracy_decimals: 0
    unit_of_measurement: 'm/s'
    state_class: measurement
    device_class: speed
  - platform: template
    name: "Target3 Resolution"
    id: target3_resolution
    accuracy_decimals: 0
    unit_of_measurement: 'mm'
    state_class: measurement
    device_class: distance
  - platform: template
    name: "Target1 Angle"
    id: target1_angle
    unit_of_measurement: 'º'
    accuracy_decimals: 1
    icon: mdi:angle-acute
  - platform: template
    name: "Target2 Angle"
    id: target2_angle
    accuracy_decimals: 1
    unit_of_measurement: 'º'
    icon: mdi:angle-acute
  - platform: template
    name: "Target3 Angle"
    id: target3_angle
    accuracy_decimals: 1
    unit_of_measurement: 'º'
    icon: mdi:angle-acute

light:
  - platform: status_led
    name: sys_status
    pin: GPIO10
    internal: True
    restore_mode: ALWAYS_OFF
  - platform: binary
    name: "Red Info Light"
    output: board_info_ed
    entity_category: diagnostic
    restore_mode: ALWAYS_OFF

time:
  - platform: sntp
    id: time_now

output:
  - platform: gpio
    id: board_info_ed
    pin: GPIO3

switch:
  - platform: factory_reset
    name: Factory Reset
    disabled_by_default: True
    icon: mdi:heart-broken
    entity_category: diagnostic
  - platform: template
    name: Zout1 Enable
    id: zone_ex1_enable
    optimistic: True
    icon: mdi:account-cancel
    entity_category: config
    restore_mode: RESTORE_DEFAULT_OFF
  - platform: template
    name: Zone Enable
    id: zone_fn_enable
    optimistic: True
    icon: mdi:target-variant
    entity_category: config
    restore_mode: RESTORE_DEFAULT_ON
  - platform: template
    name: Illuminance Fast-Update
    id: bh1750_fast_update
    optimistic: True
    entity_category: diagnostic
    restore_mode: RESTORE_DEFAULT_OFF
    icon: mdi:run-fast

button:
  - platform: restart
    icon: mdi:power-cycle
    name: "ESP Reboot"
    entity_category: diagnostic

uart:
  id: uart_bus
  tx_pin:
    number: GPIO21
    mode:
      input: true
      pullup: true
  rx_pin:
    number: GPIO20
    mode:
      input: true
      pullup: true
  baud_rate: 256000
  parity: NONE
  stop_bits: 1
  data_bits: 8
  debug:
    direction: BOTH
    dummy_receiver: True
    after:
     delimiter: [0X55, 0XCC]
    sequence:
      - lambda: |-
          if ((millis() - id(last_update_ld2450)) <= 500) {
            return;
          };
          id(last_update_ld2450) = millis();

          // p1
          int16_t p1_x = (uint16_t((bytes[5] << 8) | bytes[4] ));
          if ((bytes[5] & 0x80) >> 7){
            p1_x -= pow(2, 15);
          }else{
            p1_x = 0 - p1_x;
          }

          int16_t p1_y = (uint16_t((bytes[7] << 8) | bytes[6] ));
          if ((bytes[7] & 0x80) >> 7){
            p1_y -= pow(2, 15);
          }else{
            p1_y = 0 - p1_y;
          }

          int p1_speed = (bytes[9] << 8 | bytes[8] );
          if ((bytes[9] & 0x80) >> 7){
            p1_speed -= pow(2, 15);
          }else{
            p1_speed = 0 - p1_speed;
          }
          int16_t p1_distance_resolution = (uint16_t((bytes[11] << 8) | bytes[10] ));

          // p2
          int16_t p2_x = (uint16_t((bytes[13] << 8) | bytes[12] ));
          if ((bytes[13] & 0x80) >> 7){
            p2_x -=  pow(2, 15);
          }else{
            p2_x = 0 - p2_x;
          }

          int16_t p2_y = (uint16_t((bytes[15] << 8) | bytes[14] ));
          if ((bytes[15] & 0x80) >> 7){
            p2_y -= pow(2, 15);
          }else{
            p2_y = 0 - p2_y;
          }

          int p2_speed = (bytes[17] << 8 | bytes[16] );
          if ((bytes[17] & 0x80) >> 7){
            p2_speed -= pow(2, 15);
          }else{
            p2_speed = 0 - p2_speed;
          }
          int16_t p2_distance_resolution = (uint16_t((bytes[19] << 8) | bytes[18] ));

          // p3
          int16_t p3_x = (uint16_t((bytes[21] << 8) | bytes[20] ));
          if ((bytes[21] & 0x80) >> 7){
            p3_x -=  pow(2, 15);
          }else{
            p3_x = 0 - p3_x;
          }

          int16_t p3_y = (uint16_t((bytes[23] << 8) | bytes[22] ));
          if ((bytes[23] & 0x80) >> 7){
            p3_y -= pow(2, 15);
          }else{
            p3_y = 0 - p3_y;
          }

          int p3_speed = (bytes[25] << 8 | bytes[24] );
          if ((bytes[25] & 0x80) >> 7){
            p3_speed -= pow(2, 15);
          }else{
            p3_speed = 0 - p3_speed;
          }

          int16_t p3_distance_resolution = (uint16_t((bytes[27] << 8) | bytes[26] ));

          bool p1_vaild = (p1_x != 0 || p1_y > 0);
          bool p2_vaild = (p2_x != 0 || p2_y > 0);
          bool p3_vaild = (p3_x != 0 || p3_y > 0);

          // zone exlude 1

          int16_t target_count_in_zone_ex1 = 0;

          int16_t zone_ex1_x_min = id(zone_ex1_x_begin).state;
          int16_t zone_ex1_x_max = id(zone_ex1_x_end).state;
          int16_t zone_ex1_y_min = id(zone_ex1_y_begin).state;
          int16_t zone_ex1_y_max = id(zone_ex1_y_end).state;

          bool p1_zone_ex_enter = false;
          bool p2_zone_ex_enter = false;
          bool p3_zone_ex_enter = false;

          if (id(zone_ex1_enable).state){
            if (p1_vaild){
              if (p1_x >= zone_ex1_x_min && p1_x <= zone_ex1_x_max && p1_y >= zone_ex1_y_min && p1_y <= zone_ex1_y_max){
                  p1_zone_ex_enter = true;
                  target_count_in_zone_ex1 ++;
              }
            }
            if (p2_vaild){
              if (p2_x >= zone_ex1_x_min && p2_x <= zone_ex1_x_max && p2_y >= zone_ex1_y_min && p2_y <= zone_ex1_y_max){
                  p2_zone_ex_enter = true;
                  target_count_in_zone_ex1 ++;
              }
            }
            if (p3_vaild){
              if (p3_x >= zone_ex1_x_min && p3_x <= zone_ex1_x_max && p3_y >= zone_ex1_y_min && p3_y <= zone_ex1_y_max){
                  p3_zone_ex_enter = true;
                  target_count_in_zone_ex1 ++;
              }
            }
          }

          bool has_target_in_zone_ex1 = (target_count_in_zone_ex1 > 0);

          int16_t all_target_counts = 0;
          if (p1_vaild && !p1_zone_ex_enter){
            all_target_counts ++;
          }
          if (p2_vaild && !p2_zone_ex_enter){
            all_target_counts ++;
          }
          if (p3_vaild && !p3_zone_ex_enter){
            all_target_counts ++;
          }

          bool has_target_in_zone_all = (all_target_counts > 0);

          int16_t target_count_in_zone1 = 0;
          bool has_target_in_zone1 = false;

          int16_t target_count_in_zone2 = 0;
          bool has_target_in_zone2 = false;

          int16_t target_count_in_zone3 = 0;
          bool has_target_in_zone3 = false;

          if (id(zone_fn_enable).state){

            // zone 1 check

            int16_t zone1_x_min = id(zone1_x_begin).state;
            int16_t zone1_x_max = id(zone1_x_end).state;
            int16_t zone1_y_min = id(zone1_y_begin).state;
            int16_t zone1_y_max = id(zone1_y_end).state;

            if (p1_vaild && !p1_zone_ex_enter){
              if (p1_x >= zone1_x_min && p1_x <= zone1_x_max && p1_y >= zone1_y_min && p1_y <= zone1_y_max){
                  target_count_in_zone1 ++;
              }
            }
            if (p2_vaild && !p2_zone_ex_enter){
              if (p2_x >= zone1_x_min && p2_x <= zone1_x_max && p2_y >= zone1_y_min && p2_y <= zone1_y_max){
                  target_count_in_zone1 ++;
              }
            }
            if (p3_vaild && !p3_zone_ex_enter){
              if (p3_x >= zone1_x_min && p3_x <= zone1_x_max && p3_y >= zone1_y_min && p3_y <= zone1_y_max){
                  target_count_in_zone1 ++;
              }
            }
            has_target_in_zone1 = (target_count_in_zone1 > 0);

            // zone 2 check

            int16_t zone2_x_min = id(zone2_x_begin).state;
            int16_t zone2_x_max = id(zone2_x_end).state;
            int16_t zone2_y_min = id(zone2_y_begin).state;
            int16_t zone2_y_max = id(zone2_y_end).state;

            if (p1_vaild && !p1_zone_ex_enter){
              if (p1_x >= zone2_x_min && p1_x <= zone2_x_max && p1_y >= zone2_y_min && p1_y <= zone2_y_max){
                  target_count_in_zone2 ++;
              }
            }
            if (p2_vaild && !p2_zone_ex_enter){
              if (p2_x >= zone2_x_min && p2_x <= zone2_x_max && p2_y >= zone2_y_min && p2_y <= zone2_y_max){
                  target_count_in_zone2 ++;
              }
            }
            if (p3_vaild && !p3_zone_ex_enter){
              if (p3_x >= zone2_x_min && p3_x <= zone2_x_max && p3_y >= zone2_y_min && p3_y <= zone2_y_max){
                  target_count_in_zone2 ++;
              }
            }

            has_target_in_zone2 = (target_count_in_zone2 > 0);

            // zone 3 check

            int16_t zone3_x_min = id(zone3_x_begin).state;
            int16_t zone3_x_max = id(zone3_x_end).state;
            int16_t zone3_y_min = id(zone3_y_begin).state;
            int16_t zone3_y_max = id(zone3_y_end).state;

            if (p1_vaild && !p1_zone_ex_enter){
              if (p1_x >= zone3_x_min && p1_x <= zone3_x_max && p1_y >= zone3_y_min && p1_y <= zone3_y_max){
                  target_count_in_zone3 ++;
              }
            }
            if (p2_vaild && !p2_zone_ex_enter){
              if (p2_x >= zone3_x_min && p2_x <= zone3_x_max && p2_y >= zone3_y_min && p2_y <= zone3_y_max){
                  target_count_in_zone3 ++;
              }
            }
            if (p3_vaild && !p3_zone_ex_enter){
              if (p3_x >= zone3_x_min && p3_x <= zone3_x_max && p3_y >= zone3_y_min && p3_y <= zone3_y_max){
                  target_count_in_zone3 ++;
              }
            }
            has_target_in_zone3 = (target_count_in_zone3 > 0);

          }

          // Angle, Position and Direction, idea from walberjunior.

          float p1_angle = 0;
          if (p1_vaild){
            p1_angle = ((float)p1_x / (float)p1_y) * 180 / M_PI;;
          }

          std::basic_string<char> p1_position = "Static";
          if (p1_speed > 0) {
            p1_position = "Moving away";
          } else if (p1_speed < 0) {
            p1_position = "Approaching";
          }

          std::basic_string<char> p1_direction = "None";
          if (p1_x > 0) {
            p1_direction = "Right";
          } else if (p1_x < 0) {
            p1_direction = "Left";
          } else if (p1_y > 0){
            p1_direction = "Middle";
          }

          float p2_angle = 0;
          if (p2_vaild){
            p2_angle = ((float)p2_x / (float)p2_y) * 180 / M_PI;;
          }

          std::basic_string<char> p2_position = "Static";;
          if (p2_speed > 0) {
            p2_position = "Moving away";
          } else if (p2_speed < 0) {
            p2_position = "Approaching";
          }

          std::basic_string<char> p2_direction = "None";
          if (p2_x > 0) {
            p2_direction = "Right";
          } else if (p2_x < 0) {
            p2_direction = "Left";
          } else if (p2_y > 0){
            p2_direction = "Middle";
          }

          float p3_angle = 0;
          if (p3_vaild){
            p3_angle = ((float)p3_x / (float)p3_y) * 180 / M_PI;;
          }

          std::basic_string<char> p3_position = "Static";;
          if (p3_speed > 0) {
            p3_position = "Moving away";
          } else if (p3_speed < 0) {
            p3_position = "Approaching";
          }

          std::basic_string<char> p3_direction = "None";
          if (p3_x > 0) {
            p3_direction = "Right";
          } else if (p3_x < 0) {
            p3_direction = "Left";
          } else if (p3_y > 0){
            p3_direction = "Middle";
          }

          if (id(target1_angle).state != p1_angle){
            id(target1_angle).publish_state(p1_angle);
          }
          if (id(target2_angle).state != p2_angle){
            id(target2_angle).publish_state(p2_angle);
          }
          if (id(target3_angle).state != p3_angle){
            id(target3_angle).publish_state(p3_angle);
          }

          if (p1_position != id(target1_position).state){
            id(target1_position).publish_state(p1_position);
          }
          if (p2_position != id(target2_position).state){
            id(target2_position).publish_state(p2_position);
          }
          if (p3_position != id(target3_position).state){
            id(target3_position).publish_state(p3_position);
          }

          if (p1_direction != id(target1_direction).state){
            id(target1_direction).publish_state(p1_direction);
          }
          if (p2_direction != id(target2_direction).state){
            id(target2_direction).publish_state(p2_direction);
          }
          if (p3_direction != id(target3_direction).state){
            id(target3_direction).publish_state(p3_direction);
          }

          // public all info

          if (id(target1_x).state != p1_x){
            id(target1_x).publish_state(p1_x);
          }
          if (id(target1_y).state != p1_y){
            id(target1_y).publish_state(p1_y);
          }

          float p1_m_speed = float(p1_speed) / 100.0;
          if (id(target1_speed).state != p1_m_speed){
            id(target1_speed).publish_state(p1_m_speed);
          }
          if (id(target1_resolution).state != p1_distance_resolution){
            id(target1_resolution).publish_state(p1_distance_resolution);
          }

          if (id(target2_x).state != p2_x){
            id(target2_x).publish_state(p2_x);
          }
          if (id(target2_y).state != p2_y){
            id(target2_y).publish_state(p2_y);
          }
          if (id(target2_speed).state != p2_speed){
            id(target2_speed).publish_state(p2_speed);
          }
          if (id(target2_resolution).state != p2_distance_resolution){
            id(target2_resolution).publish_state(p2_distance_resolution);
          }

          if (id(target3_x).state != p3_x){
            id(target3_x).publish_state(p3_x);
          }
          if (id(target3_y).state != p3_y){
            id(target3_y).publish_state(p3_y);
          }
          if (id(target3_speed).state != p3_speed){
            id(target3_speed).publish_state(p3_speed);
          }
          if (id(target3_resolution).state != p3_distance_resolution){
            id(target3_resolution).publish_state(p3_distance_resolution);
          }

          // publish target info

          if (id(all_target_count).state != all_target_counts){
            id(all_target_count).publish_state(all_target_counts);
            id(any_target_exsits).publish_state(has_target_in_zone_all);
          }else if (id(any_target_exsits).state != has_target_in_zone_all){
            id(any_target_exsits).publish_state(has_target_in_zone_all);
          }

          if (id(zone1_target_count).state != target_count_in_zone1){
            id(zone1_target_count).publish_state(target_count_in_zone1);
            id(zone1_target_exsits).publish_state(has_target_in_zone1);
          }else if (id(zone1_target_exsits).state != has_target_in_zone1){
            id(zone1_target_exsits).publish_state(has_target_in_zone1);
          }

          if (id(zone2_target_count).state != target_count_in_zone2){
            id(zone2_target_count).publish_state(target_count_in_zone2);
            id(zone2_target_exsits).publish_state(has_target_in_zone2);
          }else if (id(zone2_target_exsits).state != has_target_in_zone2){
            id(zone2_target_exsits).publish_state(has_target_in_zone2);
          }

          if (id(zone3_target_count).state != target_count_in_zone3){
            id(zone3_target_count).publish_state(target_count_in_zone3);
            id(zone3_target_exsits).publish_state(has_target_in_zone3);
          }else if (id(zone3_target_exsits).state != has_target_in_zone3){
            id(zone3_target_exsits).publish_state(has_target_in_zone3);
          }

          // zout
          if (id(zone_ex1_target_count).state != target_count_in_zone_ex1){
            id(zone_ex1_target_count).publish_state(target_count_in_zone_ex1);
          }

          if (id(zone_ex1_target_exsits).state != has_target_in_zone_ex1){
            id(zone_ex1_target_exsits).publish_state(has_target_in_zone_ex1);
          }

          if (!id(init_zone_publish)){
            id(init_zone_publish) = true;
          }