class DC_motor_controller{
  public:
  	// Set_pins:
    void hBridge(uint8_t in1, uint8_t in2, uint8_t en );	// Pinos da ponte H L298N responsáveis pelo motor
    void hBridge(uint8_t in1, uint8_t in2);
    void setEncoderPin(uint8_t pinA, uint8_t pinB);		// Pinos do encoder do motor

    // Set technical features:
    void setRefreshTime(unsigned long t);				// Refresh time for wakl and gyrate, default is 50, but you can change it with this function
    void setPPR(uint16_t ppr);							// Pulses per revolution, default is 11, but you can change it with this function.
    void setRR(float rr);								// Rotation ratio
    void setPIDconstants(float kp, float ki, float kd);	// PID constants (KP, Ki and KD), para mudar os valores das constantes do PID conforme a necessidade
    void setMaxI(int max);

    // Setting pins:
    void setPins();	// Configura os pinos usados pelo programa

    // Actions:
    void run(int pwm);					// Apply a simple pwm on the motor
    void walk(float sp);
    void walk(float sp, float rot);	// Motor simple walk - For only one motor and it uses While
    void gyrate(float sp, float rot=0);	// Motor gyrate - For one or two motors and needs be into a while
    void stop(unsigned int t=0);
    void stop_both(int time=0);
   	void accelerate(float sp, float accel);
   	   	
   	//Debug functions
   	void debugMaxVel();		// Debug function that fix max velocity motor error
   	void invertDirection();	// Revert the direction of the motor (when it's inverted)
   	
    // Others...
    void isr();
    void computeRPM();
    float getRPM();
    void startCounting();	// Starts counting rotations number since now
    void stopCounting();	// Stops counting rotations number
    float getRotations();	// Returns the actual rotations cumulated number
    void reset();
    bool canRun();
    bool canStop();
    void resetForGyrate();
    int getPWM(); // Retorna o PWM aplicado aos motores
    unsigned int getRefreshTime();

    volatile long int pulses[2] = {0, 0}; // pulses[0] - para o RPM, pulses[1]- rotação
    bool anti_inertia = true;

//private:
    int maxI = 200;
    void applyIntegralLimit();
    void ifNegativeAllNegative(float &val_1, float &val_2);
    unsigned long lastTime = 0, deltaTime, refreshTime=50;  // Usado pelo PID
    unsigned long lastTime_accel = 0;// For acceleration control
    unsigned long elapsed_time_accel;
    uint8_t encoderPinA, encoderPinB;
    float ppr = 11, rr, rpm;
    float kp = 1.2, ki = 1, kd = 0.15, P = 0, I = 0, D = 0, pid;	// Valores padrão para as constantes do PID;
    int pwm = 0;
    int default_acceleration = 50;	// 50 RMP/s
    
    float error, lastError = 0;
    int computePID(float input, float sp, bool derivative);
    int computeAll(float sp);
    byte doPID(float input, float sp); // Compute PID based on a input value and refresh 
    uint8_t in1, in2, en;
    bool can_run = false, can_stop = false, can_accelerate = false;
    uint16_t deltaT = 0, lastT; // Controle de tempo e pulsos do métodp gyrate
    long Pulses = 0;
    
    bool is_counting = false;
    float total_rot = 0;
    int direction = 1;

    float inertia_time_coeficient = 1.7;
    int max_anti_inertia_time = 250; // Max anti_intertia time action.
    float anti_inertia_time(float vel=0);
    void stop_vel(unsigned int vel = 0);
    unsigned int elapsed_stop_time = 0;

};



void DC_motor_controller::hBridge(uint8_t in1, uint8_t in2, uint8_t en){
	this-> in1 = in1;
	this-> in2 = in2;
	this-> en  =  en;
}

void DC_motor_controller::hBridge(uint8_t in1, uint8_t in2){
	hBridge(in1, in2, in2);
}

void DC_motor_controller::setPins(){
	pinMode(in1, OUTPUT);
	pinMode(in2, OUTPUT);
	pinMode(en, OUTPUT);
	pinMode(encoderPinA, INPUT);
	pinMode(encoderPinB, INPUT);
}

