import serial
import time
import math


class Ship:
    # @parameter badudrate 波特率
    def __init__(self):
        self.connect = serial.Serial("/dev/ttyAMA0",baudrate=9600,timeout=0.5)
        
        self.Target_Pitch_Angle = 0
        self.Target_Roll_Angle = 0
        self.Target_Yaw_Angle = 0
        self.Target_Pitch_Palstance = 0
        self.Target_Roll_Palstance = 0
        self.Target_Yaw_Palstance = 0
        self.Target_X = 0
        self.Target_Y = 0
        self.Target_Depth = 0
        self.Target_Height = 0
        self.Target_X_Speed = 0
        self.Target_Y_Speed = 0
        self.Target_Z_Speed = 0
        self.Target_X_Acc = 0
        self.Target_Y_Acc = 0
        self.LED0 = False
        self.LED1 = False
      
    def sendMessage(self,tmp:int , val:float):
        end = 255
        end = end.to_bytes(1,'big')
        id = tmp.to_bytes(1, 'big')
        try:
           self.connect.write(id)
           self.connect.write(str(float()).encode('ascii'))
           self.connect.write(str(' ').encode('ascii'))
           self.connect.write(end)
        except:
            return 
    
    def setSlopeAndInter(self , slope : float , val : float):
        tmp = 0x02
        end = 0xff
        id = tmp.to_bytes(1, 'big')
        mark = end.to_bytes(1, 'big')
        self.connect.write(id)
        self.connect.write(str(slope).encode('ascii'))
        self.connect.write(str(' ').encode('ascii'))
        self.connect.write(str(val).encode('ascii'))
        self.connect.write(str(' ').encode('ascii'))    
        self.connect.write(end)   
 
    def LED0ON(self):
        if self.LED0 == False:
            self.LED0 = True
            tmp = 0x10
            self.sendMessage(tmp , 0.0)
    
    def LED0OFF(self):
        if self.LED0 == True:
            self.LED0 = False
            tmp = 0x0f
            self.sendMessage(tmp , 0.0)
    
    def LED1OFF(self):
        if self.LED1 == True:
            self.LED1 = False
            tmp = 0x11
            self.endMessage(tmp , 0.0)
    
    def LED1ON(self):
        if self.LED1 == False:
            self.LED1 = False
            tmp = 0x12
            self.sendMessage(tmp, 0.0)
    
    def setTarget_Depth(self , val : float):
        tmp = 0x08
        self.Target_Depth = val
        self.sendMessage(tmp, val)
    
    def setTarget_Yaw_Angle(self, val : float):
        tmp = 0x02
        self.Target_Yaw_Angle = val
        self.sendMessage(tmp, val)

    def setTarget_X_Speed(self , val : float):
        tmp = 0x0A
        self.Target_X_Speed = val
        self.sendMessage(tmp, val)
    
    def setTarget_Y_Speed(self , val : float):
        tmp = 0x0B
        self.Target_Y_Speed = val
        self.sendMessage(tmp, val)


    def sendAlpha(self , ch : int):
        tmp = 0x13 + ch
        self.sendMessage(tmp, 0.0)
    
    def setFinal(self):
        tmp = 0x2d
        self.sendMessage(tmp, 0.0)

    def setForward_Thrust(self, val:float):
        tmp = 0x2f
        self.sendMessage(tmp, val)

    def setSidesway_Thrust(self , val: float):
        tmp = 0x30
        self.sendMessage(tmp ,val)

    def setYaw_Rotate_Torque(self , val : float):
        tmp = 0x32
        self.sendMessage(tmp, val)