#include <avr/interrupt.h>
#include <avr/io.h> //include to get access to port defenitions
#include <Robostix.h>
#include <timer0.h>
#include <led.h>
#include <uart0.h>
#include <uart1.h>

/* Definitions and globals */
int get_adc(int channel);

int get_adc(int channel) {
  ADMUX = _BV(ADLAR) | _BV(REFS0) | channel;
  ADCSRA |= _BV(ADSC);
  while (!(ADCSRA & _BV(ADIF)));
  uint8_t data = ADCL;
  uint8_t data2 = ADCH;
  return (data >> 6) | (data2 << 2);
}

int arrayloop[32];
int pos = 0;
int maverage[8];
int mpos = 0;
int csum, mvsum;

int i = 0;
void handle_timer(ticks_t ticks);
void handle_timer(ticks_t ticks) {
  i++;
  if (ticks % 100 == 0) {
    LED_TOGGLE(BLUE);
    uart0_put('a', NULL);
    uart1_put('b', NULL);
    i = 0;
  }
}

int main(void) {
  led_init();
  init_uart0();
  init_uart1();
  init_timer0(handle_timer);
  sei();
  while (1) {
    LED_TOGGLE(RED);

    uart0_put('a', NULL);
    uart1_put('b', NULL);
//
    //  uart0_put(uart1_get(NULL),NULL);
  }
#if 0
ADCSRA |= _BV(ADEN);
  ADMUX |= _BV(ADLAR);

  int i;
  int v1 = get_adc(0);
  for (i = 0; i < 32; i++) {
    arrayloop[i] = v1;
  }
  for (i = 0; i < 8; i++) {
    maverage[i] = v1;
  }
  csum = v1 * 32;
  mvsum = v1 * 8;

  sei();
  int stepcount = 0;

  while (1) {
    int x = get_adc(0);
    int oldsum = mvsum;
    mvsum -= maverage[mpos];
    mvsum += x;
    maverage[mpos++] = x;
    if (mpos >= 8)
      mpos = 0;
    csum -= arrayloop[pos];
    csum += x;
    arrayloop[pos++] = x;
    if (pos >= 8)
      pos = 0;

    oldsum -= csum >> 2;
    int newsum = mvsum - (csum >> 2);
    if ((newsum > 0) ^ (oldsum > 0)) {
      printf("Step %d\r\n", stepcount++);
    }


    /*int y = get_adc(1);

    printf("x=%d\r\n", get_adc(0));

    printf("y=%d\r\n", get_adc(1));
    */
    delay_100ms(1);
  }
#endif
}