// -*- 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 super::{
    super::{
        base::{kmph_mps, AccelMode, Axis, PhyType},
        brake::Brake,
        curveipo::{CurveIpo, CurvePoint},
        rotation::Rotation,
        vector::{Vector, VectorLim},
        world::WorldContext,
    },
    CollDir, CollHyst, CommonObject, PhyObject,
};
use lazy_static::lazy_static;
use std::time::{Duration, Instant};

lazy_static! {
    // Friction curve on ground
    // CurvePoint::new(speed in m/s, deceleration in m/s**2)
    static ref FRIC_GROUND: CurveIpo = CurveIpo::new(vec![
        CurvePoint::new(kmph_mps(0.0.into()), 8.into()),
        CurvePoint::new(kmph_mps(1.0.into()), 7.into()),
        CurvePoint::new(kmph_mps(20.0.into()), 5.into()),
    ]);

    // Friction curve in air
    // CurvePoint::new(speed in m/s, deceleration in m/s**2)
    static ref FRIC_AIR: CurveIpo = CurveIpo::new(vec![
        CurvePoint::new(kmph_mps(0.into()), 3.into()),
    ]);
}

const WALKER_COLL_HYST: PhyType = PhyType::new_f64(0.1);

#[derive(Copy, Clone, PartialEq, Debug)]
pub enum MoveDirection {
    Stop,
    Fwd,
    Bwd,
}

#[derive(Copy, Clone, PartialEq, Debug)]
pub enum StrafeDirection {
    Stop,
    Left,
    Right,
}

#[derive(Copy, Clone, PartialEq, Debug)]
pub enum TurnDirection {
    Stop,
    Left(PhyType),
    Right(PhyType),
}

#[derive(Copy, Clone, PartialEq, Debug)]
enum JumpState {
    Not,
    Begin,
    Accel,
    Air,
    Repeat,
}

impl std::fmt::Display for JumpState {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "{}",
            match self {
                JumpState::Not => "JumpState::Not",
                JumpState::Begin => "JumpState::Begin",
                JumpState::Accel => "JumpState::Accel",
                JumpState::Air => "JumpState::Air",
                JumpState::Repeat => "JumpState::Repeat",
            }
        )
    }
}

#[derive(Clone, Debug)]
pub struct Walker {
    common: CommonObject,

    move_acc_value: PhyType, // Move acceleration (m/s**2)
    move_acc: Vector,        // Move acceleration vector

    strafe_acc_value: PhyType, // Strafe acceleration (m/s**2)
    strafe_acc: Vector,        // Strafe acceleration vector

    turn_acc: PhyType, // Current turn acceleration (rad/s**2)

    air_trans_acc_fact: PhyType, // Air speed factor for translational movement
    air_rot_acc_fact: PhyType,   // Air speed factor for rotational movement
    walk_speed: VectorLim,       // Walk speed vector

    jump: JumpState,            // Jump state machine
    jump_request: bool,         // Jump requested?
    jump_repeat: bool,          // Repeat jump after hitting ground?
    jump_acc: Vector,           // Jump acceleration m/(s*s)
    jump_acc_sec: Duration,     // Number of seconds to accelerate jump
    jump_acc_end_time: Instant, // Jump acceleration end time (abs)
}

impl Walker {
    pub fn new(
        walk_speed_limit: PhyType,
        air_trans_acc_fact: PhyType,
        air_rot_acc_fact: PhyType,
    ) -> Walker {
        let mut self_ = Walker {
            common: CommonObject::new(),

            move_acc_value: 100.into(),
            move_acc: Vector::new_zero(),

            strafe_acc_value: 100.into(),
            strafe_acc: Vector::new_zero(),

            turn_acc: 0.into(),

            air_trans_acc_fact,
            air_rot_acc_fact,
            walk_speed: Default::default(),

            jump: JumpState::Not,
            jump_request: false,
            jump_repeat: false,
            jump_acc: Vector::new_zero(),
            jump_acc_sec: Duration::from_secs(0),
            jump_acc_end_time: Instant::now(),
        };
        self_.set_walk_speed_limit(walk_speed_limit);
        self_
    }

