

import datetime
from math import cos, pi, sin, sqrt
import os
import time
from time import sleep
import xpc
from cognitiveModel import AircraftLandingModel
import shutil
import csv
from enum import Enum


class messageType(Enum):
    REGULAR = 1
    ERROR = 2

def eulerToQuat(psiInput,thetaInput,phiInput):
    
    psi = float(pi / 360 * psiInput)
    theta = float(pi / 360 * thetaInput)
    phi = float(pi / 360 * phiInput)
    q0 =  cos(psi) * cos(theta) * cos(phi) + sin(psi) * sin(theta) * sin(phi)
    q1 =  cos(psi) * cos(theta) * sin(phi) - sin(psi) * sin(theta) * cos(phi)
    q2 =  cos(psi) * sin(theta) * cos(phi) + sin(psi) * cos(theta) * sin(phi)
    q3 = -cos(psi) * sin(theta) * sin(phi) + sin(psi) * cos(theta) * cos(phi)
    e = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)

    quat = [q0/e,q1/e,q2/e,q3/e]
    return quat

def loadFile():
    filename = "Python3/src/experiments/weather_files/weather.csv"
    with open(filename,"r") as f:
        matrix = list(csv.reader(f,delimiter=','))
        print(matrix)
        return matrix
    

def selectWeather(matrix,experimentNumber):
    return matrix[experimentNumber]

def experimentSetUp(client,currentConditions,newExperiment):
    # input("Check The Loaded File Now")
    print("Entered: EXPERIMENTSETUP")
    if(newExperiment):
        #Location:
        groundLevel = 5434
        offset = float(currentConditions[1])
        altitudeFEET = groundLevel + offset
        altitudeMETERS = altitudeFEET/3.281
        altitude = altitudeMETERS
        print(str(altitude))
        location1 =  [20,   -998, 39.96239,  -104.696032, altitude, -998, -998, -998, -998]
        # testLocation = [20,   -998, 27.20579,  -80.08621, altitude, -998, -998, -998, -998] # 27.20579°N/80.08621°W
        data = [
            location1\
            ]
        client.sendDATA(data)



        print("Zero velocities")
        x = "sim/flightmodel/position/local_vx"
        y = "sim/flightmodel/position/local_vy"
        z = "sim/flightmodel/position/local_vz"
        client.sendDREF(x,0)
        client.sendDREF(y,0)
        client.sendDREF(z,0)


        print("Zero rotation")
        p = "sim/flightmodel/position/P"
        q = "sim/flightmodel/position/Q"
        r = "sim/flightmodel/position/R"
        client.sendDREF(p,0)
        client.sendDREF(q,0)
        client.sendDREF(r,0)

        print("set heading")
        # client.pauseSim(True)
        orient = "sim/flightmodel/position/q"
        # orientCommand = [1,0.0,0.0,0.0]
        pitch = client.getDREF("sim/flightmodel/position/true_theta")
        roll = client.getDREF("sim/flightmodel/position/true_phi")
        
        orientCommand = eulerToQuat(179,0,0) # heading, pitch,Roll
        orientTest = [1.0,1.0,1.0,1.0] # heading, pitch,Roll

        print("ORIENT TO: " + str(orientCommand))
        client.sendDREF(orient,orientCommand)
        orientResult = client.getDREF(orient)
        print(str(orientResult))
        # client.pauseSim(False)

        #Weather:
        windLayer = "sim/weather/wind_altitude_msl_m[0]"
        windLayer2 = "sim/weather/wind_altitude_msl_m[1]"
        windLayer3 = "sim/weather/wind_altitude_msl_m[2]"
        windDirection = "sim/weather/wind_direction_degt[0]"
        windSpeed = "sim/weather/wind_speed_kt[0]"
            # windDirections = [50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0]
            # client.sendDREF(windDirection,windDirections)
            # winds = [50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0]
            # client.sendDREF(windSpeed,winds)
            # result = client.getDREF(windLayer)
            # print("Wind:" + str(result))
        print("Setting Wind Layers")
        client.sendDREF(windLayer,float(currentConditions[2]))
        # client.sendDREF(windLayer2,15000)
        # client.sendDREF(windLayer3,15000)
        print("Setting Wind Direction and Speed")

        client.sendDREF(windDirection,float(currentConditions[3]))
        print("Set 1")
        client.sendDREF(windSpeed,float(currentConditions[4]))
        print("Set 2")


        # client.sendDREF(turbulence,turbulencePercentage)
        # preset = "sim/weather/region/weather_preset"
        # value = 8
        # client.sendDREF(preset,value)
        client.pauseSim(True)
        # input("Press Enter to finish setting up Simulation")
        client.pauseSim(False)
        print("Setting initial velocity")
        zInit = "sim/flightmodel/position/local_vz"
        client.sendDREF(zInit, 50)
        print("setup complete")


        
        """
        Set orientation and Position
            1 - Time 
            3- Speeds: [3,   V-indicated, 3,   2, 2, -998, VindMPH, V trueMPHas, vtrue MPHgs],\
            16 - Angular Velocities
            17 - PitchRoll and Headings: [17,   Pitch, Roll,   Heading True, -998, -998, -998, -998, -998],\   
            18 - Angle of Attack
            20 - Latitude and Longitude
        """
        message = "Conditions are set as\n" \
        "Starting Altitude: {} \n" \
        "Layer Altitude: {} \n" \
        "Wind Direction: {} \n" \
        "Wind Speed: {} \n".format(currentConditions[1],currentConditions[2],currentConditions[3],currentConditions[4])
        specialPrint(message,False,messageType.REGULAR)        
    else:
        print("Experiment currently in progress, not resetting position and environmental conditions")

