import pyactr
# from XPlaneConnect import *
import xpc
import math
# import geopy
from geographiclib.geodesic import Geodesic as geo
from rich import print



# Initialize XPlaneConnect client
class scaleFactor():
    SCALEYOKEPULL = 10
    SCALEYOKESTEER = 10
    SCALERUDDER = 10
    SCALELATITUDERUDDER = 0.001
    SCALETHROTTLE = 1000

###Define variables/parameters for aircraft class/category : Wisdom of Raju 
class AircraftLandingModel(pyactr.ACTRModel):
    def __init__(self,client,printFlag):
        super().__init__()
        self.client = client
        self.inProgress = True
        self.printControlsFlag = printFlag
        self.targetLat = 39.895791
        self.targetLong = -104.696032
        """
        Setting DREF variables and loading into drefs array
        """
        airspeedDREF = "sim/cockpit2/gauges/indicators/airspeed_kts_pilot"
        rollDREF = "sim/cockpit2/gauges/indicators/roll_AHARS_deg_pilot"
        magneticHeadingDREF = "sim/cockpit2/gauges/indicators/heading_AHARS_deg_mag_pilot"
        latitudeDREF = "sim/flightmodel/position/latitude" ## Lat 
        longitudeDREF = "sim/flightmodel/position/longitude" ##Long
        verticalSpeedDREF = "sim/flightmodel/position/vh_ind_fpm"
        altitudeAGLDREF = "sim/flightmodel/position/y_agl"
        pitchDREF = "sim/flightmodel/position/true_theta"
        brakeDREF = "sim/cockpit2/controls/parking_brake_ratio"
        wheelSpeedDREF = "sim/flightmodel2/gear/tire_rotation_speed_rad_sec"
        wheelWeightDREF = "sim/flightmodel/parts/tire_vrt_def_veh"
        # self.sources = [airspeedDREF,rollDREF,magneticHeadingDREF,verticalSpeedDREF,altitudeAGLDREF,pitchDREF,brakeDREF,wheelSpeedDREF,wheelWeightDREF]

        self.sources = {
            "airspeed" : airspeedDREF,
            "roll" : rollDREF,
            "heading" : magneticHeadingDREF,
            "latitude": latitudeDREF,
            "longitude": longitudeDREF,
            "vertical speed" : verticalSpeedDREF,
            "altitude": altitudeAGLDREF,
            "pitch" : pitchDREF,
            "brakes": brakeDREF,
            "wheelSpeed": wheelSpeedDREF,
            "wheelWeight": wheelWeightDREF
        }

        """
        Initial Initialization of destination Variables and loading into destinations array
        """
        airspeed = self.client.getDREF("sim/cockpit2/gauges/indicators/airspeed_kts_pilot")
        roll = self.client.getDREF("sim/cockpit2/gauges/indicators/roll_AHARS_deg_pilot")
        heading = self.client.getDREF("sim/cockpit2/gauges/indicators/heading_AHARS_deg_mag_pilot")
        latitude = self.client.getDREF("sim/flightmodel/position/latitude") ##Current Lat 
        longitude = self.client.getDREF("sim/flightmodel/position/longitude") ##Current Long
        descent_rate = self.client.getDREF("sim/flightmodel/position/vh_ind_fpm")
        altitude = self.client.getDREF("sim/flightmodel/position/y_agl")
        pitch = self.client.getDREF("sim/flightmodel/position/true_theta")
        brake = self.client.getDREF("sim/cockpit2/controls/parking_brake_ratio")
        wheelS = self.client.getDREF("sim/flightmodel2/gear/tire_rotation_speed_rad_sec")
        wheelW = self.client.getDREF("sim/flightmodel/parts/tire_vrt_def_veh")

        HARDCODE_HEADING = 179
        
        self.airspeed = airspeed[0]
        self.roll = roll[0]
        self.heading = HARDCODE_HEADING
        self.latitude = latitude[0]
        self.longitude = longitude[0]
        self.descent_rate = descent_rate[0]
        self.altitude = altitude[0]
        self.pitch = pitch[0]
        self.brakes = brake[0]
        self.wheelSpeed = wheelS[0]
        self.wheelWeight = wheelW[0]        
        # self.destinations = [self.airspeed,self.roll,self.heading,self.descent_rate,self.altitude,self.pitch,self.brakes,self.wheelSpeed,self.wheelWeight]
        
        self.destinations = {
            "airspeed" : self.airspeed,
            "roll" : self.roll,
            "heading" : self.heading,
            "latitude": self.latitude,
            "longitude": self.longitude,
            "vertical speed" : self.descent_rate,
            "altitude": self.altitude,
            "pitch" : self.pitch,
            "brakes": self.brakes,
            "wheelSpeed": self.wheelSpeed,
            "wheelWeight": self.wheelWeight
        }

        """
        Initial Initialization of target Values
        """
        self.target_airspeed = 80
        self.target_roll = 0
        self.target_heading = self.heading #Track heading from initialization[DEPRECATED]
        self.target_Lat = 39.895791
        self.target_Long = -104.696032
        self.target_descent_rate = 500
        self.target_altitude = -998
        self.target_pitch = 25
        self.targets = [self.target_airspeed,self.target_roll,self.target_heading,self.target_Lat,self.target_Long,self.target_descent_rate,self.target_altitude,self.target_pitch]
        

        #State Flags (Boolean)  & Current State (Integer)
        self.descent = False
        self.flare = False
        self.rollOut = False
        self.currentState = 0
        # self.stateFlags = [self.descent,self.flare,self.rollOut]
        self.phaseFlags = {
            "descent" : self.descent,
            "flare" : self.flare,
            "roll out" : self.rollOut
        }


        # Declare the state for previous values
        self.previous_airspeed = None
        self.previous_roll = None
        self.previous_heading = None
        self.previous_descent_rate = None

        # Initialize the integral errors for each parameter
        self.integral_airspeed = 0
        self.integral_roll = 0
        self.integral_heading = 0
        self.integral_Latitude = 0
        self.integral_Longitude = 0
        self.integral_descent_rate = 0

        #Flare Specific Parameters
        self.integral_pitch = 0 

        # Integral gains (tune these values for performance)
        self.Kp = 0.1  # Proportional gain
        self.Ki = 0.01  # Integral gain                
        # self.Ki = 2  # Integral gain


        """
        Variable Atlas Schema: source => destination => target
        """
        # self.variableAtlas = list(xrange(1000))
        # idx = 0
        # while(idx < len(self.sources) - 1):
        #     self.variableAtlas[0] = [self.sources[idx],self.destinations[idx],self.targets[idx]]
        #     idx += 1

    def dictionaryAccess(self,dictionary,key):
        # print("dictionary access for: " + str(key))
        result = dictionary[key]
        if isinstance(result, tuple):
            return result[0]
        else: 
            return result

        
        
    
    def reassignClient(self,newClient):
        self.client = newClient

    def get_bearing(self,lat1, lat2, long1, long2):
        brng = geo.WGS84.Inverse(lat1, long1, lat2, long2)['azi1']
        self.target_heading = brng
        # print("TARGET BEARING: " + str(brng))

    def getAndLoadDREFS(self):
        results = self.client.getDREFs(self.sources.values())
        idx = 0
        for key in self.sources:
            self.destinations[key] = results[idx]
            idx+=1
        #Update Target Heading
        lat = self.client.getDREF("sim/flightmodel/position/latitude") ##Current Lat 
        long = self.client.getDREF("sim/flightmodel/position/longitude") ##Current Long
        self.get_bearing(lat[0],self.targetLat,long[0],self.targetLong)
        

        # print("getDrefs: " + str(results[1][0]))
        # print("current destination: " + str(self.destinations["airspeed"]))
        # print("current main Airspeed: " + str(self.airspeed))

        # while(idx < len(results) - 2):    
        #     self.destinations[idx] = results[idx][0]
        #     if(idx == 1):
        #         print("getDrefs: " + str(results[idx][0]))
        #         print("current destination: " + str(self.destinations[idx]))
        #         print("current main Airspeed: " + str(self.airspeed))
        #     idx += 1
        

    def printControls(self,calculated,errors,yokePull,yokeSteer,rudder,throttle):
        # print("In print controls")
        if(calculated == 1):
            # print("*        Calculated Controls         *")
            # print("*Parameter,Target,Current,Yoke Pull:      " + "Airspeed, " + str(self.target_airspeed) + "," +  str(self.airspeed)+ "," + str(yokePull))
            # print("*Parameter,Target,Current,Yoke Steer:     " + "Roll, " + str(self.target_roll) + "," +  str(self.destinations["roll"])+ "," + str(yokeSteer))
            # print("*Parameter,Target,Current,Rudder:         " + "Heading, " + str(self.target_heading) + "," +  str(self.heading)+ "," + str(rudder))
            # print("*Parameter,Target,Current,Throttle:       " + "Descent Rate, " + str(self.target_descent_rate) + "," +  str(self.descent_rate)+ "," + str(throttle))
            parameter = ["Airspeed","Roll","Heading","Longitude","Descent Rate","Altitude","Flare: Pitch", "Brakes: Wheel Speed", "Brakes: Wheel Weight"]
            target =  [str(round(self.target_airspeed)),
                       str(round(self.target_roll)),
                       str(round(self.target_heading,3)),
                       str(round(self.target_Long,6)),
                       str(round(self.target_descent_rate,3)),
                       str(round(self.altitude,3)),
                       str(round(self.target_pitch,3)),
                       0, 
                       0]
            
            current = [str(round(self.dictionaryAccess(self.destinations,"airspeed"),3)),
                       str(round(self.dictionaryAccess(self.destinations,"roll"),3)),
                       str(round(self.dictionaryAccess(self.destinations,"heading"),3)),
                       str(round(self.dictionaryAccess(self.destinations,"longitude"),6)),

                       str(round(self.dictionaryAccess(self.destinations,"vertical speed"),3)),
                       str(round(self.dictionaryAccess(self.destinations,"altitude"),3)),
                       str(round(self.dictionaryAccess(self.destinations,"pitch"),3)),
                       str(round(self.dictionaryAccess(self.destinations,"wheelSpeed"),3)),
                       str(round(self.dictionaryAccess(self.destinations,"wheelWeight"),3))]
            controlVal = [str(round(yokePull,3)),
                          str(round(yokeSteer,3)),
                          str(round(rudder,3)),
                          str(round(rudder,3)), 
                          str(round(throttle,3)),
                          str(round(self.altitude,3)),
                          str(self.dictionaryAccess(self.phaseFlags,"flare")),
                          str(round(self.dictionaryAccess(self.destinations,"brakes"),3)),
                          str(round(self.dictionaryAccess(self.destinations,"brakes"),3))]

            header_row = "{:<20} {:<20} {:<20} {:>10}"
            headers = "Parameter Target Current Control_Value".split()
            row = "{:<20} {:<20} {:<20} {:>10}"
            print("\n" + header_row.format(*headers))
            print("-" * 81)
            for parameter, target, current, controlVal in zip(parameter, target, current, controlVal):
                    print(row.format(parameter, target, current, controlVal))


    def printVariables(self,errors,target,current,error,param1,param2):
        if(errors == 1):
            targetF = [str(round(target,3))]
            currentF = [str(round(current,3))]
            errorF = [str(round(error,3))]
            param1F = [str(round(param1,3))]
            param2F = [str(round(param2,3))]
            row = "{:<20} {:<20} {:<20} {:>10} {:>7.2f}"
            header_row = "{:<20} {:<20} {:<20} {:>10} {:>7}"
            headers = "Target Current Error (self.Kp*error) (self.Ki*integral_error)".split()
            print("\n" +header_row.format(*headers))
            print("-" * 81)
            # print(print(row.format(first_, last_, major_, credits_, gpa_)))
            for targetF, currentF, errorF, param1F, param2F in zip(targetF, currentF, errorF, param1F, param2F):
                print(row.format(target, current, error, param1, param2))

            # print("*        Target,     Current,     Error,       param1,        param2      \n" +
            #       "*        ______      _______      ______       _______        ______\n       " +
            #       str(round(target,2)) + "\n                " + str(round(current,2)) +
            #         "\n                     " +  str(round(error,2)) +
            #           "\n                               " +  str(round(param1,2)) +
            #             "\n                             " + str(round(param2,2)))
        


    def proportionalIntegralControl(self,print, current, target, integral_error,scalingFactor):
        """
        Proportional-Integral control rule implementation for multiple parameters.
        """
        # Calculate the error (current - target)
        error = target - current
        # error = current - target
        # print("Error: " +  str(error))
        # Update the integral of the error
        integral_error += error
        # print("Integral_error: " +  str(integral_error))

        # Calculate the control value using the PI formula
        control_value = (self.Kp * error) + (self.Ki * integral_error)

        ###Transformations:
        #Simple Sigmoid:
        control_value = (2 / (1 + math.exp(-(control_value/scalingFactor)))) - 1

        # self.printVariables(print,target,current,error,(self.Kp * error),(self.Ki * integral_error))
        return control_value, integral_error  # Return control value and updated integral error


    def update_controls_simultaneously(self):
        """
        Update all controls at the same time by calculating control values for each parameter.
        """
        # print("In update controls")
        # print("Entered Update Controls Simultaneously")
        # Compute control values for all parameters (yoke pull, yoke steer, rudder, throttle)
        if(self.dictionaryAccess(self.phaseFlags,"flare")): 
             yoke_pull, self.integral_airspeed = self.proportionalIntegralControl(1,self.dictionaryAccess(self.destinations,"pitch"), 
                                                                             self.target_pitch, 
                                                                             self.integral_pitch, 
                                                                     scaleFactor.SCALEYOKEPULL)
        
        if(self.dictionaryAccess(self.phaseFlags,"flare") == False):
             self.target_pitch = 20
             yoke_pull, self.integral_airspeed = self.proportionalIntegralControl(1,self.dictionaryAccess(self.destinations,"pitch"), 
                                                                             self.target_pitch, 
                                                                             self.integral_pitch, 
                                                                             scaleFactor.SCALEYOKEPULL)
            # yoke_pull, self.integral_airspeed = self.proportionalIntegralControl(1,self.airspeed, 
            #                                                                  self.target_airspeed, 
            #                                                                  self.integral_airspeed, 
            #                                                                  scaleFactor.SCALEYOKEPULL)

        yoke_steer, self.integral_roll = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"roll"), self.target_roll, self.integral_roll,scaleFactor.SCALEYOKESTEER)

        """
        RUDDER CONTROLS
        """
        #ORIGINAL
        # rudder, self.integral_heading = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"heading"), self.target_heading, self.integral_heading,scaleFactor.SCALERUDDER) 
        #lATITUDE/LONGITUDE
        if(self.destinations,"longitude" == self.target_Long):
            self.integral_Longitude = 0

        rudder, self.integral_Longitude = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"longitude"), self.target_Long, self.integral_Longitude,scaleFactor.SCALELATITUDERUDDER)

        
        
        
        throttle, self.integral_descent_rate = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"vertical speed"), self.target_descent_rate, self.integral_descent_rate,scaleFactor.SCALETHROTTLE)
        ### 1. For Calculated Yoke and Throttle Values 
        #Invert Throttle Control & divide by 5 to scale

        throttle = -throttle
        throttle = throttle/5
        #Invert Yoke Pull & divide by 5 to scale
        yoke_pull = yoke_pull/5
        ## 2. For Constant Yoke and Throttle Values      
        # Constant yoke "back pressure" equal to 20% of total travel distance
        if(self.dictionaryAccess(self.phaseFlags,"flare") == False):
            yoke_pull = yoke_pull * 20
            # yoke_pull = 0.23
            throttle = 0.20
        if(self.dictionaryAccess(self.phaseFlags,"flare") == True):
            # yoke_pull = -yoke_pull
            yoke_pull = yoke_pull * 20
            throttle = 0
        # Constant throttle setting below the threshold needed to maintain straight and level flight
        
        ## Method 1: 
        # if(self.altitude < 350 and self.airspeed > 175): ## Integrate using the control equations;; A goal state update
        #     throttle = 0.1
        #     yoke_pull = 0.4


        # if(self.altitude < 300 and self.airspeed > 170): ## Integrate using the control equations;; A goal state update
        #     throttle = 0.05
        #     yoke_pull = 0.6

        # if(self.altitude < 250 and self.airspeed > 160): ## Integrate using the control equations;; A goal state update
        #     throttle = 0
        #     yoke_pull = 0.8


        # if(self.altitude < 250 and self.airspeed > 160): ## Integrate using the control equations;; A goal state update
        #     throttle = 0
        #     yoke_pull = 0.8
 
        ##Method 2: Same Control Statements with Change in Parameter to decided pitch from Airspeed ---> Local Pitch Relative to the Horizon

        rudder = rudder * -1

        additive = rudder*1.0
        #Switch Target for Pitch to Local Pitch Axis (ex. +10 Degrees nose up)
        if(self.printControlsFlag):
            self.printControls(1,0,yoke_pull,yoke_steer+additive,rudder,throttle) #PRINT CONTROLS 
        # Send all controls simultaneously to X-Plane
        self.send_controls_to_xplane(yoke_pull, yoke_steer+additive, rudder, throttle)


    def send_controls_to_xplane(self, yoke_pull, yoke_steer, rudder, throttle):
        """
        Sends all control inputs to X-Plane using XPlaneConnect
        """
        # Send yoke pull, yoke steer, rudder, and throttle simultaneously
        # print("Yoke Pull:" + str(yoke_pull))

        #Set the Trim to a value that allows the aircraft to osscilate around the target airspeed
        if(self.dictionaryAccess(self.phaseFlags,"flare") == False):
            trimdref = "sim/flightmodel/controls/elv_trim"
            trim = -0.3
            self.client.sendDREF(trimdref,trim)

        if(self.dictionaryAccess(self.phaseFlags,"flare")):
            trimdref = "sim/flightmodel/controls/elv_trim"
            trim = 0
            self.client.sendDREF(trimdref,trim)

        if(self.dictionaryAccess(self.phaseFlags,"roll out")):
            #Cut the Throttle
            throttle = 0
            #Release Yoke Back Pressure (Pitch Up Pressure from the flare maneuver)
            yoke_pull = 0
            #Hit the Brakes
            brakedref = "sim/cockpit2/controls/parking_brake_ratio"
            brake = 1
            self.client.sendDREF(brakedref,brake)

        self.client.sendCTRL([yoke_pull, yoke_steer, rudder, throttle, -998, -998])  # Control inputs: [yoke_pull, yoke_steer, rudder, throttle]

    def conditionChecks(self):

        if(self.dictionaryAccess(self.destinations,"wheelWeight") > 0.01 
           and self.dictionaryAccess(self.destinations,"wheelSpeed") > 1):
            #Two Parameters to Confirm Touchdown and wheel contact
            # "sim/flightmodel/parts/tire_vrt_def_veh" #Gear Strut Deflection (Weight on wheels)
            # "sim/flightmodel2/gear/tire_rotation_rate_rad_sec" #Tire Rotation Rate
            self.phaseFlags["roll out"] = True
            print("Hit the brakes")

        if(self.dictionaryAccess(self.destinations,"altitude") <= 20 
           and self.dictionaryAccess(self.phaseFlags,"flare") == False): 
            self.phaseFlags["flare"] = True
            self.Ki = 0.01  ## Increase Control Authority to compensate for decreasing airspeed
            print("Altitude < 500; Flare Set True")

        if(self.dictionaryAccess(self.destinations,"wheelWeight") > 0.01 
           and self.dictionaryAccess(self.destinations,"wheelSpeed") < 1 
           and self.dictionaryAccess(self.destinations,"airspeed") < 2 
           and self.dictionaryAccess(self.destinations,"brakes") == 1):            
            self.inProgress = False
    
    def getSimulationStatus(self):
        return self.inProgress
        

    # Update the model's DM based on X-Plane data
    def update_aircraft_state(self):
        """
        Faster Method 
        """
        self.getAndLoadDREFS()
        self.conditionChecks()

    # def logData(self):
