// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#include "CoreMinimal.h"
#include "GameFramework/Pawn.h"
#include "EKFilter.h"
#include "IMUSensor.h"
#include "GPSSensor.h"
#include "Barometer.h"
#include "SharedDef.h"
#include "MultiRotorDrone.generated.h"

#define GYR0_LIMIT   2000
#define ACCEL_LIMIT  16
#define GRAVITY      9.81
#define MF_INTECITY  31.84
#define MF_INC       -30.89
#define MF_DEC       -4.016
#define AIR_DENCITY  1.1839

USTRUCT(BlueprintType)
struct FTrueDroneState
{
	GENERATED_BODY()

	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	FVector Position;

	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	FVector Velocity;

	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	FVector Acceleration;

	UPROPERTY(EditAnywhere, BlueprintReadWrite)
	FVector Orientation;

	bool initialised = false;
};

USTRUCT(BlueprintType)
struct FDroneData
{
	GENERATED_BODY()

public:
	UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
	TArray<float> MotorsRPM = { 0.0, 0.0, 0.0, 0.0 };
	UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
	FVector Position;
	UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
	FVector Velocity;
	UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
	FVector Orientation;
};

UCLASS()
class LUCKYWORLDV2_API AMultiRotorDrone : public APawn
{
	GENERATED_BODY()

public:
	// Sets default values for this pawn's properties
	AMultiRotorDrone();

	UFUNCTION(BlueprintCallable)
	void SetImuSensor(UIMUSensor* sensor);

	UFUNCTION(BlueprintCallable)
	void SetGPSSensor(UGPSSensor* sensor);

	UFUNCTION(BlueprintCallable)
	void SetBaroSensor(UBarometer* sensor);

	UFUNCTION(BlueprintCallable)
	void SetTrueState(FTrueDroneState true_state);

	UFUNCTION(BlueprintCallable)
	void IncreaseDecreaseTargetHeight(float value);

	UFUNCTION(BlueprintCallable)
	void HoldAltitude();

	UFUNCTION(BlueprintCallable)
	void MoveLeftRight(float value);

	UFUNCTION(BlueprintCallable)
	void HoldXYPosition(bool X, bool Y);

	UFUNCTION(BlueprintCallable)
	void MoveForwardBackward(float value);

	UFUNCTION(BlueprintCallable)
	void UpdateYaw(float value);

	UFUNCTION(BlueprintCallable)
	FDroneData GetDronInfo();
protected:
	// Called when the game starts or when spawned
	virtual void BeginPlay() override;

	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Motors", meta = (AllowPrivateAccess = true))
	TArray<double> RPM = { 0, 0, 0, 0 };

	UPROPERTY(BlueprintReadWrite)
	TArray<double> ForcesToApply = { 0, 0, 0, 0 };

	UPROPERTY(BlueprintReadWrite)
	TArray<double> MomentsToApply = { 0, 0, 0, 0 };

	UPROPERTY(BlueprintReadWrite)
	TArray<double> ComputedDrag = { 0, 0, 0 };

	UPROPERTY(VisibleAnywhere, BlueprintReadWrite, meta = (AllowPrivateAccess = true))
	TArray<double> GyroBuffer;

	UPROPERTY(VisibleAnywhere, BlueprintReadWrite, meta = (AllowPrivateAccess = true))
	bool LandingStarted = false;

	UIMUSensor* Imu_sensor;
	UGPSSensor* GPS_sensor;
	UBarometer* Baro_sensor;

	EKFilter StateEstimator;

	struct IMUdata
	{
		FVector data;
		double dt;
	};

	union
	{
		IMUdata RecentIMUData;
		double IMUDataArray[4];
	};

	enum FlyingMode
	{
		MANUAL = 1,
		GUIDED
	};

	bool KeepAltitude = false;
	bool KeepXYPosition = false;

	FlyingMode Mode = GUIDED;
	bool use_true_state = false;


	struct PID_Controller
	{
		//float Kpz = 100;
		float Kpz = 7.0;
		float Kvz = 5.0;
		//float Kvz = 30;
		float Kd_z = 65;
		//float Ki_z = 10;
		float Ki_z = 0;
		float Zvel_int = 0.0;
		//float Kp_yaw = 0.004;
		//float Kd_yaw = 0.3 * 0.004;
		float Kp_yaw = 100;
		float Kd_yaw = 10;
		//float Kpx = -30.0;
		float Kpx = -0.05;
		//float Kpy = 30.0;
		float Kpy = 0.05;
		//float Kvx = 0.1;
		//float Kvy = -0.1;
		float Kvx = 0.1;
		float Kvy = -0.1;
		float Kd_x = 0.75;
		float Kd_y = 0.75;
		float Ki_x = -0.5;
		float Ki_y = 0.5;
		float Xvel_int = 0;
		float Yvel_int = 0;
		float I_xy = 0;
		//float Kpr = 0.01;
		float Kpr = 30;
		//float Kpp = 0.013;
		float Kpp = 30;
		//float Kdr = 0.0028;
		float Kdr = 10;
		//float Kdp = 0.002;
		float Kdp = 20;
		//float I_pr = 0.01;
		float I_pr = 0;
		//float AntiWU = 0.001;
		float AntiWU = 0;
		FVector pr_error_int = { 0.0, 0.0, 0.0 };
		FVector prev_pr_error = { 0.0, 0.0, 0.0 };
		FVector XY_error_int = { 0.0, 0.0, 0.0 };
		FVector prev_XYZ_error = { 0.0, 0.0, 0.0 };
	}pid;

	float k_throttle = 600;
	float k_roll = 200.0;
	float k_pitch = 200.0;
	float k_yaw = 200.0;

	float Ts2Q[16];

