#include <pic.h>

#define SPACE   0x20
#define LF      0x0A
#define CR      0x0D

#define PAIN_DECAY1     31
#define PAIN_DECAY2     32
#define TIMER_PAIN_CONST    40

#define BACK_FAST       -255
#define BACK_SLOW       -128
#define STOP            0
#define FORWARD_SLOW    128
#define FORWARD_FAST    255

#define min(a, b)   (((a) < (b)) ? (a) : (b))
#define max(a, b)   (((a) > (b)) ? (a) : (b))
#define abs(a)      (((a) < 0) ? -(a) : (a))
#define sgn(x)      (((x) < 0) ? -1 : (((x) == 0) ? 0 : 1))

////////////////////// DEKLARACIJE FUNKCIJA //////////////////////

// domene jezicnih varijabli
unsigned char delta_LEFT(int x);
unsigned char delta_CENTER(int x);
unsigned char delta_RIGHT(int x);
unsigned char pain_LOW(int x);
unsigned char pain_MEDIUM(int x);
unsigned char pain_HIGH(int x);
unsigned char motor_BACK_FAST(int x);
unsigned char motor_BACK_SLOW(int x);
unsigned char motor_STOP(int x);
unsigned char motor_FORWARD_SLOW(int x);
unsigned char motor_FORWARD_FAST(int x);

// proracun razina aktivacije pravila
void update_activation();

// defuzifikacija motora
void update_motor();

// vremensko ponasanje varijable pain
void update_pain();

// odredjivanje smjera svjetla
void update_delta();

// a/d konverzija
unsigned char adc(unsigned char channel);

// inic. timera i portova
void init_timer();
void init_ports();

// ispis varijabli na ser. port
void init_serial();
void send(char data);
void send_int(int data);
void send_char(unsigned char data);
void report_variables();



////////////////////// GLOBALNE VARIJABLE //////////////////////
int delta = 0;
int pain_left = 0;
int pain_right = 0;
int motor_left = 0;
int motor_right = 0;

// timer i stepper
const char seq_left[4] = { 0x0A, 0x0C, 0x05, 0x03 };
const char seq_right[4] = { 0x90, 0xC0, 0x60, 0x30 };
char timer_left = 1;
char timer_right = 1;
char timer_pain = 1;
char step_left = 0;
char step_right = 0;

// pravila:
//
// 1. if pain_left = LOW and pain_right = LOW and delta = CENTER
//      then motor_left = FORWARD_FAST and motor_right = FORWARD_FAST;
//
// 2. if pain_left = LOW and pain_right = LOW and delta = LEFT
//      then motor_left = BACK_FAST and motor_right = FORWARD_FAST;
//
// 3. if pain_left = LOW and pain_right = LOW and delta = RIGHT
//      then motor_left = FORWARD_FAST and motor_right = BACK_FAST;
//
// 4. if pain_left = MEDIUM or pain_right = MEDIUM
//      then motor_left = FORWARD_FAST and motor_right = FORWARD_FAST;
//
// 5. if pain_left = HIGH
//      then motor_left = BACK_SLOW & motor_right = BACK_FAST;
//
// 6. if pain_right = HIGH
//      then motor_left = BACK_FAST & motor_right = BACK_SLOW;

// razine aktivacije pravila
unsigned char rule1 = 0;
unsigned char rule2 = 0;
unsigned char rule3 = 0;
unsigned char rule4 = 0;
unsigned char rule5 = 0;
unsigned char rule6 = 0;



////////////////////// GLAVNI PROGRAM //////////////////////
main() {
    init_ports();
    // init_serial();
    init_timer();
    
    for (;;) {
        update_delta();
        update_activation();
        update_motor();
        // report_variables();
    }
}

////////////////////// INTERRUPT RUTINA //////////////////////
void interrupt isr() {
    char dir;
    unsigned char seq;

    timer_pain--;
    if (timer_pain == 0) {
        timer_pain = TIMER_PAIN_CONST;
        update_pain();
    }
    
    timer_left--;
    if (timer_left == 0) {
        timer_left = 11 - (abs(motor_left) / 32);
        dir = sgn(motor_left);
        
        seq = PORTD;
        seq &= 0xF0;
        if (dir != 0) {
            seq |= seq_left[step_left];
   		    step_left = (step_left + dir) & 0x03;        
        }
        PORTD = seq;
    }
    
    timer_right--;
    if (timer_right == 0) {
        timer_right = 11 - (abs(motor_right) / 32);
        dir = sgn(motor_right);
        
        seq = PORTD;
        seq &= 0x0F;
        if (dir != 0) {
            seq |= seq_right[step_right];
       		step_right = (step_right + dir) & 0x03;
        }
        PORTD = seq;
    }
    
    T0IF = 0;
    T0IE = 1;
}


////////////////////// DEFINICIJE FUNKCIJA //////////////////////

unsigned char delta_LEFT(int x) {
    if (x <= -96) {
        return 255;
    } else if (x < -32) {
        return -x * 4 - 128;    
    } else {
        return 0;
    }
}

