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

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


static struct link_pair {
  struct pwm_event *next; 
  struct pwm_event *prev;
};

//if next is ever zero, node is assumed not part of list/disabled
static struct pwm_event {
  struct link_pair active;
  struct link_pair twins;
  uint8_t time;
  uint8_t pins;
  bool enabled;
} events[SOFTPWM_PIN_COUNT + 1];

//if list_insert and list_remove end up being too slow, these have a lot of room for
//optimization. We could do a preprocessor IF/THEN instead of using member offset
//TODO accept pointer instead of actual value
#define LIST_INSERT(new, prev, member) (\
    list_insert(new->member, prev->member, offsetof(struct pwm_event, member)))

#define LIST_REMOVE(event, member) list_remove((event)->member, offsetof(struct pwm_event, member))

#define EVENT_FROM_MEMBER(member_ptr, member_offset) \
  ((struct pwm_event *)(char *)member_ptr - member_offset)

#define MEMBER_FROM_EVENT(event_ptr, member_offset) \
  ((struct link_pair *)(char *)event_ptr + member_offset)

static void list_insert(struct link_pair *new_links, struct link_pair *prev_links, size_t parent_offset) {
  new_links->next = prev_links->next;
  new_links->prev = EVENT_FROM_MEMBER(prev_links, parent_offset);
  MEMBER_FROM_EVENT(prev_links->next, parent_offset)->prev = 
    EVENT_FROM_MEMBER(new_links, parent_offset); //GYAAAAGH
  prev_links->next = EVENT_FROM_MEMBER(prev_links, parent_offset);
}

static void list_remove(struct link_pair *event_link, size_t parent_offset) {
  MEMBER_FROM_EVENT(event_link->next, parent_offset)->prev = 
    EVENT_FROM_MEMBER(event_link->prev, parent_offset);
  MEMBER_FROM_EVENT(event_link->prev, parent_offset)->next = 
    EVENT_FROM_MEMBER(event_link->next, parent_offset);
}

void init_softpwm() {

  //TODO: move all to zero once init_softpwm tested
  events[0].active.prev = events;
  events[0].active.next = events;
  events[0].enabled = true;
  events[0].pins = 0;
  events[0].time = 0;

  for(size_t event_i = 1; event_i <= SOFTPWM_PIN_COUNT; event_i++) {
    events[event_i].time = event_i;
    events[event_i].pins = (1 << event_i) - 1;
    LIST_INSERT(&(events[event_i]), 
        &events[((int)event_i - 1) % (SOFTPWM_PIN_COUNT + 1)], active);
    events[event_i].enabled = true;
  }

  SOFTPWM_DDR = 0xff; 

  //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[SOFTPWM_PIN_COUNT].time;
  TIFR0 = 0b00000010; //interupts enabled!
  sei();
}

void softpwm_set(uint8_t pin, uint8_t duty) { 
  struct pwm_event *pin_event = &events[pin + 1];
  struct pwm_event *closest_event;
  if(duty == pin_event->time) return;
  for(closest_event = pin_event; !pin_event->enabled; pin_event = pin_event->twins.next);
  if(pin_event->time < duty) {
    while(duty > closest_event->time) {
      closest_event->time ^= (1 << pin);
      closest_event = closest_event->active.prev;
    }
  }
  if(pin_event->time > duty) {
    while(duty < closest_event->time) {
      closest_event->time |= (1 << pin);
      closest_event = closest_event->active.prev;
    }
  }

  //I'm sure I could clean the following a bit
  //unlinking
  if(pin_event->enabled) {
    if(pin_event->twins.next == pin_event) { LIST_REMOVE(pin_event, active); }
    else {
      //lol
      pin_event->active.prev->active.next = pin_event->twins.next;
      pin_event->active.next->active.prev = pin_event->twins.next;
      pin_event->twins.next->active.prev = pin_event->active.prev;
      pin_event->twins.next->active.next = pin_event->active.next;
    }
  }
  else { LIST_REMOVE(pin_event, twins); }

  //linking
  pin_event->enabled = !(duty == pin_event->time);
  if(pin_event->enabled) {
    LIST_INSERT(pin_event, closest_event, active);
  }
  else {
    LIST_INSERT(pin_event, closest_event, twins);
  }

  pin_event->time = duty;
}

/**
ISR(TIMER0_OVF_vect) {
}
**/

ISR(TIMER0_COMP_vect) {
  static struct pwm_event *on_event = &events[SOFTPWM_PIN_COUNT];
  SOFTPWM_PORT = on_event->pins;
  on_event = on_event->active.next;
  OCR0A = on_event->time;
}