# -*- coding: utf-8 -*-

'''
Created on 03/02/2016

@author: david
'''
from math import radians
from sensors.vector import Vector


class Propeller(object):
    '''
    Represents the drone's propeller
    '''
    
    THROTTLE_THRESHOLD = 10.0 #Below this threshold the motor cannot move the propeller
    LPF = 0.9 #Low-pass filter threshold
    
    GRAVITY = 9.807 #m/s²
    
    ROTATION_CW = 0
    ROTATION_CCW = 1

    def __init__(self, drone, rotationThrustRate, throttleRotationRate, thrustedWeight, rotation, counterRotationRate):
        '''
        Constructor
        
        @param drone: Drone object to notify changes  
        @param rotationThrustRate: Rate of thrust increment per angular-speed. 
            For example: let a propeller of max. 4kg. This means that 100% angular-speed is 4kg thrust, 
            then the rate is 0.04
        @param throttleRotationRate: Rate of rotation per throttle. Simulates the power down due to the presence
            of the other propellers. Should be near to 1.0
        @param thrustedWeight: Weight that this propeller carries
        @param rotation: Rotation mode (clockwise/counter clockwise)
        @param counterRotationRate: Reaction rotation to the current rotation
        '''

        self._throttleThrustRate = rotationThrustRate * throttleRotationRate
        self._thrustedWeight = thrustedWeight
        
        #IMU's positive angles are CCW
        if rotation == Propeller.ROTATION_CW:
            self._counterRotationRate = counterRotationRate
        else:
            self._counterRotationRate = -counterRotationRate

        self._throttle = 0.0
        self._thrustModule = 0.0
        self._thrust = [0.0]*3
        
        self._angles = [0.0] * 3
        
        self._drone = drone
        
        self._lowPassFilter = Propeller.LPF if self._drone._realisticFlight else 1.0
    
    
    def getWeight(self):
        
        return self._thrustedWeight
    
    
    def setThrottle(self, throttle):
        '''
        Set the throttle
        
        @param throttle: Percentage of the throttle        
        '''
        
        self._throttle = throttle
        self._update()

        
    def setAngles(self, angles):
        '''
        Set the angles

        @param angles: Angles as degrees of the propeller
        '''

        self._angles = [radians(angle) for angle in angles]

    
    def getThrust(self):
        
        return self._thrust
    
    
    def getTorque(self):
        
        return self._counterRotation
    
    
    def getThrottle(self):
        
        return self._throttle


    def getOrtogonalAccel(self):
        
        return self._throttle * self._throttleThrustRate * Propeller.GRAVITY / self._thrustedWeight
    
    
    def _update(self):

        effectiveThrottle = self._throttle - Propeller.THROTTLE_THRESHOLD \
            if self._throttle > Propeller.THROTTLE_THRESHOLD else 0.0        

        newThrustModule = effectiveThrottle * self._throttleThrustRate * Propeller.GRAVITY
        self._thrustModule += self._lowPassFilter * (newThrustModule - self._thrustModule) 
        self._thrust = Vector.rotateVector3D([0.0, 0.0, self._thrustModule], self._angles)
        self._thrust[2] -= Propeller.GRAVITY * self._thrustedWeight

        #Counter-rotation        
        self._counterRotation = self._thrustModule * self._counterRotationRate
    
        self._drone.onPropellerUpdated()

# -*- coding: utf-8 -*-

'''
Created on 03/02/2016

@author: david
'''
from math import radians
from sensors.vector import Vector


class Propeller(object):
    '''
    Represents the drone's propeller
    '''
    
    THROTTLE_THRESHOLD = 10.0 #Below this threshold the motor cannot move the propeller
    LPF = 0.9 #Low-pass filter threshold
    
    GRAVITY = 9.807 #m/s²
    
    ROTATION_CW = 0
    ROTATION_CCW = 1

    def __init__(self, drone, rotationThrustRate, throttleRotationRate, thrustedWeight, rotation, counterRotationRate):
        '''
        Constructor
        
        @param drone: Drone object to notify changes  
        @param rotationThrustRate: Rate of thrust increment per angular-speed. 
            For example: let a propeller of max. 4kg. This means that 100% angular-speed is 4kg thrust, 
            then the rate is 0.04
        @param throttleRotationRate: Rate of rotation per throttle. Simulates the power down due to the presence
            of the other propellers. Should be near to 1.0
        @param thrustedWeight: Weight that this propeller carries
        @param rotation: Rotation mode (clockwise/counter clockwise)
        @param counterRotationRate: Reaction rotation to the current rotation
        '''

        self._throttleThrustRate = rotationThrustRate * throttleRotationRate
        self._thrustedWeight = thrustedWeight
        
        #IMU's positive angles are CCW
        if rotation == Propeller.ROTATION_CW:
            self._counterRotationRate = counterRotationRate
        else:
            self._counterRotationRate = -counterRotationRate

        self._throttle = 0.0
        self._thrustModule = 0.0
        self._thrust = [0.0]*3
        
        self._angles = [0.0] * 3
        
        self._drone = drone
        
        self._lowPassFilter = Propeller.LPF if self._drone._realisticFlight else 1.0
    
    
    def getWeight(self):
        
        return self._thrustedWeight
    
    
    def setThrottle(self, throttle):
        '''
        Set the throttle
        
        @param throttle: Percentage of the throttle        
        '''
        
        self._throttle = throttle
        self._update()

        
    def setAngles(self, angles):
        '''
        Set the angles

        @param angles: Angles as degrees of the propeller
        '''

        self._angles = [radians(angle) for angle in angles]

    
    def getThrust(self):
        
        return self._thrust
    
    
    def getTorque(self):
        
        return self._counterRotation
    
    
    def getThrottle(self):
        
        return self._throttle


    def getOrtogonalAccel(self):
        
        return self._throttle * self._throttleThrustRate * Propeller.GRAVITY / self._thrustedWeight
    
    
    def _update(self):

        effectiveThrottle = self._throttle - Propeller.THROTTLE_THRESHOLD \
            if self._throttle > Propeller.THROTTLE_THRESHOLD else 0.0        

        newThrustModule = effectiveThrottle * self._throttleThrustRate * Propeller.GRAVITY
        self._thrustModule += self._lowPassFilter * (newThrustModule - self._thrustModule) 
        self._thrust = Vector.rotateVector3D([0.0, 0.0, self._thrustModule], self._angles)
        self._thrust[2] -= Propeller.GRAVITY * self._thrustedWeight

        #Counter-rotation        
        self._counterRotation = self._thrustModule * self._counterRotationRate
    
        self._drone.onPropellerUpdated()

# -*- coding: utf-8 -*- Created on 03/02/2016  @author: david Represents the drone's propeller #Below this threshold the motor cannot move the propeller #Low-pass filter threshold #m/s² Constructor                  @param drone: Drone object to notify changes           @param rotationThrustRate: Rate of thrust increment per angular-speed.              For example: let a propeller of max. 4kg. This means that 100% angular-speed is 4kg thrust,              then the rate is 0.04         @param throttleRotationRate: Rate of rotation per throttle. Simulates the power down due to the presence             of the other propellers. Should be near to 1.0         @param thrustedWeight: Weight that this propeller carries         @param rotation: Rotation mode (clockwise/counter clockwise)         @param counterRotationRate: Reaction rotation to the current rotation #IMU's positive angles are CCW Set the throttle                  @param throttle: Percentage of the throttle Set the angles          @param angles: Angles as degrees of the propeller #Counter-rotation