'''
Created on Jul 4, 2017

@author: jamie
'''

from enum import Enum
import numpy as np
from pyPassiveRanging.sim.missile.guidance import Guidance
from pyPassiveRanging.managers.passiveRangingManager import PassiveRangingManager
from pyPassiveRanging.extraMath.extraLinearAlgebra import dcmProjectParallel
from pyPassiveRanging.dynamicsModels.propModel import initTruthState
from scipy import random
from scipy import linalg
import sys

class GuidSetup(Enum):
    InitialLoft = 1
    ProNav = 2
    ConstantAccel = 3
    ThurstProfile = 4
    
class ExitConditions(object):
    def __init__(self, minTof=0.5, rdotThresh=3., maxTof=30., minRange=5.):
        self.minTof     = minTof         # seconds
        self.rdotThresh = rdotThresh     # m/s
        self.maxTof     = maxTof         # seconds
        self.minRange   = minRange       # meters

class SimLoop(object):
    
    def __init__(self, rM_I_0, vM_I_0, rT_I_0, vT_I_0,
                 randomSeed, losError1Sigma, ukfAlpha, ukfBeta, ukfKappa,
                 meLearningRate, meExpDecayFactor, meMinLearnRate,
                 numFiltersInBank, minRange, maxRange, 
                 guidSetup=GuidSetup.ConstantAccel,
                 exitConditions=ExitConditions()):
        self.rM_I = rM_I_0
        self.vM_I = vM_I_0
        self.rT_I = rT_I_0
        self.vT_I = vT_I_0
        self.randomSeed = randomSeed
        self.losError1Sigma = losError1Sigma
        self.ukfAlpha = ukfAlpha
        self.ukfBeta = ukfBeta
        self.ukfKappa = ukfKappa
        self.meLearningRate = meLearningRate
        self.meExpDecayFactor = meExpDecayFactor
        self.meMinLearnRate = meMinLearnRate
        self.numFiltersInBank = numFiltersInBank
        self.minRange = minRange
        self.maxRange = maxRange
        self.simTime = 0.
        self.prm = None
        self.passiveRangingTm = None
        self.truthLos_I_u = None
        self.guidSetup = guidSetup
        self.exitConditions = exitConditions
                
        self.isExitCondition = False
        
        # TODO: Add IMU, Gimbal, and Navigation models along with relevant error
        #       models
        
        random.seed(self.randomSeed)
        # for now just guide on truth
        self.guidance = Guidance(self.rM_I, self.vM_I, self.rT_I, self.vT_I)
        self.computeMeasurement()
        
        self.prm = PassiveRangingManager(self.los_I, self.vM_I, self.losError1Sigma,
                                         self.ukfAlpha, self.ukfBeta, self.ukfKappa,
                                         self.simTime, self.meLearningRate,
                                         self.meExpDecayFactor, self.meMinLearnRate,
                                         self.numFiltersInBank, self.minRange,
                                         self.maxRange)
        self.dt = self.guidance.dt
        self.prevSimTime = self.simTime
        
    def run(self):
        self.isExitCondition = False
        
        while not self.isExitCondition:
            try:
                # prop the missile and target
                self.simTime = self.prevSimTime+self.dt
                self.propTarget()
                mslState = self.guidance.run(False, self.rT_I, self.vT_I)
                self.rM_I = mslState[0:3]
                self.vM_I = mslState[3:6]
                self.computeMeasurement()
                self.prm.run(self.vM_I, self.simTime, self.los_I,
                             self.truthState, self.truthCartState)
                
                self.prevSimTime = self.simTime
                self.isExitCondition = self.determineExitCondition()
            except:
                print(sys.exc_info()[1])
                
        self.prm.finalize()
        self.passiveRangingTm = self.prm.passiveRangingTm
    
    def propTarget(self):
        # non-maneuvering target for now
        self.rT_I += self.vT_I * self.dt
            
    def computeMeasurement(self):
        self.relPos = self.rT_I - self.rM_I
        self.relVel = self.vT_I - self.vM_I 
        self.tgtTruthRange = linalg.norm(self.relPos)
        self.truthLos_I_u = self.relPos / self.tgtTruthRange
        self.los_I = self.truthLos_I_u \
                + random.normal(0., self.losError1Sigma, (3,))
        self.dcmParallelLosI = dcmProjectParallel(self.truthLos_I_u)
        self.truthRDot = np.dot(self.relVel, self.truthLos_I_u)
        self.truthState = initTruthState(self.relPos, self.relVel)
        self.truthCartState = np.hstack((self.rM_I, self.vM_I, self.rT_I, self.vT_I))
    
    def determineExitCondition(self):
        minTof = self.exitConditions.minTof
        rdotThresh = self.exitConditions.rdotThresh
        maxTof = self.exitConditions.maxTof
        minRange = self.exitConditions.minRange
        
        if self.tgtTruthRange < minRange:
            return True
        
        if self.simTime <= minTof:
            return False
        
        if self.truthRDot > rdotThresh:
            return True
        
        if self.simTime > maxTof:
            return True
        
        return False
            
    