#include "config.h"
#include "serial.c"
#include "serio.c"
#include "i2cmsu.c"
#include "delay.h"

// Robot Code



volatile unsigned int last_capture, this_capture;
volatile unsigned long delta;
volatile unsigned char tmr1_ov;  // timer 1 overflow cnt
#define MAXBYTES 32
volatile unsigned char cbuff[MAXBYTES]; 
volatile unsigned char bitcount, bytecount,bit_edge;
volatile unsigned char state,edge_capture,current_bit;
volatile unsigned char this_byte, cmd,last_cmd, mode;


#define IDLE_TIME 4
#define BITCHANGE   10000
#define IDLE         0
#define START_PULSE  1
#define BIT_CAPTURE  2
#define IO_FINISH    3



#define PWM_MAX 10  // # of timer1 interrupts for one PWM period
#define OP_LEFT 5   // left wheel port bit
#define OP_RIGHT 6  // right wheel port bit
#define OP_LDIR 3   // left wheel direction port bit
#define OP_RDIR 4   // right wheel direction port bit

void do_ircap(void);

//Manual PWM control variables
unsigned char dc_left,dc_right, period_left, period_right;

// use timer3, CCPR2 for PWM generation
#define MATCH_INTERVAL 0x0800
unsigned int match;
unsigned char adc_val, old_adc_val;

#if defined(HI_TECH_C)
void interrupt isr(void)
#endif
#if defined(__18CXX)
#pragma interrupt isr
void isr(void)
#endif
{
  if (CCP2IF) {
    CCP2IF = 0;
    adc_val = ADRESH;  // get AD value
    GODONE = 1;  // new conversion
    // don't clear timer1, change compare register
    match = match + MATCH_INTERVAL;
    CCPR2H = match >> 8;
    CCPR2L = match & 0xFF;
    CCP2IF = 0;
    period_left++;
    if (period_left == PWM_MAX) {
      period_left = 0;
      if (!dc_left) bitclr(PORTB,OP_LEFT);
	else bitset(PORTB,OP_LEFT);
    } else if (period_left == dc_left) {
      bitclr(PORTB,OP_LEFT);
      old_adc_val = adc_val;
    }
    period_right++;
    if (period_right == PWM_MAX) {
      period_right = 0;
      if (!dc_right) bitclr(PORTB,OP_RIGHT);
	else bitset(PORTB,OP_RIGHT);
    } else if (period_right == dc_right) {
      bitclr(PORTB,OP_RIGHT);
    }
  }
  if (TMR1IF) {
    tmr1_ov++;  // increment timer1 overflow
  }
  if (CCP1IF) {
    // read CCPR1 as 16-bit value
    this_capture = CCPR1;
    if (!tmr1_ov) { 
      // no overflow at all
      delta = this_capture - last_capture ;
    } else {
      delta = tmr1_ov - 1;
      // delta valid from 0x000000 to 0x07FFFFF
      delta = (delta << 16);
      last_capture = 0 - last_capture;  // time to overflow
      delta = delta + last_capture;
      delta = delta + this_capture;
    }
    last_capture = this_capture;
    tmr1_ov = 0;  // clear timer 1 overflow count
    if (CCP1CON & 0x01)  {//test LSb
      CCP1CON = 0x0; //reset first
      CCP1CON = 0x4; //falling edge
    }
    else {
      CCP1CON = 0x0; //reset first 
      CCP1CON = 0x5; // rising edge
    }
    edge_capture = 1;
  }
  if (TMR1IF || CCP1IF)  do_ircap();
}

// decode IO pulses
void do_ircap(void){
  TMR1IF = 0; CCP1IF = 0;
  switch (state) {
  case IDLE: 
    // wait for line to become idle
    if (tmr1_ov > IDLE_TIME){
      tmr1_ov = 0;
      state = START_PULSE;
      edge_capture = 0;
    }
    break;
  case START_PULSE: 
    if (edge_capture) { // wait for edge
      edge_capture = 0;
      bit_edge = 0;
      state = BIT_CAPTURE;
    }
    break;
  case BIT_CAPTURE: 
    // wait for edge or idle condition
    if (tmr1_ov > 1) {
      // finished
      CCP1IE = 0;   // disable interrupts
      TMR1IE = 0;   // allow timer 1 interrupts
      TMR1ON = 0;   // disable timer 1
      state = IO_FINISH;
    } else if (edge_capture) {
      edge_capture = 0;
      //accumulating bits, LSB to MSB 
      if ((delta > BITCHANGE) || bit_edge) {
	if (delta > BITCHANGE) {
	  // toggle current bit if wide pulse
	  current_bit = ~current_bit;
	}
	if (current_bit) this_byte = this_byte | 0x01; //set LSb
	bitcount++;
	bit_edge = 0; // next edge is not a bit
	if (bitcount == 8) {
	  bitcount = 0;
	  cbuff[bytecount] = this_byte;
	  bytecount++;
	  this_byte = 0;
	} else{
	  this_byte = this_byte << 1;
	}
      } else if (!bit_edge) bit_edge = 1;
    }
    break;
  }
}

