'''
Created on Sep 3, 2017

@author: DougBrownWin
create a framework to visualize a continous confidence area generated by the UAVs
'''
import Variables as varis
from ConfArea_Matrix import ConfArea_Matrix
from ConfArea_Matrix_v2 import ConfArea_Matrix_v2
from ViewLive import ViewLive
from UavPso import UavPso
from RecordData import RecordData
import numpy as np
from UavPso_add_sub import UavPso_addSub
import math


class ConfArea(object):
    '''
    classdocs
    '''

    def __init__(self):
        '''
        Constructor
        '''
        
        self.confArea = ConfArea_Matrix_v2()
        self.viewLive = ViewLive()
        
    # this controls which uav object to return
    def getUav(self, indx):
        uav = UavPso_addSub(varis.params,indx)
        
        # add uav to params
        varis.params.addUav(uav)
        
        return(uav)   
            
    
    
    # begin simualtion test
    def begin(self):
        
        # create uavs
        uavs = []
        for indx in range(0,varis.params.uav_num):
            uav = self.getUav(indx)
            uavs.insert(indx, uav)
        
        # data/pic/video recorder
        # record position, confArea size, time, 
        # also save pic/video files
        recorder = RecordData(self.confArea.getSearchEdge()) 
        
        
        print("Running confidence area simulation")
        
        # basically just keeptrack of percent to manage printing
        percent_previous = 0

        # loop until radius is at 99 percent
        # have done all add/sub proceedures
        # and proceedures are complete
        while(np.linalg.norm(uavs[0].getPos())/np.linalg.norm(varis.params.get_radiusMax(varis.uav_num)) < varis.pso_radius_fraction or 
              len(varis.uavChangeArray) > 0 or 
              uavs[0].getPsoStatus() != varis.PSO_NORMAL):
            
            percent = np.floor(100 * np.linalg.norm(uavs[0].getPos())/np.linalg.norm(varis.params.get_radiusMax(varis.uav_num)))
            if(percent != percent_previous):
                percent_previous = percent
                print('%% %.0f' % (percent))
            
            # update positions for uavs
            for indx in range(0,varis.params.uav_num):
                uav = uavs[indx]
                uav.moveStep()

                
            # update confidence area
            # update map based on UAV position
            # if UaV covers 50% or more, make 1
            # initialy all zeros
            # update other squares when target could have traveld 1 full square lengthwise
            self.confArea.update_area(uavs)
             
            # update time
            varis.params.timeStep()
            
            # realTime view of stuff
            if (varis.view_liveBool == True):
                self.viewLive.plotLive(self.confArea, uavs)
            
             
            # record stuff
            recorder.rec_confAreaSize(self.confArea)
            recorder.rec_anime(self.confArea)
            
            # check if need to jump into add/subtract procedure
            if(np.linalg.norm(uavs[0].getPos())/varis.params.get_radiusMax(varis.uav_num) > varis.pso_radius_fraction and
               len(varis.uavChangeArray) > 0 and uavs[0].getPsoStatus() == varis.PSO_NORMAL):

                status = varis.uavChangeArray.pop(0)
                
                # if adding a uav
                if(status == varis.OP_ADD):
                    print("---", varis.params.current_time, "s : Adding UAV---")
                    
                    # add uav to end of uavs
                    # place between first and last
                    uav = self.getUav(varis.uav_num)                    
                    
                    # calc radius and position of uav
                    uav.radius = np.linalg.norm(uavs[0].getPos())
                    uav.angle = 2*math.pi + uavs[0].angle - (math.pi)/varis.uav_num
                    uav.updatePos()
                    
                    # append uavs
                    uavs.append(uav)
                    
                    
                    # increase UAV number
                    varis.uav_num += 1
                    varis.params.uav_num += 1
                    
                    # mark time when uav as added
                    recorder.uavAdded()
                    
                    # recalcualte pso path
                    varis.params.calc_psoPath()
                    
                    
                    # reference uav
                    uavs[0].setUavStatus(varis.UAV_REFERENCE)
                    
                    # make all other uavs reorient
                    for indx in range(1,varis.uav_num):
                        uavs[indx].setUavStatus(varis.UAV_REORIENT)
                    
                    # pso status is to reorient
                    uavs[0].setPsoStatus(varis.PSO_PRE_REORIENT)
                    uavs[0].startPreReorient()                 
                    
                
                # else subtracting a uav
                else: 
                    print("---", varis.params.current_time, "s : Removing UAV---")
                    # delete uav right in front of reference uav
                    # update uav NUM
                    uavs.pop(1)
                    varis.params.delUav(1)
                    varis.params.uav_num -= 1
                    varis.uav_num -= 1
                    
                    # update recorder for when uavs was subtracted
                    recorder.uavSub()
                    
                    # define uav 0 as reference
                    # reference uav
                    uavs[0].setUavStatus(varis.UAV_REFERENCE)
                    
                    # make all other uavs reorient
                    for indx in range(1,varis.uav_num):
                        # update indices
                        uavs[indx].indx = indx
                        uavs[indx].setUavStatus(varis.UAV_REORIENT)
                        
                    
                    # make pso path as subtracting
                    uavs[0].setPsoStatus(varis.PSO_REDUCE_RADIUS)
                    
        print(varis.params.current_time, "s : Final finishing time")
        
        # print and save simulation data...
        print(recorder.toString())
        recorder.save_txt()
        recorder.save_graphs(self.confArea)
        recorder.saveAnime()



# RUN Z PROGRAM HANZ!!
if __name__ == '__main__':
    pass

    # initialize and run simulation
    confArea = ConfArea()
    confArea.begin()
    