/*
 * step_detection.c
 *
 *  Created on: May 20, 2025
 *      Author: 911BL
 */
#include "step_detection.h"
#include "globals.h"

uint32_t imu_magnitude(int16_t x, int16_t y, int16_t z) {
    return (uint32_t)(x * x) + (uint32_t)(y * y) + (uint32_t)(z * z);
}

StepState step_state = WAITING_FOR_PEAK;

void process_step(uint32_t acc_mag_squared) {
    static enum { WAITING_FOR_PEAK, WAITING_FOR_VALLEY } state = WAITING_FOR_PEAK;

    switch (state) {
        case WAITING_FOR_PEAK:
            if (acc_mag_squared > UPPER_THRESHOLD) {
                state = WAITING_FOR_VALLEY;
            }
            break;

        case WAITING_FOR_VALLEY:
            if (acc_mag_squared < LOWER_THRESHOLD) {
            	STEPCOUNT++;
                state = WAITING_FOR_PEAK;
            }
            break;
    }
}