unsigned char delta_CENTER(int x) {
    if (x < -64) {
        return 0;
    } else if (x < 0) {
        return x * 4 + 256;
    } else if (x == 0) {
        return 255;
    } else if (x < 64) {
        return 256 - x * 4;
    } else {
        return 0;
    }
}

unsigned char delta_RIGHT(int x) {
    if (x < 32) {
        return 0;
    } else if (x < 96) {
        return x * 4 - 128;
    } else {
        return 255;
    }
}

unsigned char pain_LOW(int x) {
    if (x <= 64) {
        return 255;
    } else if (x < 96) {
        return 768 - x * 8;
    } else {
        return 0;
    }
}

unsigned char pain_MEDIUM(int x) {
    if (x < 64) {
        return 0;
    } else if (x < 128) {
        return x * 4 - 256;
    } else if (x == 128) {
        return 255;
    } else if (x < 192) {
        return 768 - x * 4;
    } else {
        return 0;
    }
}

unsigned char pain_HIGH(int x) {
    if (x < 160) {
        return 0;
    } else if (x < 192) {
        return x * 8 - 1280;
    } else {
        return 255;
    }
}

unsigned char motor_BACK_FAST(int x) {
    if (x <= -192) {
        return 255;
    } else if (x < -128) {
        return -x * 4 - 512;        
    } else {
        return 0;
    }
}

unsigned char motor_BACK_SLOW(int x) {
    if (x < -192) {
        return 0;
    } else if (x < -128) {
        return x * 4 + 768;
    } else if (x == -128) {
        return 255;
    } else if (x < -64) {
        return -x * 4 - 256;
    } else {
        return 0;
    }
}

unsigned char motor_STOP(int x) {
    if (x < -64) {
        return 0;
    } else if (x < 0) {
        return x * 4 + 256;        
    } else if (x == 0) {
        return 255;
    } else if (x < 64) {
        return -x * 4 + 256;
    } else {
        return 0;
    }
}

unsigned char motor_FORWARD_SLOW(int x) {
    if (x < 64) {
        return 0;
    } else if (x < 128) {
        return x * 4 - 256;
    } else if (x == 128) {
        return 255;
    } else if (x < 192) {
        return -x * 4 + 768;
    } else {
        return 0;
    }
}

unsigned char motor_FORWARD_FAST(int x) {
    if (x < 128) {
        return 0;
    } else if (x < 192) {
        return x * 4 - 512;
    } else {
        return 0;
    }
}

void update_activation() {
    rule1 = min(min(pain_LOW(pain_left), pain_LOW(pain_right)), delta_CENTER(delta));
    rule2 = min(min(pain_LOW(pain_left), pain_LOW(pain_right)), delta_LEFT(delta));
    rule3 = min(min(pain_LOW(pain_left), pain_LOW(pain_right)), delta_RIGHT(delta));
    rule4 = max(pain_MEDIUM(pain_left), pain_MEDIUM(pain_right));
    rule5 = pain_HIGH(pain_left);
    rule6 = pain_HIGH(pain_right);
}


/*
////////////////////// MAMDANI ZAKLJUCIVANJE //////////////////////
void update_motor() {
    int x;
    unsigned char m, m1, m2, m3, m4, m5, m6;
    long sum, div;
    
    // za motor_left
    sum = 0;
    div = 0;
    for (x = -256; x < 256; x += 4) {
        m1 = min(rule1, motor_FORWARD_FAST(x));
        m2 = min(rule2, motor_BACK_FAST(x));
        m3 = min(rule3, motor_FORWARD_FAST(x));
        m4 = min(rule4, motor_FORWARD_FAST(x));
        m5 = min(rule5, motor_BACK_SLOW(x));
        m6 = min(rule6, motor_BACK_FAST(x));
        
        m = max(max(max(max(max(m1, m2), m3), m4), m5), m6);
        div += m;
        sum += m * x;
    }
    motor_left = sum / div;
    
    // za motor_right
    sum = 0;
    div = 0;
    for (x = -256; x < 256; x += 4) {
        m1 = min(rule1, motor_FORWARD_FAST(x));
        m2 = min(rule2, motor_FORWARD_FAST(x));
        m3 = min(rule3, motor_BACK_FAST(x));
        m4 = min(rule4, motor_FORWARD_FAST(x));
        m5 = min(rule5, motor_BACK_FAST(x));
        m6 = min(rule6, motor_BACK_SLOW(x));
        
        m = max(max(max(max(max(m1, m2), m3), m4), m5), m6);
        div += m;
        sum += m * x;
    }
    motor_right = sum / div;
}
*/


