#include <p18cxxx.h>
#include <delays.h>
#include <stdlib.h>
#include <stdio.h>

#pragma config OSC=HS
#pragma config WDT=OFF
#pragma config LVP=OFF
#pragma config PBADEN=OFF

/* A robot moving toward the maximum light region.
Four light detectors connected to four A/D channels :
Front detector : AN0
Rear detector : AN1
Left detector : AN2
Right detector : AN3

Motors are controlled by the two PWM sources : 
Left motor : RC3 (direction) RC2/CCP1 (PWM)
Right motor : RC4 (direction) RC1/CCP2 (PWM)

Coding wheel on RB0-2

 Proximity sensors on RC0,RC5,RC6,RC7 
 RC0 : Front Right
 RC5 : Back Left
 RC6 : Back Right
 RC7 : Front Left

Algorithm : 
	Standard Q-Learning, with optimistic initial values
	
	A coding wheel to enable at least four behaviours: 
	000) Learn from scratch and don't store in EEPROM
	001) Learn from scratch and store in EEPROM
	010) Initiate learning by reading values from EEPROM at the beginning, but don't store new values
	011) Same as 3, but store values in EEPROM
	1XX) Reflex behaviour (no learning)

*/

#define true 1
#define false 0


typedef unsigned char boolean;
typedef struct {
	unsigned char light[4]; // Light sensors
	unsigned char front;	// front proxy sensors
	unsigned char rear; 	// back proxy sensors
	}state;

/* The Q(s,a) values */
#pragma idata bigdata
double qsa[384]; 
#pragma idata
double alpha=0.01; 
double gamma=0.9; 
double *q_ptr=&qsa[0];
int rank=10;
int rank2=0; 
boolean storeEEPROM=false;
boolean readEEPROM=false; 
boolean reflex=false;

void writeDoubleToEEPROM(int address){
		int i;
	int pro=2*address; 
	/* This assumes that qsa[i] will always be between 0 and 10 */
	double p=qsa[address]*6553.5; 
	unsigned int pp=(int)p;
	char *ptr=&pp;
 	for(i=0;i<2;i++){	
	EECON1bits.EEPGD=0;
	EECON1bits.CFGS=0;
	EECON1bits.WREN=1;
	EEADRH=pro/256;
	EEADR=pro%256; 
	EEDATA=*ptr;
	EECON2=0x55;
	EECON2=0xAA;
	EECON1bits.WR=1;
	while(!PIR2bits.EEIF);
	PIR2bits.EEIF=0;
	pro++;
	ptr++;
	}/*for*/
	}
	
	
	
double readDoubleInEEPROM(int address){
	int i;
	int pro=2*address; 
	unsigned int dest; 
	char *ptr=&dest;
 	for(i=0;i<2;i++){	
	EECON1bits.EEPGD=0;
	EECON1bits.CFGS=0;
	EEADRH=pro/256;
	EEADR=pro%256; 
	EECON1bits.RD=1;
	pro++;
	*ptr=EEDATA;
	ptr++;
	}/*for*/
	return dest/6553.5;
	}


int stateToInt(state st){
	unsigned char *s=st.light; 
	int sum=6*s[0];
	int pro; 
	if((s[1]>s[2])&&(s[1]>s[3])) sum+=4; 
	else{
		if((s[1]<s[2])&&(s[1]<s[3])) sum+=0;
		else sum+=2;
		}
	if(s[2]>s[3]) sum+=1;
	pro=sum;
	if(st.rear) pro+=96; 
	if(st.front) pro+=192; 
	/*pro=(2*st.front+st.rear)*96+sum;*/
	return (2*st.front+st.rear)*96+sum;
	}

void learn(state s1,state s2,unsigned char action,double reward){
	int i,j;
	int val1=stateToInt(s1); 
	int val2=stateToInt(s2); 
	/* find max_{a'}(q(s',a') */
    double max=q_ptr[val2]; 
    for(i=1;i<4;i++)
	    if(q_ptr[val2+24*i]>max) max=q_ptr[val2+24*i];
	q_ptr[val1+24*action]=q_ptr[val1+24*action]+alpha*(reward+gamma*max-q_ptr[val1+24*action]); 	
	alpha*=0.999999;
	if(storeEEPROM){
		if(val1+24*action>192){
				j=j;
				Nop();
				}
		writeDoubleToEEPROM(val1+24*action);
		}
	}/* learn */
	
unsigned char  chooseAction(state s1){
	int val=stateToInt(s1); 
	int i=0;
	unsigned char  bestAction=0;
	double bestValue=q_ptr[val]; 
	for(i=1;i<4;i++){
		if((q_ptr[val+24*i]>bestValue)||
			((q_ptr[val+24*i]==bestValue)&&(rand()%2)))
				{
			bestValue=q_ptr[val+24*i]; 
			bestAction=i;
			}
		}/* for */
	if((rand()%rank)==0) return (unsigned char)(rand()%4); 
	else return bestAction;	
	}/* choose action */
/* Initialize ports (A,C), pwm, motors (still/brake) */

