#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>

/* 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 maverage[8];
int mpos = 0;
int mvsum;
int minavg = 10000, maxavg=-10000;
int is_step(int avg, int oldavg) {
  if (stepflag == 2) {
    if (avg > (oldavg + STEP_THRESH))
      stepflag = 1;
    if (avg < (oldavg - STEP_THRESH)) 
      stepflag = 0;
      return 0;
  } else if (stepflag == 1) {
    if (maxavg > minavg && avg > (maxavg + minavg) / 2 && oldavg <= (maxavg + minavg) / 2)
      return 1;
    if (avg < (oldavg - step_thresh)) {
      stepflag = 0;
      if (oldavg > maxavg)
        maxavg = oldavg;
    }
    return 0;
  } else if (stepflag == 0) {
    if (avg > (oldavg - step_thresh)) {
      stepflag = 1;
      if (oldavg < minavg)
        minavg = oldavg;
    }
    return 0;
  }
  return 0;
}



int main(void) {
  led_init();
  init_uart0();
  init_timer0(NULL);
  ADCSRA |= _BV(ADEN);
  ADMUX |= _BV(ADLAR);

  int numsamples = 0;
  mvsum = v1 * 8;

  sei();
  int stepcount = 0;

  int stepflag = 2;

  #define STEP_THRESH 5;

  while (1) {
    int x = get_adc(0) - (1 << 9),
        y = get_adc(1) - (1 << 9);
    int newval = x * x + y * y;
    int oldavg = mvsum;
    mvsum += newval;
    if (numsamples < 8) {
      numsamples++;
    } else {
      mvsum -= maverage[mpos];
    }
    maverage[mpos++] = newval; 
    if (numsampes < 8)
      continue;
    if (isstep(mvsum, oldavg)) {
      printf("Step %d\r\n", stepcount++);
      maxavg = -10000;
      minavg = 10000;
    }
      
    delay_100ms(1);
  }
}