/*
////////////////////// LARSENOVO ZAKLJUCIVANJE //////////////////////
void update_motor() {
    int x;
    char m, m1, m2, m3, m4, m5, m6;
    long sum, div;
    
    // za motor_left
    sum = 0;
    div = 0;
    for (x = -256; x < 256; x += 4) {
        m1 = (int) rule1 * motor_FORWARD_FAST(x) / 256;
        m2 = (int) rule2 * motor_BACK_FAST(x) / 256;
        m3 = (int) rule3 * motor_FORWARD_FAST(x) / 256;
        m4 = (int) rule4 * motor_FORWARD_FAST(x) / 256;
        m5 = (int) rule5 * motor_BACK_SLOW(x) / 256;
        m6 = (int) rule6 * motor_BACK_FAST(x) / 256;
        
        m = max(max(max(max(max(m1, m2), m3), m4), m5), m6);
        div += m;
        sum += m * x;
    }
    motor_left = sum / div;
    
    // za motor_right
    sum = 0;
    div = 0;
    for (x = -256; x < 256; x += 4) {
        m1 = (int) rule1 * motor_FORWARD_FAST(x) / 256;
        m2 = (int) rule2 * motor_FORWARD_FAST(x) / 256;
        m3 = (int) rule3 * motor_BACK_FAST(x) / 256;
        m4 = (int) rule4 * motor_FORWARD_FAST(x) / 256;
        m5 = (int) rule5 * motor_BACK_FAST(x) / 256;
        m6 = (int) rule6 * motor_BACK_SLOW(x) / 256;
        
        m = max(max(max(max(max(m1, m2), m3), m4), m5), m6);
        div += m;
        sum += m * x;
    }
    motor_right = sum / div;
}
*/

////////////////////// TEZINSKA SREDNJA VRIJEDNOST //////////////////////
void update_motor() {
    float s1, s2, s3, s4, s5, s6;
    float div = (float) rule1 + (float) rule2 + (float) rule3 + (float) rule4 + (float) rule5 + (float) rule6;
    
    s1 = (float) rule1 * FORWARD_FAST;
    s2 = (float) rule2 * BACK_FAST;
    s3 = (float) rule3 * FORWARD_FAST;
    s4 = (float) rule4 * FORWARD_FAST;
    s5 = (float) rule5 * BACK_SLOW;
    s6 = (float) rule6 * BACK_FAST;
          
    motor_left = (int) ((s1 + s2 + s3 + s4 + s5 + s6) / div);
        
    s1 = (float) rule1 * FORWARD_FAST;
    s2 = (float) rule2 * FORWARD_FAST;
    s3 = (float) rule3 * BACK_FAST;
    s4 = (float) rule4 * FORWARD_FAST;
    s5 = (float) rule5 * BACK_FAST;
    s6 = (float) rule6 * BACK_SLOW;
          
    motor_right = (int) ((s1 + s2 + s3 + s4 + s5 + s6) / div);
}

void update_pain() {
    if (!RB6) {
        pain_left = 256;
    } else {
        pain_left = pain_left * PAIN_DECAY1 / PAIN_DECAY2;
    }
    
    if (!RB7) {
        pain_right = 256;
    } else {
        pain_right = pain_right * PAIN_DECAY1 / PAIN_DECAY2;
    }
    
}

void update_delta() {
    int left = adc(0);
    int right = adc(2);
    
    delta = (left - right) * 4;
}


////////////////////// PRISTUP PERIFERIJAMA //////////////////////

unsigned char adc(unsigned char channel) {
	ADCON1 = 0;
	ADCON0 =  (channel << 3) | 0x81;
	ADGO = 1;
	while (ADGO);
	
	return ADRESH;
}

void init_timer() {
    OPTION &= 0xC0;
    OPTION |= 0x05; // prescaler 1:64
    TMR0 = 1;
	
	T0IE = 1;		// uklj. timer interrupt
	PEIE = 1;       // uklj. periph. interrupte
	GIE = 1;		// uklj. sve interrupte
}

void init_ports() {
	TRISB = 0xFF;	// port B ulazni
	RBPU = 0;		// port B weak pullup
	
	PORTD = 0;		// postavi port D u 0
	TRISD = 0x00;	// port D - donja 4 bita izlazna
}

void init_serial() {
	SPBRG = 0x0A;	// 115.2 kbps
	TXSTA = 0x26;	// 8 data bits, async transmit	
	RCSTA = 0x90;
}

void send(char data) {
	while (!TXIF);
	TXREG = data;
}

void send_int(int data) {
    int i;
    
    if (data < 0) {
        send('-');
        data = -data;
    } else {
        send(' ');
    }
    
    for (i = 10000; i != 0; i /= 10) {
        send(data / i + '0');
        data %= i;
    }
}

void send_unsigned_char(unsigned char data) {
    int i;
    
    for (i = 100; i != 0; i /= 10) {
        send(data / i + '0');
        data %= i;
    }
}

void report_variables() {
    send_int(delta);
    send(SPACE);
    send_int(pain_left);
    send(SPACE);
    send_int(pain_right);
    send(SPACE);
    send_int(motor_left);
    send(SPACE);
    send_int(motor_right);
    send(SPACE);
    send_unsigned_char(rule1);
    send(SPACE);
    send_unsigned_char(rule2);
    send(SPACE);
    send_unsigned_char(rule3);
    send(SPACE);
    send_unsigned_char(rule4);
    send(SPACE);
    send_unsigned_char(rule5);
    send(SPACE);
    send_unsigned_char(rule6);
    send(CR);
    send(LF);
}