void reset_ir(void) {
    state = IDLE;    // look for idle
    CCP1CON = 0;  // turn off when changing modes
    CCP1CON = 0x4; // capture every falling edge 
    current_bit = ~(0);
    bitcount = 0;
    bytecount = 0;
    tmr1_ov = 0;  // clear timer 1 overflow count
    last_capture = 0;
    // enable interrupts 
    CCP1IF = 0;   // clear CCP1IF interupt flag 
    CCP1IE = 1;   // capture interrupt enable 
    TMR1IF = 0;   // clear timer 1 interrupt flag
    TMR1IE = 1;   // allow timer 1 interrupts
    TMR1ON = 1;   // enable timer 1
}


unsigned char i;

// convert last byte to philip format
// 0x32 - rewind
// 0x35 - play
// 0x34 - FF
// 0x37 - rec
// 0x36 - stop
// 0x29 - pause

// on radio shack universal remote code 333 VCR
// 0x0 - 0x9  buttons 0-9.
#define STOP       0x36  // stop (stop)
#define SPEEDUP    0x29  // speedup (pause)
#define SLOWDOWN   0x37  // slowdown (rec)
#define GOFORWARD  0x35  // forward (play)
#define TURNLEFT   0x32  // turn left (rewind)
#define TURNRIGHT  0x34  // turn right (fast fwd)
#define REVERSE    0x05  // reverse (#5)
#define SPIN       0x09  // spin (#9)
#define MODESWAP   0x01  // man/auto tgl (#1)

#define MAN_DRIVE  0x00
#define AUTO_DRIVE 0x01

void philips_convert(void){
  i = cbuff[0] ; // last full byte
  if (i & 0x01) bitset(cmd,6);
  else  bitclr(cmd,6);
  cmd = cmd >> 1;
}

void Delay_tens(char k){
  while(k){
    DelayMs(100);
    k--;
  }
}

void all_stop(void) {
  while(dc_left || dc_right) {
    if (dc_left) dc_left--;
    if (dc_right)dc_right--;
    DelayMs(50);
  }
  Delay_tens(2);
}

void dir_fwd(void) {
  bitclr(PORTB,OP_RDIR); bitclr(PORTB,OP_LDIR);
}

void dir_back(void) {
  bitset(PORTB,OP_RDIR); bitset(PORTB,OP_LDIR);
}

void do_speedup (char target) {
  dc_left = 0; dc_right = 0;
  while (dc_left != target) {
    dc_left++;dc_right++;
    DelayMs(75);
  }
}

void do_turn (char ltarg,char rtarg) {
  dc_left = 0;  dc_right = 0;
  while ((dc_left != ltarg) || (dc_right != rtarg)) {
    if (dc_left != ltarg) dc_left++;
    if (dc_right != rtarg) dc_right++;
    DelayMs(75);
  }
}

#define BLOCKAGE_THRESHOLD 60
#define CLEAR_THRESHOLD 40
#define FWD_SPEED 6
#define BACK_SPEED 9

void handle_cmd(void){
  cmd = this_byte;
  philips_convert();
  if (last_cmd != cmd) {
    last_cmd = cmd;
    return;
  }
  last_cmd = 0;
  cbuff[0] =0 ;  cbuff[1] =0 ;
  switch (cmd) {
  case STOP: 
    all_stop();
    break;
  case SPEEDUP: 
    if (dc_left < PWM_MAX)dc_left += 2;
    if (dc_right < PWM_MAX)dc_right += 2;
    break;
  case SLOWDOWN: 
    if (dc_left) dc_left -= 2;
    if (dc_right)dc_right -= 2;
    break;
  case GOFORWARD: 
    bitclr(PORTB,OP_RDIR);
    bitclr(PORTB,OP_LDIR);
    if (dc_left > dc_right) dc_right = dc_left;
    else dc_left = dc_right;
    break;
  case TURNLEFT: 
    if (dc_left) dc_left -= 2;
    break;
  case TURNRIGHT: 
    if (dc_right) dc_right -= 2;
    break;
  case REVERSE: 
    dc_left = 0; dc_right = 0;
    DelayMs(200); DelayMs(200);
    bitset(PORTB,OP_RDIR);
    bitset(PORTB,OP_LDIR);
    dc_right = FWD_SPEED; dc_left = FWD_SPEED;
    break;
  case SPIN: 
    dc_left = 0; dc_right = 0;
    DelayMs(100); DelayMs(100);
    DelayMs(100); DelayMs(100);
    bitset(PORTB,OP_RDIR);
    dc_right = FWD_SPEED;dc_left = FWD_SPEED;
    break;
  case MODESWAP:
    if (mode == AUTO_DRIVE) mode = MAN_DRIVE;
    else mode = AUTO_DRIVE;
    all_stop();
    dir_fwd();
    break;
  }
}