void DC_motor_controller::run(int pwm){
	if(pwm > 255) pwm = 255;
	if(pwm < (-255)) pwm = (-255);

	if(pwm == 0){
		digitalWrite(in1, LOW);
		digitalWrite(in2, LOW);
	} else {
		bool forward = (pwm > 0) ? true : false;
	
		pwm = abs(pwm);
		if((in1 == en) || (in2 == en)){	// If enable pin is also a direction control pin, PWM must be applied on a control pin that is on HIGH state
			if(forward){
				analogWrite(in1, pwm);
				digitalWrite(in2, LOW);
			} else {
				digitalWrite(in1, LOW);
				analogWrite(in2, pwm);
			}	
		} else {
			if(forward){
				digitalWrite(in1, HIGH);
				digitalWrite(in2, LOW);
			} else {
				digitalWrite(in1, LOW);
				digitalWrite(in2, HIGH);
			}
			analogWrite(en, pwm);
		}
	}
}

void DC_motor_controller::setEncoderPin(uint8_t pinA, uint8_t pinB){
	this->encoderPinA = pinA;
	this->encoderPinB = pinB;
}

void DC_motor_controller::isr(){
	if(digitalRead(encoderPinB)){ // Sentido horário
		pulses[0]+= direction;
		pulses[1]+= direction;
	} else {  // Sentido anti-horário
		pulses[0]-= direction;
		pulses[1]-= direction;
	}
}

void DC_motor_controller::setRefreshTime(unsigned long t){
	this->refreshTime = t;
}

void DC_motor_controller::setPPR(uint16_t ppr){
	this->ppr = ppr;
}

void DC_motor_controller::setRR(float rr){
	this->rr = rr;
}

void DC_motor_controller::setMaxI(int max){
    this->maxI = max;
}

void DC_motor_controller::computeRPM(){
	deltaTime = millis() - lastTime;

	if(deltaTime >= refreshTime){
		rpm = (pulses[0] * (60000.0 / (ppr * rr))) / deltaTime;;
		pulses[0] = 0; 

		if(is_counting){
			total_rot += rpm*deltaTime/60000.0;
		}
	}
}

float DC_motor_controller::getRPM(){
	return rpm;
}

int DC_motor_controller::getPWM(){
	return pwm;
}

unsigned int DC_motor_controller::getRefreshTime(){
	return refreshTime;
}

void DC_motor_controller::setPIDconstants(float kp, float ki, float kd){
	this->kp = kp;
	this->ki = ki;
	this->kd = kd;
}

void DC_motor_controller::debugMaxVel(){
	direction = -direction;
}

void DC_motor_controller::invertDirection(){
	int old_in1 = in1, old_in2 = in2, old_encoderPinA = encoderPinA, old_encoderPinB = encoderPinB;
	
	debugMaxVel();
	
	in1 = old_in2;
	in2 = old_in1;
}


int DC_motor_controller::computePID(float input, float sp, bool derivative){ // Compute and return the PID value.
	error = sp - input;                                   // Calcula o erro

	P = error * kp;                                       // Calcula a proporcional
	I += error * ki * (deltaTime / 1000.0);               // Calcula a integral
	derivative ? D = (error - lastError) * kd / (deltaTime / 1000.0) : D=0;

	applyIntegralLimit();
	pid = P + I + D;                                      // pid receba a soma de P, I e D

	lastError = error;                                    // Erro anterior = erro atual

	return pid;                                           // Retorna o valor do pid
}

void DC_motor_controller::applyIntegralLimit(){
	if(I > maxI) I=maxI;
    if(I < -maxI) I=-maxI;
}

