From 1f74d8e77ce93104f3da560ed8855fb039f2623e Mon Sep 17 00:00:00 2001
From: Brett Weiland <brett_weiland@bpcspace.com>
Date: Fri, 3 Nov 2023 18:19:15 -0500
Subject: just need to fix the macro

---
 software_pwm.c | 84 ++++++++++++++++++++++++++++++++++++++++++----------------
 1 file changed, 61 insertions(+), 23 deletions(-)

diff --git a/software_pwm.c b/software_pwm.c
index ca120d9..1de0d37 100644
--- a/software_pwm.c
+++ b/software_pwm.c
@@ -23,9 +23,13 @@ static struct pwm_event {
   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)))
+    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)
@@ -36,30 +40,33 @@ static struct pwm_event {
 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 = 
+  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) {
-  event_link->prev
+  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() {
 
-  //this one never moves
-  events[SOFTPWM_PIN_COUNT].time = 0;
-  events[SOFTPWM_PIN_COUNT].pins = 0;
-  events[SOFTPWM_PIN_COUNT].active.next = &events[SOFTPWM_PIN_COUNT];
-  events[SOFTPWM_PIN_COUNT].active.prev = &events[SOFTPWM_PIN_COUNT];
-  events[SOFTPWM_PIN_COUNT].enabled = true;
-
   //TODO: move all to zero once init_softpwm tested
-  for(size_t event_i = 0; event_i < SOFTPWM_PIN_COUNT; event_i++) {
+  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)) - 1;
-    LIST_INSERT(events[event_i], 
-        events[((int)event_i - 1) % (SOFTPWM_PIN_COUNT + 1)], active);
+    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; 
@@ -74,16 +81,47 @@ void init_softpwm() {
 }
 
 void softpwm_set(uint8_t pin, uint8_t duty) { 
-  /**
-  struct pwm_event *event_unmoved = &events[pin]; //TODO describe if you want ig
-  struct pwm_event *this_event = &events[pin];
-  this_event->time = duty;
-  if(duty < events[pin].time) {
-    while(duty < event_unmoved->time) event_unmoved = event_unmoved->next;
+  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;
 }
 
 /**
@@ -94,6 +132,6 @@ 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->next;
+  on_event = on_event->active.next;
   OCR0A = on_event->time;
 }
-- 
cgit v1.2.3