#include <stdint.h>
#include <string.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#include "software_pwm.h"
#include "pins.h"

static struct pwm_event {
  //ENTIRE port
  //(ex if pin 3, 2, 4 are on and we want 6, this should be 0b01011100)
  uint8_t port; 
  uint8_t time;
  struct pwm_event **map_ptr; 
} events[SOFTPWM_PIN_COUNT];

static struct pwm_event *pin_event_map[SOFTPWM_PIN_COUNT];


void init_softpwm() {
  SOFTPWM_DDR = 0xff; 
  for(size_t event_i = 0; event_i < SOFTPWM_PIN_COUNT; event_i++) {
    pin_event_map[event_i] = &events[event_i];
    events[event_i].map_ptr = &pin_event_map[event_i];
    events[event_i].time = event_i + 1;
    events[event_i].port = (1 << (event_i + 1)) - 1;
  }

  //use external 10mhz clock (we just have one laying around)
  //then prescale by 1024. Check at different freqs for brightness
  TCCR0A = 0b00000010;
  TIMSK0 = 0b00000011;
  OCR0A = events[0].time;
  TIFR0 = 0b00000010; //interupts enabled!
  sei();
}


static void event_move(struct pwm_event *dest, struct pwm_event *src) {
  *dest = *src;
  *(dest->map_ptr) = src;
}

/**
 * okay, let me explain. This is really weird and gross, I know. 
 * I did it when I was *very* low on sleep.
 * Why did I do it like this? Well, I was worried about accuracy; 
 * for some stupid reason I thought I really needed the actual pwm ISR to be as fast
 * as possible. I don't feel like redoing this in a simpler fasion, 
 * so while this code *is* embaracing, I'll get around to it later.
 **/
void softpwm_set(uint8_t pin, uint8_t value) { 
  struct pwm_event *event_to_shift;
  struct pwm_event modified_event_cpy = *pin_event_map[pin]; 

  modified_event_cpy.time = value;
  if(value < modified_event_cpy.time) {
    for(event_to_shift = pin_event_map[pin - 1];
        (event_to_shift >= events) 
        || (modified_event_cpy.time < event_to_shift[-1].time);
         event_to_shift = &event_to_shift[-1]) 
    {
      event_to_shift->port |= (1 << pin);
      event_move(&event_to_shift[1], event_to_shift);
    }
    event_move(&event_to_shift[1], &modified_event_cpy);
    return;
  }
  if(value > modified_event_cpy.time) {
    for(event_to_shift = pin_event_map[pin + 1];
        (event_to_shift < &events[SOFTPWM_PIN_COUNT]) 
        || (modified_event_cpy.time > event_to_shift[1].time);
         event_to_shift = &event_to_shift[1]) 
    {
      event_to_shift->port &= ~(1 << pin);
      event_move(&event_to_shift[-1], event_to_shift);
    }
  }
  event_move(&event_to_shift[-1], &modified_event_cpy);
}

ISR(TIMER0_OVF_vect) {
  SOFTPWM_PORT = 0;
  PORTA ^= 1;
}

//ISR for pwm timer. Need to look at assembly output to ensure proper optimization
//(would it be faster to have two different arrays vs a struct? idk if the offset is computed compile or runtime)
ISR(TIMER0_COMP_vect) {
  static uint8_t eventcount = 0;
  //eventcount = (eventcount <= SOFTPWM_PIN_COUNT) ? eventcount + 1 : 0;
  SOFTPWM_PORT = events[eventcount].port;
  eventcount = (eventcount + 1) % 8;
  OCR0A = events[eventcount].time;
  PORTA ^= 2;
}