from UnitCommands import *
from Landing import *
import math

VALID_ROE = 100


def GetMessageParam(BB, messageName):
    msg = BB.ReadMessage(messageName)
    if (msg == ''):
        return 0
    else:
        return float(msg)

def RadarOn(TI):
    UI = TI.GetPlatformInterface()
    BB = TI.GetBlackboardInterface()

    n_sensors = UI.GetSensorCount()
    for n in range(0, n_sensors):
        sens_info = UI.GetSensorInfo(n)
        # 1 radar, 2 ESM, 4 passive sonar, 8 active sonar, 16 optical, 0 error
        if (sens_info.type == 1): 
            UI.SetSensorState(n, 1)
    TI.EndTask()

# intermittent so that radar is on about 25% of the time within group, not synchronized
# across group so this isn't ideal
def RadarRandom(TI):
    UI = TI.GetPlatformInterface()
    TI.SetUpdateInterval(90.0)
    
    nGroup = 1
    mission = UI.GetMissionInfo()
    if (mission.IsValid()):
        nGroup = mission.GetMissionAircraftCount()
        
    p_on = 0.25 / float(nGroup)
    
    state = UI.Rand() < p_on
    
    n_sensors = UI.GetSensorCount()
    for n in range(0, n_sensors):
        sens_info = UI.GetSensorInfo(n)
        # 1 radar, 2 ESM, 4 passive sonar, 8 active sonar, 16 optical, 0 error
        if (sens_info.type == 1): 
            UI.SetSensorState(n, state)
    
def RadarOff(TI):
    UI = TI.GetPlatformInterface()
    BB = TI.GetBlackboardInterface()

    n_sensors = UI.GetSensorCount()
    for n in range(0, n_sensors):
        sens_info = UI.GetSensorInfo(n)
        # 1 radar, 2 ESM, 4 passive sonar, 8 active sonar, 16 optical, 0 error
        if (sens_info.type == 1): 
            UI.SetSensorState(n, 0)
    TI.EndTask()   
    
def ActiveSonarOn(TI):
    UI = TI.GetPlatformInterface()
    BB = TI.GetBlackboardInterface()

    n_sensors = UI.GetSensorCount()
    for n in range(0, n_sensors):
        sens_info = UI.GetSensorInfo(n)
        # 1 radar, 2 ESM, 4 passive sonar, 8 active sonar, 16 optical, 0 error
        if (sens_info.type == 8): 
            UI.SetSensorState(n, 1)
    TI.EndTask()         
                
def ActiveSonarOff(TI):
    UI = TI.GetPlatformInterface()
    BB = TI.GetBlackboardInterface()

    n_sensors = UI.GetSensorCount()
    for n in range(0, n_sensors):
        sens_info = UI.GetSensorInfo(n)
        # 1 radar, 2 ESM, 4 passive sonar, 8 active sonar, 16 optical, 0 error
        if (sens_info.type == 8): 
            UI.SetSensorState(n, 0)
    TI.EndTask()   

def WaitForGroup(TI):
    BB = TI.GetBlackboardInterface()
    if (not GetConnControl(BB)):
        TI.EndTask() # cant get controls so just end
        return
        
    TI.SetUpdateInterval(30.0)
    UI = TI.GetPlatformInterface()
    alt_m = UI.GetAltitude()
    cruise_speed = UI.GetCruiseSpeedForAltitude(alt_m)
    UI.SetSpeed(0.8*cruise_speed)
        
    UI.SetHeading(UI.GetHeading() + 90)

    
    iteration = TI.GetMemoryValue(1)
    iteration = iteration + 1
    TI.SetMemoryValue(1, iteration)
    
    mission = UI.GetMissionInfo()
    if (mission.IsValid()):
        lagging_waypoint = mission.GetSmallestWaypointIndex()
        my_waypoint = UI.GetCurrentWaypoint()
        if ((lagging_waypoint == my_waypoint) or (iteration > 100)): # dont wait more than about 50 min
            UI.SetSpeed(cruise_speed)
            TI.EndTask()
    else:
        TI.EndTask()  
 
