import component
import control
import numpy
from math import radians, atan, tan, sin, cos, acos

class PosToDistance(control.NumberWritable):
    def __init__(self):
        super().__init__()
        self.lonA = self.new_property('s', 0)
        self.latA = self.new_property('s', 0)
        self.lonB = self.new_property('s', 0)
        self.latB = self.new_property('s', 0)

    def changed(self, property):
        if property == self.lonA or property == self.lonB or property == self.latA or self.latB:
            self.set_out( self.getdis(self.latA.value, self.lonA.value, self.latB.value, self.lonB.value) )

    def getdis(self, latA,lonA,latB,lonB):
        ra = 6378140#赤道半径,m
        rb = 6356755#地极半价,m
        flatten = (ra - rb)/ ra
        radlatA = radians(latA)
        radlonA = radians(lonA)
        radlatB = radians(latB)
        radlonB = radians(lonB)
        pA=atan(rb/ra*tan(radlatA))
        pB=atan(rb/ra*tan(radlatB))
        x=acos(sin(pA)*sin(pB)+cos(pA)*cos(pB)*cos(radlonA-radlonB))
        c1=(sin(x)-x)*(sin(pA)+sin(pB))**2/cos(x/2)**2
        c2=(sin(x) +x)* (sin(pA) - sin(pB)) ** 2 / sin(x/2) ** 2
        dr=flatten/8*(c1-c2)
        dis=ra*(x+dr)
        print(dis)
        return dis

import component
import control
import numpy
from math import radians, atan, tan, sin, cos, acos

class VolumetoDepth(control.NumberWritable):
    def __init__(self):
        super().__init__()
        self.Volume = self.new_property('s', 0)
        self.BalanceVol = self.new_property('s', 850)

    def changed(self, property):
        if property == self.Volume or property == self.BalanceVol:
            self.set_out( self.getVolumetoDepth(self.Volume.value, self.BalanceVol.value) )


<<<<<<< HEAD
class HeadControl(control.NumberWritable):
    def __init__(self):
        super().__init__()
        self.current_angle = self.new_property('s', 0)
        self.rudder_angle = self.new_property('s', 0)
        self.time = self.new_property('s', 0)
    def changed(self, property):
        if  property == self.rudder_angle or property == self.time:
            if self.rudder_angle.value == 60:
                self.set_value(self.current_angle, (self.current_angle.value + self.time.value*3.545))
            elif self.rudder_angle.value == 120:
                self.set_value(self.current_angle, (self.current_angle.value - self.time.value*3.545))
     		
=======
    def getVolumetoDepth(self, Volume, BalanceVol):
        x=Volume-BalanceVol  #求体积变化量
        k1=0.5/(BalanceVol-100)  #负浮力，下降
        k2=0.5/(1400-BalanceVol)  #正浮力，上升
        if  Volume<BalanceVol:
            Depth=k1*x
        elif  Volume>BalanceVol:
            Depth=k2*x
        elif  Volume == BalanceVol:
            Depth=0
        return -Depth

import component
import control
import numpy

class PitchControl(component.Component):
    def __init__(self):
        super().__init__()
        self.shift = self.new_property('s', 0)
        self.angle = self.new_property('s', 0)
        self.balanceShift = self.new_property('s', 65)

    def __calculate_angle(self, shift, balance_shift):
        if (shift - balance_shift) * 1.139 > 70:
            self.set_value(self.angle, 70)
        elif (shift - balance_shift) * 1.139 < -70:
            self.set_value(self.angle, -70)
        else:
            self.set_value(self.angle, (shift - balance_shift) * 1.139)
    def changed(self, property):
        if property is self.shift or property is self.balanceShift:
            self.__calculate_angle(self.shift.value, self.balanceShift.value)



import component
import control
import numpy
import threading


class TimeTrigger(component.Component):
    def __init__(self):
        super().__init__()
        self.trigger = self.new_action('s', None)
        self.enabled = self.new_action('s', None)
        self.disabled = self.new_action('s', None)
        self.period = self.new_property('s', 1)
        self.flag = self.new_property('',0)

    def do_enabled(self, v):
        self.triggerAction()
        self.flag = True

    def do_disabled(self,v):
        self.flag = False

    def triggerAction(self):
        self.post(self.trigger, None)
        if self.flag == True:
            threading.Timer(self.period, self.triggerAction).start()

    def changed(self, property):
        if property is self.period:
            self.triggerAction()
>>>>>>> 417513c8f9dca52411eb81f5a3bf47db0ae11c2c