	struct PID_Controller2
	{
		float zKp = 100;
		float zKd = 80;
		float xKp = 1.0;
		float xKd = 0;
		float yKp = 1.0;
		float yKd = 0;
		float Kp_roll = 10;
		float Kp_pitch = 10;
		float Kp_yaw = 10.0;
		//float Kp_p = 100;
		//float Kp_q = 5;
		float Kp_p = 10;
		float Kp_q = 10;
		float Kp_r = 1;
	}pid2;

	struct Rotor
	{
		float Ct = 0.05;
		float Cq = 0.05 / (2 * PI);
		TArray<double> cThrust = { 0.2, 0.1, 0.3, 0.09, 0.4, 0.075, 0.5, 0.055, 0.6, 0.04 };
		TArray<double> cTorque = { 0.2, 0.05, 0.3, 0.05, 0.4, 0.05, 0.5, 0.04, 0.6, 0.03, 0.7, 0.02, 0.8, 0.01 };
		float Diameter = (21 * 25.4) / 1000;
		FVector p1 = { 0.286 + 0.02, 0.363, 0.217 };
		FVector p2 = { -0.308 + 0.02, -0.316, 0.217 };
		FVector p3 = { 0.286 + 0.02, -0.363, 0.217 };
		FVector p4 = { -0.308 + 0.02, 0.316, 0.217 };
		double get_value(TArray<double> table, double key)
		{
			if (key <= table[0])
				return table[1];
			else if (key >= table[table.Num() - 2])
				return table[table.Num() - 1];

			unsigned int r = 1;
			while (table[2 * r] < key) r++;

			double x0 = table[2 * r - 2];
			double span = table[2 * r] - x0;
			double factor = (key - x0) / span;
			double y0 = table[2 * r - 1];
			return factor * (table[2 * r + 1] - y0) + y0;
		};
	}Rotors;

	float Kt, Km;

	float Mass = 6.6;

	float ThrustToRPM = 100;

	int NumMotors = 4;

	float MinRPM = 1000;
	float MaxRPM = 10000;
	float PWM_min_us = 1200;
	float PWM_max_us = 1800;
	float ThrottleRate = 100;
	float CurrentThrottle = 1500;
	float CurrentRollPWM = 1500;
	float CurrentPitchPWM = 1500;
	float RollPitchPWMRate = 300;
	float TargetThrust = 0;
	float MaxThrust;
	float XYerrTol = 0.1;

	float MaxRoll = PI / 6;
	float MaxPitch = PI / 6;
	float MaxRPrate = 2 * PI;
	float MaxVelocity = 10;
	float MaxZVelocity = 20;
	float MaxAcceleration = 3.0;

	float ComputedRPM[4];

	FVector Inertia = { 0.06, 0.06, 0.1 };

	FVector TargetPosition;
	FVector TargetVelocity = { 0.0, 0.0, 0.0 };
	FVector TargetAccel = { 0.0, 0.0, 0.0 };

	//FVector Euler = { 0.0, 0.0, 0.0 };
	FVector PQR = { 0.0, 0.0, 0.0 };

	FVector COMLocation;
	FTrueDroneState BodyState;

	float TargetYaw;
	float YawRate = PI / 6;
	float TurnDir = 0.0f;

	bool TakeOffStarted = false;
	bool OnGround = true;

	float minThrust;

	float maxThrustPerMotor = 70 / 4;

	float ThrustCmd = 0.0;

	float CdZ = 3;
	float CdXY = 1.5;
	float SrefZ;
	float SrefX = 0.2 * 0.2;
	float SrefY = 0.2 * 0.3;

	FTrueDroneState TrueState;
	FTrueDroneState State;
	OutputState EstState;

	bool AltitudeManualControl = true;
	bool XYmanualControl = true;

	void SetCurrentState();

	void EulerDt2PQR(FVector& EulerDt);

	void AltitudeController(float& target_height, float& AltCmd);

	void YawController(float& target_yaw, float& tau_yaw);

	void RollPitchCmdController(float& Xref, float& Yref, float& roll_cmd, float& pitch_cmd);

	void AttitudeController(float& roll_cmd, float& pitch_cmd, float& roll_tau, float& pitch_tau);

	void Mixer(float& tot_thrust, float& roll_tau, float& pitch_tau, float& yaw_tau);

	void ControllerUpdate();

	void ComputeForcesAndMoments();

	void ComputedDragForce();

	//void CorrectForces();

	void LateralController(float& x, float& dx, float& ddx, float& y, float& dy, float& ddy, float& c, float& bx_c, float& by_c);

	void RollPitchController(float& bx_c, float& by_c, float& p_c, float& q_c);

	void BodyRateController(float& p_c, float& q_c, float& r_c, float& u_bar_p, float& u_bar_q, float& u_bar_r);

	void HeadingController(float& yaw_c, float& yaw_tau);

	void HeightController(float& z_t, float& dz_t, float& ddz_t, float& c);

	void Controller2Update();

	float GetJ(FVector PropLoc, float RPS, float D);

	void Landing();

	//void StructuralToCOM();

public:
	// Called every frame
	virtual void Tick(float DeltaTime) override;

	// Called to bind functionality to input
	virtual void SetupPlayerInputComponent(class UInputComponent* PlayerInputComponent) override;

private:

	float MjStep = 1 * 0.001667;
	//float MjStep = 1 * 0.001;

	float wrap_2PI(const float radian)
	{
		float res = fmodf(radian, 2 * PI);
		if (res < 0) {
			res += 2 * PI;
		}
		return res;
	}
	float wrap_PI(const float radian)
	{
		float res = wrap_2PI(radian);
		if (res > PI) {
			res -= 2 * PI;
		}
		return res;
	}
};