'''
Author: WZX 17839623189@168.com
Date: 2023-12-03 17:23:03
LastEditors: WZX 17839623189@168.com
LastEditTime: 2023-12-03 17:35:15
FilePath: /wzx/zkmetaapi/ZKMetaUnit/utils/gyro.py
Description: 

Copyright (c) 2023 by LLM, All Rights Reserved. 
'''
class Gyro:  
    def __init__(self, yaw_start=0):  
        self.yaw_start = yaw_start  
        self.yaw_current = yaw_start  
        self.left_turn_angle = 0  
        self.right_turn_angle = 0  
  
    def update(self, yaw):  
        # 计算当前角度与起始角度的差值  
        # 左转
        if self.yaw_current > 170 and yaw < 0:
            delta_yaw = yaw+360 - self.yaw_current
            self.left_turn_angle += delta_yaw  
        # 右转 
        elif self.yaw_current < -170 and yaw >0:
            delta_yaw = 360 - (yaw - self.yaw_current)
            self.right_turn_angle += delta_yaw  
        # 左转
        elif self.yaw_current < yaw:
            delta_yaw = yaw - self.yaw_current
            self.left_turn_angle += delta_yaw  
        # 右转
        elif self.yaw_current >= yaw:
            delta_yaw =  - (yaw - self.yaw_current)
            self.right_turn_angle += delta_yaw  

        # 更新当前角度为新的角度  
        self.yaw_current = yaw  
  
    def get_left_turn_angle(self):  
        return self.left_turn_angle
  
    def get_right_turn_angle(self):  
        return self.right_turn_angle
    
    def get_corr_left_turn_angle(self):  
        return self.left_turn_angle - self.right_turn_angle
  
    def get_corr_right_turn_angle(self):  
        return self.right_turn_angle - self.left_turn_angle