1/*
2 * Copyright (c) 2022-2023 Huawei Device Co., Ltd.
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 *     http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15
16#ifdef DEVICE_STATUS_SENSOR_ENABLE
17#include "algo_vertical.h"
18
19#include "devicestatus_define.h"
20
21#undef LOG_TAG
22#define LOG_TAG "AlgoVertical"
23
24namespace OHOS {
25namespace Msdp {
26namespace DeviceStatus {
27namespace {
28constexpr float JUDGE_FLOAT { 1e-6 };
29} // namespace
30
31bool AlgoVertical::Init(Type type)
32{
33    CALL_DEBUG_ENTER;
34    algoCallback_ = [this](int32_t sensorTypeId, AccelData* sensorData) {
35        return this->StartAlgorithm(sensorTypeId, sensorData);
36    };
37    CHKPF(algoCallback_);
38    SENSOR_DATA_CB.SubscribeSensorEvent(type, algoCallback_);
39    return true;
40}
41
42bool AlgoVertical::StartAlgorithm(int32_t sensorTypeId, AccelData* sensorData)
43{
44    CALL_DEBUG_ENTER;
45    if (!SetData(sensorTypeId, sensorData)) {
46        FI_HILOGE("Failed to get data");
47        return false;
48    }
49    ExecuteOperation();
50    return true;
51}
52
53void AlgoVertical::ExecuteOperation()
54{
55    CALL_DEBUG_ENTER;
56    if ((abs(algoPara_.y) <= JUDGE_FLOAT) && (abs(algoPara_.z) <= JUDGE_FLOAT)) {
57        return;
58    }
59    algoPara_.pitch = -atan2(algoPara_.y, algoPara_.z) * (ANGLE_180_DEGREE / PI);
60    algoPara_.roll = atan2(algoPara_.x, algoPara_.z) * (ANGLE_180_DEGREE / PI);
61    FI_HILOGD("pitch:%{public}f, roll:%{public}f", algoPara_.pitch, algoPara_.roll);
62
63    if (((abs(algoPara_.pitch) > ANGLE_VER_LOW_THRHD) && (abs(algoPara_.pitch) < ANGLE_VER_UP_THRHD)) ||
64        ((abs(algoPara_.roll) > ANGLE_VER_LOW_THRHD) && (abs(algoPara_.roll) < ANGLE_VER_UP_THRHD))) {
65        if (state_ == VERTICAL) {
66            return;
67        }
68        counter_--;
69        if (counter_ == 0) {
70            counter_ = COUNTER_THRESHOLD;
71            UpdateStateAndReport(VALUE_ENTER, VERTICAL, TYPE_VERTICAL_POSITION);
72        }
73    } else {
74        counter_ = COUNTER_THRESHOLD;
75        if (state_ == NON_VERTICAL) {
76            return;
77        }
78        UpdateStateAndReport(VALUE_EXIT, NON_VERTICAL, TYPE_VERTICAL_POSITION);
79    }
80}
81} // namespace DeviceStatus
82} // namespace Msdp
83} // namespace OHOS
84#endif // DEVICE_STATUS_SENSOR_ENABLE