#include <Wire.

h>
#include <Servo.h>
#include <EEPROM.h>
#include <Depecabot3.h>

Depecabot robot;

#define SERVO_INIT_POS 90

// Conjuntos de Entradas //
#define LEJOS_ATRAS 0
#define MEDIO_ATRAS 1
#define CERCA_ATRAS 2
#define CORRECTO 3
#define CERCA_ALANTE 4
#define MEDIO_ALANTE 5
#define LEJOS_ALANTE 6

// Conjuntos de Salida //
#define RETROCESO_RAPIDO -200
#define RETROCESO_MEDIO -30
#define RETROCESO_LENTO -5
#define PARADO 0
#define AVANCE_LENTO 5
#define AVANCE_MEDIO 30
#define AVANCE_RAPIDO 200

#define CONSIGNA 150

unsigned long timerLCD, timerGP2D;

char pos=90;
int distancia=0;
int ERROR=0;
int motor=0;
unsigned char Pertenencias[7]={0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
int Salidas[7]={0,0,0,0,0,0,0};

void Fuzzyfication(int X)
{
char i;

for(i=LEJOS_ATRAS;i<=LEJOS_ALANTE;i++)
Pertenencias[i]=0;

for(i=LEJOS_ATRAS;i<=LEJOS_ALANTE;i++)
{
if(X > (-30+(10*i)) && X <= (-20+(10*i)))
{
Pertenencias[i]=(-10*X+(-200+(100*i)));
Pertenencias[i+1]=(10*X+(300-(100*i)));
}
}

if(X <= -30) Pertenencias[LEJOS_ATRAS]= 100;

if(X >30) Pertenencias[LEJOS_ALANTE]=100;
/*
if(X > -20 && X <= -10){
Pertenencias[MEDIO_ATRAS]=(-10*X-100);
Pertenencias[CERCA_ATRAS]=(10*X+200);
}
 if(X > -10 && X <= 0){
Pertenencias[CERCA_ATRAS]=(-10*X);
Pertenencias[CORRECTO]=(10*X+100);
}
if(X > 0 && X <= 10){
Pertenencias[CORRECTO]=(-10*X+100);
Pertenencias[CERCA_ALANTE]=(10*X);
}
if(X > 10 && X <= 20){
Pertenencias[CERCA_ALANTE]=(-10*X+200);
Pertenencias[MEDIO_ALANTE]=(10*X-100);
}
if(X > 20 && X <= 30){
Pertenencias[CERCA_ALANTE]=(-10*X+300);
Pertenencias[LEJOS_ALANTE]=(10*X-200);
}
*/

int DeFuzzyfication()
{
char i;
long sumatorio_salidas=0, sumatorio_activacion=0;

// Activaciion de cadaregla, por su consecuente

Salidas[0]=Pertenencias[LEJOS_ATRAS]*RETROCESO_RAPIDO;
Salidas[1]=Pertenencias[MEDIO_ATRAS]*RETROCESO_MEDIO;
Salidas[2]=Pertenencias[CERCA_ATRAS]*RETROCESO_LENTO;
Salidas[3]=Pertenencias[CORRECTO]*PARADO;
Salidas[4]=Pertenencias[CERCA_ALANTE]*AVANCE_LENTO;
Salidas[5]=Pertenencias[MEDIO_ALANTE]*AVANCE_MEDIO;
Salidas[6]=Pertenencias[LEJOS_ALANTE ]*AVANCE_RAPIDO;

for(i=0;i<7;i++)
{
sumatorio_salidas+=Salidas[i];
sumatorio_activacion+=Pertenencias[i];
}

return (sumatorio_salidas/sumatorio_activacion);
}

void setup()
{
robot.init ();
robot.servo1.attach(SERVO); //Inicializacion servo
robot.servo1.write(SERVO_INIT_POS);
robot.lcd.clear();
}

void loop()
{
robot.lcd.setCursor(0,0);
if(millis()-timerLCD>200)
{
 timerLCD=millis();
robot.lcd.clear();
robot.lcd.setCursor(10,0);
robot.lcd.print(distancia);
robot.lcd.setCursor(40,0);
robot.lcd.print(motor);

robot.lcd.setCursor(0,2);
robot.lcd.print(Pertenencias[LEJOS_ATRAS]);
robot.lcd.setCursor(22,2);
robot.lcd.print(Pertenencias[MEDIO_ATRAS]);
robot.lcd.setCursor(44,2);
robot.lcd.print(Pertenencias[CERCA_ATRAS]);
robot.lcd.setCursor(66,2);
robot.lcd.print(Pertenencias[CORRECTO]);
robot.lcd.setCursor(0,4);
robot.lcd.print(Pertenencias[CERCA_ALANTE]);
robot.lcd.setCursor(22,4);
robot.lcd.print(Pertenencias[MEDIO_ALANTE]);
robot.lcd.setCursor(44,4);
robot.lcd.print(Pertenencias[LEJOS_ALANTE]);

}
if(millis()-timerGP2D>2)
{
timerGP2D=millis();
distancia=robot.gp2dmm(GP2);
ERROR=CONSIGNA-distancia;
Fuzzyfication(ERROR);
motor=(DeFuzzyfication())*-1;
robot.motores(motor,motor);
}
}