    pub fn set_walk_speed_limit(&mut self, walk_speed_limit: PhyType) {
        self.walk_speed
            .set_limits(-walk_speed_limit, walk_speed_limit);
    }

    pub fn set_move_acc(&mut self, acc: PhyType) {
        self.move_acc_value = acc;
    }

    pub fn get_move_acc(&self) -> PhyType {
        self.move_acc_value
    }

    pub fn move_(&mut self, dir: MoveDirection) {
        match dir {
            MoveDirection::Stop => {
                self.move_acc.set_zero();
            }
            MoveDirection::Fwd | MoveDirection::Bwd => {
                let rot = self.common.get_rot_move().get_rot();
                self.move_acc.assign(&rot.rotate_vector(&Vector::new(
                    0.into(),
                    self.move_acc_value,
                    0.into(),
                )));
                if dir == MoveDirection::Bwd {
                    self.move_acc *= (-1).into();
                }
            }
        }
    }

    pub fn set_strafe_acc(&mut self, acc: PhyType) {
        self.strafe_acc_value = acc;
    }

    pub fn get_strafe_acc(&self) -> PhyType {
        self.strafe_acc_value
    }

    pub fn strafe(&mut self, dir: StrafeDirection) {
        match dir {
            StrafeDirection::Stop => {
                self.strafe_acc.set_zero();
            }
            StrafeDirection::Left | StrafeDirection::Right => {
                let rot = self.common.get_rot_move().get_rot();
                self.strafe_acc.assign(&rot.rotate_vector(&Vector::new(
                    self.strafe_acc_value,
                    0.into(),
                    0.into(),
                )));
                if dir == StrafeDirection::Left {
                    self.strafe_acc *= (-1).into();
                }
            }
        }
    }

    pub fn turn(&mut self, turn: TurnDirection) {
        match turn {
            TurnDirection::Stop => {
                let rot_acc = Rotation::new_rel(0.into(), 0.into(), self.turn_acc.abs().into());
                self.common
                    .get_rot_move_mut()
                    .set_rot_acc(&rot_acc, AccelMode::Decelerate);
            }
            TurnDirection::Left(turn_acc) => {
                let rot_acc =
                    Rotation::new_rel(0.into(), 0.into(), turn_acc * self.get_acc_fact(false));
                self.common
                    .get_rot_move_mut()
                    .set_rot_acc(&rot_acc, AccelMode::Accelerate);
                self.turn_acc = turn_acc;
            }
            TurnDirection::Right(turn_acc) => {
                let rot_acc = Rotation::new_rel(
                    0.into(),
                    0.into(),
                    turn_acc * (-1).into() * self.get_acc_fact(false),
                );
                self.common
                    .get_rot_move_mut()
                    .set_rot_acc(&rot_acc, AccelMode::Accelerate);
                self.turn_acc = turn_acc;
            }
        }
    }

    pub fn jump(&mut self, jumping: bool, acc: PhyType, acc_sec: PhyType, repeat: bool) {
        if !self.jump_request {
            self.jump_acc = Vector::new(0.into(), 0.into(), acc);
            self.jump_acc_sec = Duration::from_secs_f32(*acc_sec as f32);
            self.jump_repeat = repeat;
        }
        self.jump_request = jumping;
    }

    fn enter_jump_state(&mut self, now: Instant, new_state: JumpState) {
        if new_state != self.jump {
            if false {
                println!("Jump: Entering state {}", new_state);
            }
            if new_state == JumpState::Accel {
                // Stop any Z movement, if any.
                let mut speed = self.common.get_speed();
                speed.set_z(0.into());
                self.common.set_speed(speed);

                // Start Z acceleration.
                self.common.set_acc(self.common.get_acc() + self.jump_acc);

                // Set the end of acceleration.
                self.jump_acc_end_time = now + self.jump_acc_sec;
            }
            self.jump = new_state;
        }
    }