# randomly move throughout area
def AirPatrolArea(TI):
    BB = TI.GetBlackboardInterface()
    if (not GetConnControl(BB)):
        #UI.DisplayMessage('High priority task has control')
        TI.EndTask() # cant get controls so just end
        return
        
    UI = TI.GetPlatformInterface()
    if (not UI.IsAir()):
        #UI.DisplayMessage('Not an aircraft')
        TI.EndTask()
        return
    
    state = TI.GetMemoryValue(2) # 0 - uninit, 1 - moving to new waypoint
    if (state == 0):
        TI.SetMemoryValue(2, 1)
        patrol_point = UI.GetRandomPatrolPoint()
        if ((patrol_point.lon == 0) and (patrol_point.lat == 0)):
            UI.DisplayMessage('No patrol area')
            TI.EndTask() # probably dont have a patrol area defined
            return
        TI.SetMemoryValue(11, patrol_point.lon)
        TI.SetMemoryValue(12, patrol_point.lat)
        
    lon_rad = TI.GetMemoryValue(11)
    lat_rad = TI.GetMemoryValue(12)

    range_km = UI.GetRangeToDatum(lon_rad, lat_rad)
    
    if (range_km < 2): # set new patrol point
        patrol_point = UI.GetRandomPatrolPoint()
        TI.SetMemoryValue(11, patrol_point.lon)
        TI.SetMemoryValue(12, patrol_point.lat)
        lon_rad = patrol_point.lon
        lat_rad = patrol_point.lat
    elif (range_km < 25):
        TI.SetUpdateInterval(15.0)
    else:
        TI.SetUpdateInterval(45.0)
    
        
    hdg_deg = UI.GetHeadingToDatum(lon_rad, lat_rad)  
    UI.SetHeading(hdg_deg)
    
# For ASW helicopter, randomly move throughout area, stopping to drop sonobuoys and use dipping sonar
def ASWPatrolArea(TI):
    BB = TI.GetBlackboardInterface()
    if (not GetConnControl(BB)):
        TI.EndTask() # cant get controls so just end
        return
        
    UI = TI.GetPlatformInterface()
 
    state = TI.GetMemoryValue(2) # 0 - uninit, 1 - moving to new waypoint, 2 - coming to hover, 3 - drop buoy, 4 - attack target, listen
    if (state == 0):
        if (not UI.IsHelo()):
            TI.EndTask()
            return
        UI.SetAllSensorState(1)
        TI.SetMemoryValue(2, 1)
        patrol_point = UI.GetRandomPatrolPoint()
        if ((patrol_point.lon == 0) and (patrol_point.lat == 0)):
            TI.EndTask() # probably dont have a patrol area defined
            return
        TI.SetMemoryValue(11, patrol_point.lon)
        TI.SetMemoryValue(12, patrol_point.lat)
        TI.SetMemoryValue(50, HasSonar(UI))
        state = 1
    
    # check for bingo fuel and cancel task if so, 18DEC2010 commented out, should use RTB task to do this
    # t = UI.GetTime()
    # last_bingo_check = TI.GetMemoryValue(20)
    # if ((t - last_bingo_check) > 60):
        # TI.SetMemoryValue(20, t)
        # if (CheckBingo(UI, BB)):
            # cruise_alt = UI.GetCruiseAltitude()
            # UI.SetAltitude(cruise_alt)
            # UI.SetSpeed(UI.GetCruiseSpeedForAltitude(cruise_alt))
            # ReleaseConnControl(BB)
            # TI.EndTask()
            # return
    has_sonar = TI.GetMemoryValue(50)
    my_track = UI.GetTrackById(UI.GetPlatformId())
    buoy_spacing_km = 6.0
    
    if (state == 1):
        # check for a sub target to attack
        best_target, best_launcher = GetSubTarget(UI) 
        if (best_target != -1):
            tgt_track = UI.GetTargetTrackInfo()
            TI.SetMemoryValue(11, tgt_track.Lon)
            TI.SetMemoryValue(12, tgt_track.Lat)
            #UI.DisplayMessage('sub %d, %f,%f' % (tgt_track.ID, tgt_track.Lon, tgt_track.Lat))

        lon_rad = TI.GetMemoryValue(11)
        lat_rad = TI.GetMemoryValue(12)
        range_km = UI.GetRangeToDatum(lon_rad, lat_rad)
        if (range_km < 0.3): # reached waypoint
            # pick next search waypoint
            patrol_point = UI.GetRandomPatrolPoint()
            TI.SetMemoryValue(11, patrol_point.lon)
            TI.SetMemoryValue(12, patrol_point.lat)
            TI.SetMemoryValue(2, 2)
        elif (range_km < 3):
            TI.SetUpdateInterval(15.0)
            UI.SetSpeed(0.5*UI.GetMaxSpeed())
        else:
            TI.SetUpdateInterval(45.0)
            UI.SetSpeed(0.8*UI.GetMaxSpeed())
            
        hdg_deg = UI.GetHeadingToDatum(lon_rad, lat_rad)  
        UI.SetHeading(hdg_deg)