int DC_motor_controller::computeAll(float sp){
	deltaTime = millis() - lastTime;      // Tempo decorrido

	if(deltaTime >= refreshTime){        	// Se o tempo deccorrido for maior ou igual ao tempo de refresh...
		cli();                              // Desativa todas as interrupções para o cálculo
		computeRPM();                       // Calcula a velocidade atual
		
		if(rpm >= sp) can_accelerate = false;

    Serial.println("Velocidade atual:" + String(rpm));
		
		if(can_accelerate){ // Considerando a velocidade dinicial = 0

      elapsed_time_accel = millis() - lastTime_accel;
			
			static int actual_vel = default_acceleration * elapsed_time_accel;
			sp = actual_vel;
			
			Serial.println("Velocidade aceleracao: " + String(actual_vel));
		} 
		
		
		pwm = computePID(rpm, sp, false);   // Calcula o valor do PID tendo como entrada a velocidade(rpm)
											// e o set point(sp)
		lastTime = millis();                // Atualiza o tempo
		sei();                              // Reativa todas as interrupções após o cálculok		
	}
	return pwm;                           // Retorna o valor do pwm (o mesmo do pid)
}

byte DC_motor_controller::doPID(float input, float sp){ // Looks like compulte_all, but it don't use RPM as input value
	deltaTime = millis() - lastTime;      // Tempo decorrido

	if(deltaTime >= refreshTime){         // Se o tempo deccorrido for maior ou igual ao tempo de refresh...
		cli();                              // Desativa todas as interrupções para o cálculo
		pwm = computePID(input, sp, false); // Calcula o valor do PID                       
		lastTime = millis();                // Atualiza o tempo
		sei();                              // Reativa todas as interrupções durante o cálculo
	}
	return pwm;                           // Retorna o valor do pwm (o mesmo do pid)
}

void DC_motor_controller::walk(float sp){	// Simply makes the wheel run by a constant velocity
	if(sp==0)	run(0);
	else		run(computeAll(sp));
}

void DC_motor_controller::walk(float sp, float rot){
	bool can_run_local = true;
	if(rot == 0){
		if(sp==0)	run(0);
		else		run(computeAll(sp));
	} else {
		resetForGyrate();
		lastTime=millis();
		while(can_run)	gyrate(sp, rot);
		lastT=millis();
		reset();
		stop_vel(sp);
	}
}

void DC_motor_controller::resetForGyrate(){
	deltaT=0; lastT=millis(); Pulses=0; pulses[1]=0; I=0; D=0; lastError=0; lastTime=millis(); rpm=0; deltaTime=0; //lastError = error
	can_run=true; 
	can_accelerate = true;	lastTime_accel = 0;
	pwm = 0; pulses[0] = 0; // Reset the pulses for the PWM counter
	elapsed_stop_time = 0;
	run(0);
}

void DC_motor_controller::reset(){
	resetForGyrate();
}

bool DC_motor_controller::canRun(){
	return can_run;
}

void DC_motor_controller::gyrate(float sp, float rot /*= 0*/){
	if(rot == 0){
		walk(sp, 0);
		can_run = false;
	} else {
		ifNegativeAllNegative(sp, rot);
		long totalPulses=rot*ppr*rr;
		deltaTime = millis() - lastTime;   // De acordo como tempo
		if(deltaTime >= refreshTime){
			cli(); // Desativa todas as interrupções durante o cálculo;
			deltaT=millis()-lastT; // Calcula o tempo decorrido
			Pulses=(deltaT*sp*ppr*rr)/60000.0; // Calcula a quantidade necessária da pulsos
			if(rot>0)   pwm = computePID(pulses[1],Pulses,true);
			else        pwm = computePID(-pulses[1],-Pulses,true);
			lastTime = millis();
			sei(); // Reativa todas as interrupções
		}
		run((rot>0) ? pwm : -pwm);
		if(rot>0){
			can_run = (pulses[1] < totalPulses)? true : false;
		}else{
			can_run = (pulses[1] > totalPulses)? true : false;
		}
	}
}

void DC_motor_controller::stop(unsigned int t /*= 0*/){
	unsigned long lastT_local = millis();
	while((millis() - lastT_local) < t){     		// For the time "t"...
		deltaTime=millis() - lastTime;
		if(deltaTime >= refreshTime){         		// If it's time to compute...
			cli();                              		// Desativa todas as interrupções durante o cálculo;
			pwm = computePID(pulses[1],0, true);
			lastTime = millis();                		// Update lastTime
			sei();                             		// Reativa todas as interrupções
		}
		run(pwm);
	}
	run(0); // Turn off the motor
}