def runExperiment(title,currentConditions,allowPrinting,isNewExperiment):
    specialPrint("New Experiment\nSetting Up the Simulation",False,messageType.REGULAR)
    startTime = 0
    endTime = 0
    timeElapsed = endTime - startTime
    experimentInProgress = True
    timeoutLimit = 10
    newExperiment =  isNewExperiment

    while(timeElapsed <= timeoutLimit and experimentInProgress):
        # print("Time Elapsed: -----> " + str(timeElapsed))
        try:
            with xpc.XPlaneConnect() as client:
                """ 
                Test Connection
                """
                client.getDREF("sim/test/test_float")
                """
                Setup Model
                """
                cogModel = AircraftLandingModel(client,allowPrinting)
                """
                Set Simulator Conditions
                """
                experimentSetUp(cogModel.client,currentConditions,newExperiment)
                """
                """
                cogModel.client.pauseSim(False)
                newExperiment = False
                
                """
                Single Experiment Loop
                """
                while(experimentInProgress):
                    cogModel.update_aircraft_state() 
                    cogModel.update_controls_simultaneously()
                    client.pauseSim(False)          #Unpause Simulator
                    sleep(0.05)                     # Let Simulator Run 50 Milliseconds
                    startTime = time.time()
                    experimentInProgress = cogModel.getSimulationStatus()
        except:
            endTime = time.time()
            timeElapsed = endTime - startTime
            message = "Except detected\nStart Time: {a}\nEnd Time: {b}\n" \
            "Time Elapsed: -----> {c} ".format(a= startTime, b=endTime,c=timeElapsed)
            specialPrint(message,False,messageType.ERROR)
            continue

    """
    Parse End Condition: Succesful Run or Timeout-Induced End
    """
    if(timeElapsed >= timeoutLimit):
        print("Timeout[" + str(timeElapsed) +"]:"+"Error, please run test again")
    else: 
        print("Model has finished running")


    """
    Ask Experimenter if they would like to exit experiment battery early 
    """
    exitDecision = input("Press 'y' or any key to continue, press 'n' to exit...")
    if(exitDecision == "n"):
        exitExperimentLoop = True
    else:
        exitExperimentLoop = False
    return exitExperimentLoop

def setUp():
    open("/Users/flyingtopher/X-Plane 11/Data.txt", 'w')
    specialPrint("Data Collection File Ready",False,messageType.REGULAR)

def cleanUp(title):
    now = datetime.datetime
    shutil.copy("/Users/flyingtopher/X-Plane 11/Data.txt", "/Users/flyingtopher/Desktop/Test Destination/" + title + "_" + str(now.now()) + "_" + ".txt")
    print("CLEAN UP: Data File Deleted and Reset")

    specialPrint("Data File Ready",False,messageType.REGULAR)

def specialPrint(text, inputRequested,type): 
    if(type == messageType.REGULAR): 
        print("-" * 81)
        print(text)
        print("-" * 81 + "\n")
        if(inputRequested):
            inputReceived = input()
            return inputReceived
    if(type == messageType.ERROR):
        print("*" * 81)
        print(text)
        print("*" * 81 + "\n")


def ex():
    """
    One Time experimental setup
    """
    title = specialPrint("Please Enter Experiment Set Title, leave blank for trial runs\n", True, messageType.REGULAR)
    experimentConditionMatrix = loadFile()
    allowPrinting = True
    isNewExperiment = True
    experimentCount = 0
    """
    Experiment Loop
    """
    while(experimentCount<len(experimentConditionMatrix)):
        setUp()
        currentConditions = experimentConditionMatrix[experimentCount+1]
        exitExperimentLoop = runExperiment(title,currentConditions,allowPrinting,isNewExperiment)
        if(exitExperimentLoop):
            break
        experimentCount+=1
        cleanUp(title)
    """
    End of Experiments
    """
    specialPrint("Experiment Battery Complete", False,messageType.REGULAR)



if __name__ == "__main__":
    ex()