// -*- coding: utf-8 -*-
//
// Copyright 2021 Michael Büsch <m@bues.ch>
//
// Licensed under the Apache License version 2.0
// or the MIT license, at your option.
// SPDX-License-Identifier: Apache-2.0 OR MIT
//

use crate::{
    base::{kmph_mps, radians, Axis, PhyType},
    brake::Brake,
    curveipo::{CurveIpo, CurvePoint},
    object::{CollHyst, CommonObject, PhyObject},
    rotation::{rotmove::RotMove, RotAxis, RotLimits, Rotation, RotationLim},
    vector::{Vector, VectorLim},
    world::WorldContext,
};
use lazy_static::lazy_static;

lazy_static! {
    static ref DEFAULT_YOKE_PITCH_CURVE: CurveIpo = CurveIpo::new(vec![
        // yoke_rotation, pitch_speed
        CurvePoint::new(radians((-45).into()), (-1).into()),
        CurvePoint::new(radians(0.into()), 0.into()),
        CurvePoint::new(radians(45.into()), 1.into()),
    ]);

    static ref DEFAULT_YOKE_ROLL_CURVE: CurveIpo = CurveIpo::new(vec![
        // yoke_rotation, roll_speed
        CurvePoint::new(radians((-45).into()), (-1).into()),
        CurvePoint::new(radians(0.into()), 0.into()),
        CurvePoint::new(radians(45.into()), 1.into()),
    ]);

    static ref DEFAULT_SPEED_WING_LIFT_CURVE: CurveIpo = CurveIpo::new(vec![
        // speed_over_ground, lift_speed
        CurvePoint::new(kmph_mps(0.into()), 0.into()),
        CurvePoint::new(kmph_mps(600.into()), 0.2.into()),
    ]);

    static ref DEFAULT_PITCH_WING_LIFT_CURVE: CurveIpo = CurveIpo::new(vec![
        // pitch, lift_speed_factor
        CurvePoint::new(radians((-5).into()), 0.into()),
        CurvePoint::new(radians(0.into()), 1.into()),
        CurvePoint::new(radians(35.into()), 1.into()),
        CurvePoint::new(radians(45.into()), 0.into()),
    ]);

    // Friction curve on ground
    static ref DEFAULT_FRIC_GROUND_CURVE: CurveIpo = CurveIpo::new(vec![
        // speed, deceleration
        //TODO
        CurvePoint::new(kmph_mps(0.0.into()), 90.into()),
        CurvePoint::new(kmph_mps(1.0.into()), 30.into()),
        CurvePoint::new(kmph_mps(20.0.into()), 20.into()),
    ]);

    // Friction curve in air
    static ref DEFAULT_FRIC_AIR_CURVE: CurveIpo = CurveIpo::new(vec![
        // speed, deceleration
        //TODO
        CurvePoint::new(kmph_mps(0.into()), 3.into()),
    ]);

    //TODO
    // Default rotational roll acceleration.
    static ref DEFAULT_ROLL_ACC: PhyType = radians(PhyType::new_f64(10.0));
    // Default rotational pitch acceleration.
    static ref DEFAULT_PITCH_ACC: PhyType = radians(PhyType::new_f64(10.0));
}

pub struct AviationObj {
    common: CommonObject,

    yoke: RotationLim,
    yoke_pitch_curve: CurveIpo,
    yoke_roll_curve: CurveIpo,
    yoke_rot_move: RotMove,

    roll_acc: PhyType,
    pitch_acc: PhyType,

    speed_wing_lift: CurveIpo,
    pitch_wing_lift: CurveIpo,
    fric_ground_curve: CurveIpo,
    fric_air_curve: CurveIpo,

    thrust_acc: VectorLim,
}

impl AviationObj {
    pub fn new_default() -> AviationObj {
        AviationObj::new(None, None, None, None, None, None, None, None)
    }

    #[allow(clippy::too_many_arguments)]
    pub fn new(
        yoke_pitch_curve: Option<CurveIpo>,
        yoke_roll_curve: Option<CurveIpo>,
        roll_acc: Option<PhyType>,
        pitch_acc: Option<PhyType>,
        speed_wing_lift: Option<CurveIpo>,
        pitch_wing_lift: Option<CurveIpo>,
        fric_ground_curve: Option<CurveIpo>,
        fric_air_curve: Option<CurveIpo>,
    ) -> AviationObj {
        let mut self_ = AviationObj {
            common: CommonObject::new(),

            yoke: RotationLim::new(Rotation::new_zero_rel()),
            yoke_pitch_curve: yoke_pitch_curve
                .or_else(|| Some(DEFAULT_YOKE_PITCH_CURVE.clone()))
                .unwrap(),
            yoke_roll_curve: yoke_roll_curve
                .or_else(|| Some(DEFAULT_YOKE_ROLL_CURVE.clone()))
                .unwrap(),
            yoke_rot_move: RotMove::new_rel(),

            roll_acc: roll_acc.unwrap_or(*DEFAULT_ROLL_ACC),
            pitch_acc: pitch_acc.unwrap_or(*DEFAULT_PITCH_ACC),

            speed_wing_lift: speed_wing_lift
                .or_else(|| Some(DEFAULT_SPEED_WING_LIFT_CURVE.clone()))
                .unwrap(),
            pitch_wing_lift: pitch_wing_lift
                .or_else(|| Some(DEFAULT_PITCH_WING_LIFT_CURVE.clone()))
                .unwrap(),
            fric_ground_curve: fric_ground_curve
                .or_else(|| Some(DEFAULT_FRIC_GROUND_CURVE.clone()))
                .unwrap(),
            fric_air_curve: fric_air_curve
                .or_else(|| Some(DEFAULT_FRIC_AIR_CURVE.clone()))
                .unwrap(),

            thrust_acc: VectorLim::new(Vector::new_zero()),
        };

        // Set limits of the yoke.
        self_.set_yoke_limits(
            RotLimits::new((-60).into(), 60.into()),
            RotLimits::new((-15).into(), 15.into()),
        );

        // Set limits of the plane rotation movement that the yoke causes.
        self_.set_rot_limits(radians(45.into()), radians(15.into()));

        self_
    }

