# CEA model for RotorSail


from pythonfmu import Fmi2Causality, Fmi2Variability, Fmi2Slave, Real, String
import numpy as np
from scipy.interpolate import LinearNDInterpolator

# COMPILING TO FMU :
# requires pythonfmu package
# pythonfmu build -f rotorsail.py .\requirements.txt


# Positive Thrust -> Forward
# Negative Thrust-> Stern
# Positive Side Force -> Starboard
# Negative Side Force -> Port


class rotorsail(Fmi2Slave):

    def __init__(self, **kwargs):
        super().__init__(**kwargs)

        self.spinning_power = 0 #electrical power required to spin the rotor
        self.thrust = 0 # thrust [N]
        self.side_force = 0 # side force [N]. might be used to derive drift and loading on the deck structure

        self.sr_max = 2 # max SR in model data

        # interpolators
        self.rotor_spin_power_interp = None
        self.thrust_interp = None
        self.side_force_interp = None


        # Other
        self.effective_SR = 0
        self.nb_rotors = 1
        self.max_wind_velocity = 25 # m.s-1


        self.nb_steps = 0
        # data file
        self.model_data_file = 'RotorSail_D-5_De-6_height-35.csv'

        self.data_file_columns = {'rwa':0, 'rws':1, 'sr':2, 'thrust':3,'sideforce':4,'p_spin':5, 'z_ac':6}

        #model data
        self.data_rwa = None # relative wind speed
        self.data_rws = None # relative wind angle
        self.data_SR = None # Spin Ratio
        self.data_thrust = None
        self.data_sideforce = None
        self.data_p_spin = None
        self.data_z_ac = None


        # time step input
        self.rwa = 0 # relative wind speed
        self.rws = 0 # relative wind angle
        self.SR = 0 # Spin Ratio

        self.register_variable(Real("rws", causality=Fmi2Causality.input, description='Relative wind velocity (m.s-1)'))
        self.register_variable(Real("rwa", causality=Fmi2Causality.input, description='Relative wind angle (deg)'))
        self.register_variable(Real("SR", causality=Fmi2Causality.input, description='Spin ratio'))
        self.register_variable(Real("nb_rotors", causality=Fmi2Causality.parameter, variability=Fmi2Variability.tunable,description='Number of Rotors'))
        self.register_variable(Real("max_wind_velocity", causality=Fmi2Causality.parameter, variability=Fmi2Variability.tunable,description='Max Wind Velocity (m.s-1)'))
        self.register_variable(String("model_data_file", causality=Fmi2Causality.parameter, variability=Fmi2Variability.tunable, description='Rotor model data (.csv)'))

        self.register_variable(Real("spinning_power", causality=Fmi2Causality.output, description='Electrical power required to spin the rotor (kW)'))
        self.register_variable(Real("thrust", causality=Fmi2Causality.output, description='Thrust (N)'))
        self.register_variable(Real("side_force", causality=Fmi2Causality.output, description='Side Force (N)'))
        self.register_variable(Real("effective_SR", causality=Fmi2Causality.output, description='Effective Spin Ratio'))



    def do_step(self, current_time, step_size):
        try:
            if self.nb_steps == 0: #init phase - loading model data
                try:
                    #d = np.load('./'+self.model_data_file) # line to load .npy file

                    d = np.genfromtxt('./'+self.model_data_file, delimiter=';', skip_header=1)
                    d = d.transpose()
                except Exception as e:
                    self.log('Issue accessing data file {0}'.format(self.model_data_file +' ('+ str(e))+' )')

                self.data_rwa = d[self.data_file_columns['rwa']]
                self.data_rws = d[self.data_file_columns['rws']]
                self.data_SR = d[self.data_file_columns['sr']]
                self.data_thrust = d[self.data_file_columns['thrust']]
                self.data_sideforce = d[self.data_file_columns['sideforce']]
                self.data_p_spin = d[self.data_file_columns['p_spin']]
                self.data_z_ac = d[self.data_file_columns['z_ac']]

                self.sr_max = np.max(self.data_SR)
                #interpolators
                self.rotor_spin_power_interp = LinearNDInterpolator(list(zip(self.data_rwa, self.data_rws, self.data_SR)), self.data_p_spin)
                self.thrust_interp = LinearNDInterpolator(list(zip(self.data_rwa, self.data_rws, self.data_SR)), self.data_thrust)
                self.side_force_interp = LinearNDInterpolator(list(zip(self.data_rwa, self.data_rws, self.data_SR)), self.data_sideforce)

            if self.rwa > 180:
                rwa = 360 - self.rwa
            else:
                rwa = self.rwa

            if self.SR > self.sr_max or self.SR < 0:
                self.log('SR out of boundaries.')
                if self.SR < 0:
                    self.effective_SR = 0
                else:
                    self.effective_SR = self.sr_max
            else:
                self.effective_SR = self.SR

            # max wind velocity reached, SR set O
            if self.rws > self.max_wind_velocity:
                self.effective_SR = 0

            self.thrust = float(self.thrust_interp(rwa, self.rws, self.effective_SR)) * self.nb_rotors # Newton
            self.spinning_power = float(self.rotor_spin_power_interp(rwa, self.rws, self.effective_SR)) * self.nb_rotors * 1e-3  # kW
            self.side_force = float(self.side_force_interp(rwa, self.rws, self.effective_SR)) * self.nb_rotors # Newton


            self.nb_steps += 1
        except Exception as e:
            self.log('Error:'+str(e))

        return True
