// rc_car.c
// for NerdKits with ATmega168
// mrobbins@mit.edu

#define F_CPU 14745600

#include <stdio.h>

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <inttypes.h>

#include "../libnerdkits/delay.h"
#include "../libnerdkits/uart.h"

// PIN DEFINITIONS:
//
// PD3 -- OC2B -- right backward
// PD5 -- OC0B -- left backward
// PD6 -- OC0A -- left forward
// PB3 -- OC2A -- right forward

int16_t clip_pm_255(int16_t in) {
  if(in > 255) {
    return 255;
  } else if(in < -255) {
    return -255;
  } else {
    return in;
  }
}

void set_left(int16_t in) {
  in = clip_pm_255(in);
  int16_t tmp = in; if(in<0) tmp = -in;
  uint8_t mag = tmp;
  
  if(in >= 0) {
    OCR0B = 0;
    OCR0A = mag;
  } else {
    OCR0A = 0;
    OCR0B = mag;
  }  
}

void set_right(int16_t in) {
  in = clip_pm_255(in);
  int16_t tmp = in; if(in<0) tmp = -in;
  uint8_t mag = tmp;
  
  if(in >= 0) {
    OCR2B = 0;
    OCR2A = mag;
  } else {
    OCR2A = 0;
    OCR2B = mag;
  }  
}

int main() {
  // PWMs as outputs
  DDRB |= (1<<PB3);
  DDRD |= (1<<PD3) | (1<<PD5) | (1<<PD6);

  // init serial port
  uart_init();
  FILE uart_stream = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);
  stdin = stdout = &uart_stream;

  // Timer0: phase-correct PWM
  TCCR0A = (1<<COM0A1) | (1<<COM0B1) | (1<<WGM00);
  // Timer0: CK/1024
  //TCCR0B = (1<<CS02) | (1<<CS00);
  TCCR0B = (1<<CS01) | (1<<CS00); // CK/64

  // Timer2: phase-correct PWM
  TCCR2A = (1<<COM2A1) | (1<<COM2B1) | (1<<WGM20);
  // Timer2: CK/1024
  //TCCR2B = (1<<CS22) | (1<<CS21) | (1<<CS20);
  TCCR2B = (1<<CS22); // CK/64
  uint8_t result;
  char tmp_c;
  int16_t tmp_i;
  while(1) {
    // listen for bytes
    result = scanf_P(PSTR("%c %d"), &tmp_c, &tmp_i);
    if(result == 2) {
      //printf_P(PSTR("Got command '%c' with value %d.\r\n"), tmp_c, tmp_i);
      
      // Got command!
      if(tmp_c=='L') {
        set_left(tmp_i);
      } else if(tmp_c=='R') {
        set_right(tmp_i);
      }
      
    }
  } 

  return 0;
}