void DC_motor_controller::stop_vel(unsigned int vel /*= 0*/){
  stop(anti_inertia_time(vel));
}

void DC_motor_controller::stop_both(int time /*= 0*/){
	deltaTime=millis() - lastTime;
	if(deltaTime >= refreshTime){         // If it's time to compute...
		cli();                              // Desativa todas as interrupções durante o cálculo;
		pwm = computePID(pulses[1],0, true);
		sei();                              // Reativa todas as interrupções
		lastTime = millis();                // Update lastTime
		elapsed_stop_time += deltaTime;
	}
	can_stop = (elapsed_stop_time < time)? true : false;
	run(pwm);
}

void DC_motor_controller::accelerate(float sp, float accel){
	float time_sec = sp / accel;
	unsigned long last_time_local = millis(), delta_time_local = 0;
	
	ifNegativeAllNegative(sp, accel);
	
	while(delta_time_local < (time_sec*1000)){
		delta_time_local = millis() - last_time_local;
		
		long vel = (delta_time_local/1000.0) * accel;
		
		byte pwm = computeAll(vel);
		
		run(pwm);		
	}
}

float DC_motor_controller::anti_inertia_time(float vel/* = 50*/){
  unsigned int time;

  if(vel < 0) vel = -vel; // Absolute value of vel
  time = vel * inertia_time_coeficient;
  if(time > max_anti_inertia_time) time = max_anti_inertia_time;
  return time;
}

void DC_motor_controller::startCounting(){
	is_counting = true;
	total_rot = 0;
}

void DC_motor_controller::stopCounting(){
	is_counting = false;
}

float DC_motor_controller::getRotations(){
	return total_rot;
}

void DC_motor_controller::ifNegativeAllNegative(float &val_1, float &val_2){
	if ((val_1 < 0) || (val_2 < 0)){ // Garante que os dois valores sejam negativos no caso de um dos valores ser negativo.
  		val_1 = -abs(val_1);
  		val_2 = -abs(val_2);
  	}
}













DC_motor_controller motor;

// pinos da ponte H
#define IN1 6
#define IN2 7
#define EN 8

// pinos do encoder
#define pin_encoder_interrupcao 2 // cada arduino tem pinos próprios para interrupção (checar pinout do arduino utilizado)
#define pin_encoder_secundario 4 // pode ser qualquer outro pino digital

// variáveis de controle interno
#define RR 21 // motor JGA25: 21 // motor maior (JGB37): 30
#define kp 1.4
#define ki 0.9
#define kd 0.05

void interrupt_motor () {   // Função interrupt_motor
  motor.isr();              // Chama o método isr(), que realiza a contagem do pulso
}

void setup() {
  // Pinos usados:
  motor.hBridge(IN1, IN2, EN); // pinos da ponte H
  motor.setEncoderPin(pin_encoder_interrupcao, pin_encoder_secundario); // pinos do encoder (sempre o pino de interrupção vem primeiro)
  attachInterrupt(digitalPinToInterrupt(pin_encoder_interrupcao), interrupt_motor, FALLING); // ativa a interrupção para contagem dos pulsos

  // Dados do motor:
  motor.setRR(RR); // razão da caixa de redução

  // Constantes para o sistema de controle PID:
  motor.setPIDconstants(kp, ki, kd); // constantes do PID

  // Inicialização do motor
  motor.setPins(); // configura os pinos e variáveis passados nas funções anteriores
  motor.stop(); // Para o motor, garante que ele fique desligado.

  Serial.begin(9600);
  Serial.println("test");

  
}

void loop() {
  int velocidade = 50; // velocidade em RPM
  int rotacoes = 1; // rotações
    
    // Método para fazer o motor andar em uma velocidade constante por algumas rotacoes.
    // Para dar rotacoes para trás, ambos os valores (velocidade e rotações) devem ser negativos
    
    motor.walk(100);

    //int tempo_parado = 2500;
    
    //motor.stop(tempo_parado); // método para parar o motor e deixá-lo parado por algum tempo em millisegundos
}