diff --git a/src/main.rs b/src/main.rs
index 2bce6a5..208d323 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -289,8 +289,11 @@ fn main() -> ! {
                 logger.log(b"mV - Iout: ").unwrap();
                 data = String::from(iout);
                 logger.log(data.as_bytes()).unwrap();
+                logger.log(b"mA - Pout: ").unwrap();
+                data = String::from(vout * iout / 1000);
+                logger.log(data.as_bytes()).unwrap();
 
-                logger.log(b"mA - raw: [").unwrap();
+                logger.log(b"mW - raw: [").unwrap();
 
                 for i in 0..adc_value.len() {
                     data = String::from(adc_value[i]);
diff --git a/src/switch_control.rs b/src/switch_control.rs
index f886881..567ecd0 100644
--- a/src/switch_control.rs
+++ b/src/switch_control.rs
@@ -1,3 +1,7 @@
+enum ControlMode {
+    MPPT,
+    ConstantVoltage
+}
 
 struct PID<const GAINSCALE: i32, const T0: i32>
 {
@@ -59,9 +63,71 @@ impl<const GAINSCALE: i32, const T0: i32> PID<GAINSCALE, T0>
     }
 }
 
+struct MPPT<const INTERVAL: u32, const PWR_TOLERANCE: i32>
+{
+    update_delay: u32,
+
+    pwm: i32,
+    increment: i32,
+    last_power: i32,
+
+    min_pwm: i32,
+    max_pwm: i32,
+}
+
+impl<const INTERVAL: u32, const PWR_TOLERANCE: i32> MPPT<INTERVAL, PWR_TOLERANCE>
+{
+    pub fn new(start_pwm: i32, min_pwm: i32, max_pwm: i32) -> Self
+    {
+        MPPT::<INTERVAL, PWR_TOLERANCE> {
+            update_delay: INTERVAL - 1,
+            pwm: start_pwm,
+            increment: 1,
+            last_power: 0,
+            min_pwm,
+            max_pwm 
+        }
+    }
+
+    pub fn update(&mut self, vout: i32, iout: i32) -> i32
+    {
+        if self.update_delay == 0 {
+            self.update_delay = INTERVAL - 1;
+
+            let cur_power = vout * iout;
+
+            let power_change = cur_power - self.last_power;
+
+            // invert search direction if measured power is significantly lower than previous measurement
+            if power_change.abs() > PWR_TOLERANCE && cur_power < self.last_power {
+                self.increment = -self.increment;
+            }
+
+            // enforce the pwm limits
+            if self.pwm == self.max_pwm {
+                self.increment = -1;
+            } else if self.pwm == self.min_pwm {
+                self.increment = 1;
+            }
+
+            // update the reference power
+            self.last_power = cur_power;
+
+            // step to the next test PWM value
+            self.pwm += self.increment;
+        } else {
+            self.update_delay -= 1;
+        }
+
+        self.pwm
+    }
+}
+
 pub struct SwitchControl<const T0: i32>
 {
+    mode: ControlMode,
     cv_pid: PID<10000, T0>,
+    mppt: MPPT<20, 50000 /* microwatts */>,
     pwm_max: i32
 }
 
@@ -70,15 +136,20 @@ impl<const T0: i32> SwitchControl<T0>
     pub fn new(cv_target: i32, pwm_max: i32) -> Self
     {
         SwitchControl::<T0> {
+            mode: ControlMode::MPPT,
             cv_pid: PID::<10000, T0>::new(100, 50, 0, cv_target, 0, 0, pwm_max * 10000),
+            mppt: MPPT::<20, 50000>::new(pwm_max/2, 0, pwm_max),
             pwm_max
         }
     }
 
     // returns the new PWM value
-    pub fn update(&mut self, vin: i32, vout: i32, iout: i32) -> i32
+    pub fn update(&mut self, _vin: i32, vout: i32, iout: i32) -> i32
     {
-        let pwm = self.cv_pid.update(vout);
+        let pwm = match self.mode {
+            ControlMode::ConstantVoltage => self.cv_pid.update(vout),
+            ControlMode::MPPT => self.mppt.update(vout, iout),
+        };
 
         if pwm > self.pwm_max {
             self.pwm_max