void manual_drive(void){
  while(mode == MAN_DRIVE) {
    reset_ir(); 
    // wait for IR data to arrive
    while (state != IO_FINISH);
    handle_cmd();
  }
}


void auto_drive(void){
  reset_ir(); 
  while(mode == AUTO_DRIVE) {
    if (state == IO_FINISH) {
      // if IR arrives, execute it
      handle_cmd();
      reset_ir();
    } else {
      // auto driving
      if (dc_left || dc_right) {// moving....
		if (old_adc_val > BLOCKAGE_THRESHOLD) {
	  // STOP!!!!!!
	  dc_left = 0; dc_right = 0;
	  Delay_tens(3);
	}
      } else {
	// stopped
	if (old_adc_val < CLEAR_THRESHOLD) {
	  // go forward again 
	  dir_fwd(); do_speedup (FWD_SPEED);
	} else {
	  // backup, pivot
	  dir_back(); 
          do_speedup (BACK_SPEED);  
          Delay_tens(3);
	  // slow down on wheel so it pivots
	  dc_left = 2; 
	  Delay_tens(8); dc_left = 0; dc_right = 0; 
          Delay_tens(2);
	  dir_fwd(); do_turn (BACK_SPEED,2);
	  Delay_tens(6);
	  dc_left = FWD_SPEED; dc_right = FWD_SPEED;
	}
      }//end if(dc_left...)else{}...
    }//end if(state == IO_FINISH)else{}..
  } //end  while(mode == AUTO_DRIVE)
}// end auto_drive()

void main(void){
  //serial_init(95,1); //debug only
  // initialize timer1, prescale by 1, internal clock
  T1CKPS1 = 0;  T1CKPS0 = 0;  T1OSCEN = 0;  TMR1CS = 0;  
  bitset(TRISC,2);// set CCP1 as input for IR Capture
  // set up everything for PWM, direction control
  bitclr(TRISB,OP_LEFT);  bitclr(TRISB,OP_RIGHT);
  bitclr(TRISB,OP_RDIR);  bitclr(TRISB,OP_LDIR);
  bitclr(PORTB,OP_LEFT);  bitclr(PORTB,OP_RIGHT);
  bitclr(PORTB,OP_RDIR);  bitclr(PORTB,OP_LDIR);
  // init timer3, prescale by 2, int. clock
  T3CKPS1 = 0; T3CKPS0 = 1; TMR3CS = 0;  T3SYNC = 0; 
  // TMR3 with CCP2, TMR1 with CCP1
  T3CCP2=0;T3CCP1=1; 
  // setup capture mode, enable capture interrupt
  CCP2CON = 0x02; CCP2IF = 0;  CCP2IE = 1; 
  CCPR2H = (MATCH_INTERVAL >> 8); 
  CCPR2L = (0xFF & MATCH_INTERVAL);
  TMR3ON = 1;
  // configure A/D for IR sensor, right. just, channel 0
  TRISA = 0xFF;	
  ADCON1 = 0x0E;  ADCON0 = 0x80;  ADON = 1; 
  IPEN = 0;  PEIE = 1;  GIE = 1; 
  mode = 0; 
  while(1) {
    switch (mode) {
      case 0:  manual_drive();
	break;
      case 1:  auto_drive();
	break;
    }
  }
}

//for MCC18, place the interrupt vector goto
#if defined(__18CXX)
#if defined(HIGH_INTERRUPT)
#pragma code HighVector=HIGH_INTERRUPT
#else
#pragma code HighVector=0x0008  
#endif
void HighVector (void)
{
    _asm goto isr _endasm
}
#pragma code
#endif