/**
 * prox_control.c
 *
 * Initialization, calibration, and data retrieval functions for the proximity sensor.
 * 
 * Copyright (c) 2022 Bigscreen, Inc.
 */

#include <asf.h>
#include "prox_tmd2635.h"
#include "prox_control.h"

#define PROX_AVERAGE_LENGTH		(16)

uint16_t prox_moving_average[PROX_AVERAGE_LENGTH];
uint8_t prox_moving_average_loc = 0;

Proximity_T proxdata;

bool prox_init(void) {
    bool calib_complete = false;
    // Initializes the TMD2635 and performs calibration
    proxdata.connected = false;
    proxdata.calibrated = false;
    proxdata.person_detected = false;
    proxdata.proximity_threshold = PROX_SENSOR_THRESHOLD;
    proxdata.proximity_hysteresis = PROX_SENSOR_HYSTERESIS;
    proxdata.current_prox_data = 0;
	proxdata.programmed_cal = 0;
	// Attempt to grab the programmed cal from user signature
	sigv2_find_tag(SigTag_Prox_Cal, (uint8_t*)&(proxdata.programmed_cal));
	// And the threshold
	if(sigv2_tag_exists(SigTag_Prox_Threshold)) {
		sigv2_find_tag(SigTag_Prox_Threshold, (uint8_t*)&(proxdata.proximity_threshold));
	}
	// And the hysteresis
	if(sigv2_tag_exists(SigTag_Prox_Hysteresis)) {
		sigv2_find_tag(SigTag_Prox_Hysteresis, (uint8_t*)&(proxdata.proximity_hysteresis));
	}
	
    proxdata.offset = 0;
    if(pdPASS == tmd2635_init(MCU_I2C(), true)) {
        proxdata.connected = true;

        // now try calibrating
        calib_complete = tmd2635_calibrate(MCU_I2C(), Cal_Target_127);
		
		if(false == calib_complete) {
			// failed to calibrate, try re-init and calibrating again
			tmd2635_init(MCU_I2C(), false); // init without software reset
			// lower the search target this time.
			// might have failed to calibrate due to too low of a signal coming in,
			// and there would be no possible offset that would reduce the 
			// value to the calibration target
			calib_complete = tmd2635_calibrate(MCU_I2C(), Cal_Target_31);
		}
        proxdata.calibrated = calib_complete;
        if(calib_complete) {
            proxdata.offset = tmd2635_get_offset(MCU_I2C());
        } else {
			// TODO: consider a hardcoded calibration fallback value
		}
    }

    return proxdata.connected; // return value only depends on connection. if calibration failed, that's a different story.
}

bool prox_is_connected(void) {
	return proxdata.connected;
}


void prox_update(void) {
    if(proxdata.connected) {
        proxdata.current_prox_data = tmd2635_get_distance(MCU_I2C());

		if(proxdata.current_prox_data <= proxdata.programmed_cal) {
			proxdata.current_prox_data = 0;
		} else {
			proxdata.current_prox_data -= proxdata.programmed_cal;
		}
    }

	// Moving average of the last few samples.
	prox_moving_average[prox_moving_average_loc] = proxdata.current_prox_data;
	prox_moving_average_loc++;
	if(prox_moving_average_loc >= PROX_AVERAGE_LENGTH) {
		prox_moving_average_loc = 0;
	}
	
	uint32_t averaged_prox_data = 0;
	for(uint8_t i = 0; i < PROX_AVERAGE_LENGTH; i++) {
		averaged_prox_data += prox_moving_average[i];
	}
	averaged_prox_data = averaged_prox_data / PROX_AVERAGE_LENGTH;

    if(proxdata.person_detected) {
        // if now proximity value decreases beyond threshold, assume HMD has been removed and
        // displays should turn off
        if( averaged_prox_data <= 
                (proxdata.proximity_threshold - proxdata.proximity_hysteresis)) {

            proxdata.person_detected = false;
        }
    } else {
        // has the HMD been placed on?
        if( averaged_prox_data >= 
                (proxdata.proximity_threshold + proxdata.proximity_hysteresis)) {

            proxdata.person_detected = true;
        }
    }
}

bool prox_hmd_on_person(void) {
    return proxdata.person_detected;
}

uint16_t prox_get_distance(void) {
	return proxdata.current_prox_data;
}

void prox_set_threshold(uint16_t newthresh) {
    proxdata.proximity_threshold = newthresh;
}

void prox_set_hysteresis(uint16_t newhyst) {
    proxdata.proximity_hysteresis = newhyst;
}

bool prox_set_offset(uint16_t newoffset) {
    // Use with caution! Preferably offset should be set automatically
    // during calibration.
    // This routine can be used if calibration fails. You can fallback
    // on a previously good offset value. From manufacturing QA for example

    if(tmd2635_set_offset(MCU_I2C(), newoffset)) {
        proxdata.offset = newoffset;
        return true;
    } else {
        return false;
    }
}

Prox_Settings* prox_get_settings(void) {
	return tmd2635_get_settings();
}

bool prox_apply_settings(void) {
	return tmd2635_apply_settings(MCU_I2C());
}