    pub fn get_thrust_acc(&self) -> PhyType {
        self.thrust_acc.y()
    }

    pub fn set_thrust_acc(&mut self, thrust_acc: PhyType) {
        self.thrust_acc.set_y(thrust_acc);
        self.thrust_acc.apply_limits();
    }

    pub fn set_thrust_acc_limits(&mut self, lim: (PhyType, PhyType)) {
        self.thrust_acc.set_clipping(Axis::Y, lim.0, lim.1);
    }

    pub fn get_yoke_roll(&self) -> PhyType {
        self.yoke.get().alpha()
    }

    pub fn get_yoke_pitch(&self) -> PhyType {
        self.yoke.get().beta()
    }

    pub fn set_yoke_roll(&mut self, roll_rotation: PhyType) {
        self.yoke.get_mut().set_alpha(roll_rotation);
    }

    pub fn set_yoke_pitch(&mut self, pitch_rotation: PhyType) {
        self.yoke.get_mut().set_beta(pitch_rotation);
    }

    pub fn set_yoke_limits(&mut self, roll_limit: RotLimits, pitch_limit: RotLimits) {
        self.yoke.set_limits_axis(RotAxis::Alpha, roll_limit);
        self.yoke.set_limits_axis(RotAxis::Beta, pitch_limit);
        self.yoke.set_limits_axis(RotAxis::Gamma, RotLimits::zero());
    }

    pub fn set_rot_limits(&mut self, roll_limit: PhyType, pitch_limit: PhyType) {
        self.yoke_rot_move.set_rot_lim(
            &Rotation::new_rel(-roll_limit, -pitch_limit, radians(0.into())),
            &Rotation::new_rel(roll_limit, pitch_limit, radians(0.into())),
        );
    }

    pub fn get_pitch(&self) -> PhyType {
        self.yoke_rot_move.get_rot().beta()
    }

    pub fn get_roll(&self) -> PhyType {
        self.yoke_rot_move.get_rot().alpha()
    }

    pub fn get_yaw(&self) -> PhyType {
        self.yoke_rot_move.get_rot().gamma()
    }

    fn calc_aviation(&mut self, context: &WorldContext) {
        //println!("yoke {}", self.yoke);

        // Convert yoke position to roll/pitch speed.
        let roll_speed = self.yoke_roll_curve.interpolate(self.yoke.get().alpha());
        let pitch_speed = self.yoke_pitch_curve.interpolate(self.yoke.get().beta());
        // Accelerate the rotational move to the speed.
        let rot_acc = Rotation::new_rel(self.roll_acc, self.pitch_acc, 0.into());
        let rot_speed = Rotation::new_rel(roll_speed, pitch_speed, 0.into());
        self.yoke_rot_move.acc_to_speed(&rot_speed, &rot_acc);
        // Recalculate the new rotational position.
        self.yoke_rot_move.calc(context);
        //println!("{}", self.yoke_rot_move);

        // Rotate the thrust vector by the current plane rotation.
        let thrust_acc = self.yoke_rot_move.get_rot().rotate_vector(&self.thrust_acc);

        //TODO yaw based on speed and pedals

        // Rotate the plane
        self.set_rot(&self.yoke_rot_move.get_rot().absolute());

        // Calculate the new speed from thrust acceleration.
        self.common
            .set_speed(self.common.get_speed() + (thrust_acc * context.period_sec()));

        // Calculate the speed on the X/Y plane.
        let mut speed_over_ground = VectorLim::new(self.common.get_speed());
        speed_over_ground.set_clipping(Axis::Z, 0.into(), 0.into());
        speed_over_ground.apply_limits();

        // Calculate the wing lift speed.
        //TODO only if moving forward
        let mut lift_speed = Vector::new(
            0.into(),
            0.into(),
            self.speed_wing_lift.interpolate(speed_over_ground.length()),
        );
        lift_speed *= self.pitch_wing_lift.interpolate(self.get_pitch());
        self.common.set_speed(self.common.get_speed() + lift_speed);
    }
}

impl Default for AviationObj {
    fn default() -> Self {
        AviationObj::new_default()
    }
}

impl PhyObject for AviationObj {
    fn get_common(&self) -> Option<&CommonObject> {
        Some(&self.common)
    }

    fn get_common_mut(&mut self) -> Option<&mut CommonObject> {
        Some(&mut self.common)
    }

    fn calc(&mut self, context: &WorldContext) {
        self.calc_aviation(context);
        self.common.calc_acc(context);
        self.common.calc_speed(context);
        self.common.get_rot_move_mut().calc(context);
        self.common
            .set_speed(self.apply_brake(context, self.common.get_speed()));
        self.common.calc_speed_clipping(context);
        self.common.calc_pos(context);
        self.common.calc_finalize(context);
    }
}

impl Brake for AviationObj {
    fn get_brake_deceleration(&self, speed: Vector) -> PhyType {
        // Get the deceleration from the curve.
        let hyst = CollHyst::Hyst(0.1.into());
        if self.common.has_collision(hyst, Some(speed)) {
            self.fric_ground_curve.interpolate(speed.length())
        } else {
            self.fric_air_curve.interpolate(speed.length())
        }
    }
}

// vim: ts=4 sw=4 expandtab