void init(void){
	/* PWM inits */
	T2CON = 0x7F;        /* postscale 1:1, Timer2 ON, prescaler 16 */
	TRISC = 0; 			 /* RC connected to motors (direction, PWM)*/	
   	PR2 = 0xFF;          /* initialize the PWM period */
  	CCPR1L = 0x00;     
  	CCPR2L = 0x00; 
  	PORTCbits.RC3 = 0;    /* Direction motor 1 */
  	PORTCbits.RC4 = 0;    /* Direction motor 2 */ 
  	CCP1CON = 0xFF;       /* turn the PWM on */
  	CCP2CON = 0xFF; 
  	/* A/D inits */
  	TRISA=0x0F; 		  /* RA0-RA4 analog entries */
  	ADCON2=0x6E;          /* See datasheet */
  	ADCON1=0xCB;          /*Four analog channels */
  	TRISB=TRISB&0xFF;	  /* Coding wheel on RB0-2 (8 possibilities)*/
  	INTCON2=INTCON2&0x7F; /* pull ups on portB */
  	TRISC=0xE1; 		  /* Proximity sensors */
  	return;
  	 }
 
 void forward(void){
	 CCPR1L=0;
	 CCPR2L=255;
	 PORTCbits.RC3=1; 
	 PORTCbits.RC4=0;
	 } 
	 
 void backward(void){
	 CCPR1L=255;
	 CCPR2L=0;
	 PORTCbits.RC3=0; 
	 PORTCbits.RC4=1;
	 } 	 
	 
  void left(void){
	 CCPR1L=255;
	 CCPR2L=255;
	 PORTCbits.RC3=0; 
	 PORTCbits.RC4=0;
	 }
	  	 
  void right(void){
	 CCPR1L=0;
	 CCPR2L=0;
	 PORTCbits.RC3=1; 
	 PORTCbits.RC4=1;
	 } 	 
void main(void){
		/* The four values read from analog ports */
		unsigned char analogRead[4]; 
		unsigned char chosenAction; 
	
		/* States */
		state s1,s2;
		unsigned char val[4]={0xC0,0xC4,0xC8,0xCC};
		int i,j,temp,k,l,m;
		double reward;
		double prov;
		double oldReward=1024;
		double normReward;
		init(); 
		storeEEPROM=(PORTBbits.RB0==0);
		readEEPROM=(PORTBbits.RB1==0);
		reflex=(PORTBbits.RB2==0);
		srand(TMR0L);
		stdout=_H_USER;/* debug */
		/* Uncomment and execute once the following block to fill the EEPROM with 10.0 */
/*	for(i=0;i<384;i++) {
				qsa[i]=10.0;  
				writeDoubleToEEPROM(i);
				} */
		if(!reflex){
		if(!readEEPROM){
			for(i=0;i<384;i++) {
				qsa[i]=10.0; 
				}
		}
		else{
			for(i=0;i<384;i++){
				qsa[i]=readDoubleInEEPROM(i); 
				}
			}
		}
		while(1){
			/* read the four analog channels */
			reward=0; 
			for(i=0;i<4;i++){
				ADCON0=0xC0|val[i];	
				ADCON0bits.ADON=1; 
				ADCON0bits.GO=1;
				while(ADCON0bits.GO!=0) {}
				analogRead[i]=ADRESH;
				reward+=ADRESH;
				}
		if(!reflex){
		if(reward>oldReward) normReward=0.0; 
		else {normReward=1;
			if(chosenAction>1) normReward=0.5; /* penalty when turning */
		}
		oldReward=reward;
		}
		for(i=0;i<4;i++) {s2.light[i]=s1.light[i];s1.light[i]=i;}
		s2.front=s1.front;
		s2.rear=s1.rear;
		/* s2=s1 ????*/
		/* Ordering analog inputs */		
		for(i=0;i<3;i++){
			for(j=i+1;j<4;j++){
				if(analogRead[i]>analogRead[j]){
					temp=analogRead[j];
					analogRead[j]=analogRead[i]; 
					analogRead[i]=temp;
					temp=s1.light[j]; 
					s1.light[j]=s1.light[i];
					s1.light[i]=temp;
					} /* if */
				}/* for j */
			}/* for i */
		/* Proximity sensors */	
		if((PORTCbits.RC0==0)||(PORTCbits.RC7==0)) s1.front=1; 
		else s1.front=0;
		if((PORTCbits.RC5==0)||(PORTCbits.RC6==0)) s1.rear=1; 
		else s1.rear=0;
	if(!reflex){	
	learn(s2,
	s1,
	chosenAction,
	normReward); 
	chosenAction=chooseAction(s1);
	 /* Moving in the right direction */
	 switch(chosenAction){
		 case 0 : forward(); break; 
		 case 1 : backward(); break; 
		 case 2 : right(); break; 
		 case 3 : left(); break; 
		 default : forward(); 
		 }
   Delay10KTCYx(120);	
   rank2++;
   if(rank2==1000){
	   rank2=0;
	   rank++;
	   }
	} /* Not reflex */
	else{
	switch(s1.light[0]){
		 case 0 : forward(); break; 
		 case 1 : backward(); break; 
		 case 2 : right(); break; 
		 case 3 : left(); break; 
		 default : forward(); 
		 }			
		}/* reflex*/
   } /* while (1) */
}