"""
33_yaw_control
----------------


This example demonstrates how to run the ROSCO yaw controller. 
Details of the controller can be found and referenced here: https://wes.copernicus.org/articles/5/451/2020/

The nacelle wind vane signal (``NacVane``) is filtered and, along with offsets either from ``Y_MErrSet`` or ``ZMQ_YawOffset``, a target (``NacHeadingTarget``) is defined.
The error (``Yaw_Err``) is the difference between the target and the current nacelle heading.
The wind speed dependent deadband (``Y_ErrThresh``) determines when the yaw controller will activate.
A state machine will yaw the turbine at a constant rate (``Y_Rate``) until the target is reached.

The following time series should be generated by the example:

.. image:: ../images/examples/33_yaw_control.png

"""

import os
import numpy as np
import matplotlib.pyplot as plt
from rosco.toolbox.ofTools.case_gen.run_FAST import run_FAST_ROSCO
from rosco.toolbox.ofTools.case_gen import CaseLibrary as cl
from rosco.toolbox.ofTools.fast_io import output_processing

rpm2RadSec = 2.0 * (np.pi) / 60.0
deg2rad = np.pi / 180.0

# directories
this_dir = os.path.dirname(os.path.abspath(__file__))
example_out_dir = os.path.join(this_dir, "examples_out")
os.makedirs(example_out_dir, exist_ok=True)

FULL_TEST = False


def main():
    # Input yaml and output directory
    parameter_filename = os.path.join(this_dir, "Tune_Cases/IEA15MW.yaml")

    # Set DISCON input dynamically through yaml/dict
    controller_params = {}
    controller_params["DISCON"] = {}


    # simulation set up
    r = run_FAST_ROSCO()
    r.tuning_yaml = parameter_filename
    r.case_inputs = {}

    # Disable floating DOFs for clarity
    r.case_inputs[("ElastoDyn", "PtfmSgDOF")] = {"vals": ["False"], "group": 0}
    r.case_inputs[("ElastoDyn", "PtfmSwDOF")] = {"vals": ["False"], "group": 0}
    r.case_inputs[("ElastoDyn", "PtfmHvDOF")] = {"vals": ["False"], "group": 0}
    r.case_inputs[("ElastoDyn", "PtfmRDOF")] = {"vals": ["False"], "group": 0}
    r.case_inputs[("ElastoDyn", "PtfmPDOF")] = {"vals": ["False"], "group": 0}
    r.case_inputs[("ElastoDyn", "PtfmYDOF")] = {"vals": ["False"], "group": 0}

    # Enable yaw control in OpenFAST
    r.case_inputs[('ServoDyn','Ptch_Cntrl')] = {'vals': [1], 'group': 0}
    r.case_inputs[('ServoDyn','YCMode')] = {'vals': [5], 'group': 0}
    r.case_inputs[('ServoDyn','TYCOn')] = {'vals': [0], 'group': 0}
    r.case_inputs[('ElastoDyn','YawDOF')] = {'vals': ['True'], 'group': 0}

    # Enable yaw control in ROSCO
    controller_params["Y_ControlMode"] = 1

    run_dir = os.path.join(example_out_dir, "33_yaw_control")

    # Wind case
    r.wind_case_fcn = cl.direction_change
    r.wind_case_opts = {
        "U": [8,8],
        "dir_start": [0, 0],
        "dir_end": [10, -10],
        "t_start": [100, 100],
        "t_end": [130, 130],
        "TMax": [200, 200],
    }
    if not FULL_TEST:
        r.wind_case_opts["t_start"] = [.5, .5]
        r.wind_case_opts["t_end"] = [1.5, 1.5]
        r.wind_case_opts["TMax"] = [2,2]

    # Run simulation
    os.makedirs(run_dir, exist_ok=True)
    r.controller_params = controller_params
    r.save_dir = run_dir
    r.n_cores = 2
    r.run_FAST()



if __name__ == "__main__":
    main()
