#pragma once
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_gpio.h"
#include "stm32f4xx_tim.h"

#include "encoders.h"
#include "time_service.h"
#include "MPU9250.h"

#define pwm_perscaller 10
#define pwm_period 32767

#define m1_inverse 1 //-1 to inverse motor
#define m2_inverse 1
#define m3_inverse 1
#define m4_inverse 1

#define max_vel 750 //max velocity for motor in degrees/second

#define k_p 270 //270
#define k_d 1600000 //1600000
#define k_i 0.000008

/*
addition to wheel velocity to turn with angular velocity W
q += W * R / (cos(pi/4) * cos(arctg(l/L) - pi/4))
L = 100mm, l = 110mm
R = 74.33mm (sqrt(L*L + l*l)/2)
r = 22mm wheel radius
*/
#define ang_k 1.83674 //robot angular velocity to linear koef
#define LINEAR_TO_ANGULAR 2.60435 //wheel linear velocity to angular koef

float constrain(float x, float min, float max);

class Motors
{
    public:
        Motors(MPU9250_* mpu);
        void m1(int16_t val); //motors set pwm
        void m2(int16_t val);
        void m3(int16_t val);
        void m4(int16_t val);
        
        void m1_pos_add(double pos); //motors add target position
        void m2_pos_add(double pos);
        void m3_pos_add(double pos);
        void m4_pos_add(double pos);
    
        void m1_pos_set(double pos); //motors set target position
        void m2_pos_set(double pos);
        void m3_pos_set(double pos);
        void m4_pos_set(double pos);
    
        double m1_pos_get(); //motors get target position
        double m2_pos_get();
        double m3_pos_get();
        double m4_pos_get();
    
        void calculate_pid(); //PID - regulator calculation
        void drive(double v, double u, double w); //v - linear velocity, u - side velocity, w - angular velocity
		void driveGyro(double v, double u, double w);
		void stop(); //motors stop
    private:
        double m1_pos; //motors target position
        double m2_pos;
        double m3_pos;
        double m4_pos;
    
        void m1_pid(); //motors PID - regulators
        void m2_pid();
        void m3_pid();
        void m4_pid();
	
		MPU9250_* m_mpu;
};

class Servos
{
    public:
        Servos();
        void s1(float pos); //servos set pos in degrees
        void s2(float pos);
        void s3(float pos);
        void s4(float pos);
	
		void s1_stop();
		void s2_stop();	
		void s3_stop();
		void s4_stop();
    private:
};