    fn calc_jump(&mut self, context: &WorldContext) {
        // Helpers to check if there is a collision with the ground.
        let almost_collision = |_self: &Walker| {
            _self.common.has_collision_axis(
                Axis::Z,
                CollHyst::Hyst(WALKER_COLL_HYST),
                CollDir::Neg,
                None,
            )
        };
        let full_collision = |_self: &Walker| {
            _self
                .common
                .has_collision_axis(Axis::Z, CollHyst::None, CollDir::Neg, None)
        };
        let no_collision = |_self: &Walker| !almost_collision(_self) && !full_collision(_self);

        // Run the jump state machine.
        match self.jump {
            JumpState::Not => {
                if self.jump_request {
                    self.enter_jump_state(context.now(), JumpState::Begin);
                    if full_collision(self) {
                        self.enter_jump_state(context.now(), JumpState::Accel);
                    }
                }
            }
            JumpState::Begin => {
                if self.jump_request {
                    if full_collision(self) {
                        self.enter_jump_state(context.now(), JumpState::Accel);
                    }
                } else {
                    self.enter_jump_state(context.now(), JumpState::Not);
                }
            }
            JumpState::Accel => {
                if self.jump_request {
                    if context.now() >= self.jump_acc_end_time {
                        self.enter_jump_state(context.now(), JumpState::Air);
                    } else {
                        self.common.set_acc(self.common.get_acc() + self.jump_acc);
                    }
                } else {
                    self.enter_jump_state(context.now(), JumpState::Not);
                }
            }
            JumpState::Air => {
                if self.jump_repeat {
                    if self.jump_request {
                        if almost_collision(self) || full_collision(self) {
                            self.enter_jump_state(context.now(), JumpState::Repeat);
                        }
                    } else {
                        self.enter_jump_state(context.now(), JumpState::Not);
                    }
                } else if full_collision(self) {
                    self.enter_jump_state(context.now(), JumpState::Not);
                }
            }
            JumpState::Repeat => {
                if full_collision(self) || no_collision(self) {
                    self.enter_jump_state(context.now(), JumpState::Accel);
                }
            }
        }
    }

    pub fn is_jumping(&self) -> bool {
        self.jump != JumpState::Not || self.jump_request
    }

    fn calc_walk(&mut self, context: &WorldContext) {
        let walk_acc = (self.move_acc + self.strafe_acc) * self.get_acc_fact(true);

        let walk_speed_inc = walk_acc * context.period_sec();

        *self.walk_speed = self.common.get_speed();
        *self.walk_speed += walk_speed_inc;
        self.walk_speed.apply_limits();

        self.walk_speed.set_z(self.common.get_speed().z());
        self.common.set_speed(*self.walk_speed);
    }

    fn get_acc_fact(&self, translational: bool) -> PhyType {
        if self.common.has_collision_axis(
            Axis::Z,
            CollHyst::Hyst(WALKER_COLL_HYST),
            CollDir::Neg,
            None,
        ) {
            // We are on the ground.
            1.into()
        } else {
            // We are in the air.
            if translational {
                self.air_trans_acc_fact
            } else {
                self.air_rot_acc_fact
            }
        }
    }
}

impl PhyObject for Walker {
    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_jump(context);
        self.calc_walk(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 Walker {
    fn get_brake_deceleration(&self, speed: Vector) -> PhyType {
        // If we are jumping, always use the air curve,
        // unless we really are on the ground.
        let hyst = if self.is_jumping() {
            CollHyst::Hyst(WALKER_COLL_HYST)
        } else {
            CollHyst::None
        };

        // Get the deceleration from the curve.
        if self.common.has_collision(hyst, Some(speed)) {
            FRIC_GROUND.interpolate(speed.length())
        } else {
            FRIC_AIR.interpolate(speed.length())
        }
    }
}

// vim: ts=4 sw=4 expandtab