#include "stepper.h"
#include "portdeclarations.h"
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

// Absolute positions
volatile unsigned char current_flap_pos;
volatile unsigned char current_blind_pos;

// Constants determined at init
unsigned char steps_per_blind_pos;
unsigned char steps_per_flap_pos;

// Within the cycle
volatile unsigned char current_flap_step_count;
volatile unsigned char current_blind_step_count;

// Adjustments to make for up/down
volatile unsigned char adjup_pos;
volatile unsigned char adjdown_pos;

// Adjustments to make for open/close
volatile unsigned char adjopen_pos;
volatile unsigned char adjclose_pos;

volatile char stepperoutput;

unsigned char height[16];
unsigned char angle[16]; 	// Height and angle memory
unsigned char i;				// Index the shade should be on given the sensor settings.

char stepper_cycle[4] = {0b00001001, 0b00000101, 0b00000110, 0b00001010};



/****
 * Timer 2 will control motor speed movement
 *
 * Interrupt triggers and determines which
 * direction to move
 ****/
char stepperindex=0;
char flapindex=0;
ISR(TIMER2_COMPA_vect) {
  stepperoutput = 0x00;
  // Control blind
  if (adjup_pos > 0) { // Going up
    if (current_blind_pos == 0) { // Keep at top
      if (current_blind_step_count != 0) {
        current_blind_step_count--;
	if (stepperindex == 0) stepperindex = 3;
	else stepperindex -= 1;
      } else {
        adjup_pos--;
      }
    } else { // Else adjust
      if (current_blind_step_count == 0) { // One cycle done
        current_blind_step_count = steps_per_blind_pos;
	if (stepperindex == 0) stepperindex = 3;
	else stepperindex -= 1;
        adjup_pos--;
        current_blind_pos--;
      } else {
        current_blind_step_count--;
	if (stepperindex == 0) stepperindex = 3;
	else stepperindex -= 1;
      }
    }
  } else if (adjdown_pos > 0) { // Going down
    if (current_blind_pos == 255) { // At bottom already
      if (current_blind_step_count != steps_per_blind_pos) {
        current_blind_step_count++;
	if (stepperindex == 3) stepperindex = 0;
	else stepperindex += 1;
      } else {
        adjdown_pos--;
      }
    } else { // Else adjust
      if (current_blind_step_count == steps_per_blind_pos) { // One cycle done
        current_blind_step_count = 0;
	if (stepperindex == 3) stepperindex = 0;
	else stepperindex += 1;
        adjdown_pos--;
        current_blind_pos++;
      } else {
        current_blind_step_count++;
	if (stepperindex == 3) stepperindex = 0;
	else stepperindex += 1;
      }
    }
  }
  stepperoutput |= stepper_cycle[(int)stepperindex];


  // Control flap

  if (adjopen_pos > 0) { // Opening
    if (current_flap_pos == 0) { // Open most
      if (current_flap_step_count != 0) {
        current_flap_step_count--;
	if (flapindex == 0) flapindex = 3;
	else flapindex -= 1;
      } else {
        adjopen_pos--;
      }
    } else { // Else adjust
      if (current_flap_step_count == 0) { // One cycle done
        current_flap_step_count = steps_per_flap_pos;
	if (flapindex == 0) flapindex = 3;
	else flapindex -= 1;
        current_flap_pos--;
        adjopen_pos--;
      } else {
        current_flap_step_count--;
	if (flapindex == 0) flapindex = 3;
	else flapindex -= 1;
      }
    }
  } else if (adjclose_pos > 0) { // Closing
    if (current_flap_pos == 255) { // At bottom already
      if (current_flap_step_count != steps_per_flap_pos) {
        current_flap_step_count++;
	if (flapindex == 3) flapindex = 0;
	else flapindex += 1;
      } else {
        adjclose_pos--;
      }
    } else { // Else adjust
      if (current_flap_step_count == steps_per_flap_pos) { // One cycle done
        current_flap_step_count = 0;
	if (flapindex == 3) flapindex = 0;
	else flapindex += 1;
        adjclose_pos--;
        current_flap_pos++;
      } else {
        current_flap_step_count++;
	if (flapindex == 3) flapindex = 0;
	else flapindex += 1;
      }
    }
  }
  stepperoutput |= (stepper_cycle[(int)flapindex] << 4);
  
  // Update stepper motor output
  STEPPER_PORT = stepperoutput;
}
  
void stepper_init() {
  // Enable pull-up (contact will disable it)
  STEPPER_DDRD = 0xFF;
  // Initially unknown
  current_flap_pos = 0;
  current_blind_pos = 255;

  // Initially set to num steps per revolution of stepper
  steps_per_blind_pos = 10;
  steps_per_flap_pos = 1;

  current_flap_step_count = steps_per_flap_pos;
  current_blind_step_count = steps_per_blind_pos;
  stepperoutput = 0;
  // Setup timer2 for motor control
  TIMSK2 |= (1<<OCIE2A);
  OCR2A = 162;
  TCCR2B |= 0x07; // T = 1/(16MHz/1024)*163 = 10.432 ms = 2revs/sec
  TCCR2A |= (1<<WGM21);

  // Enable interrupts
  sei();

  // stepper_calib()
}

/***
 * Calibration by shorting low when touch top
 ***/
void stepper_calib() {
  SENSOR_PORT |= (1<<SENSOR_TOP_TOUCH);
  while(SENSOR_PIN & (1<<SENSOR_TOP_TOUCH)) {
    adjup_pos = 1;
    current_blind_pos = 1;
  }
  current_blind_pos = 0;
}