/**
 * 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 was_connected; // true after a connection is established, helps detect if disconnected AFTER connected

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;
	proxdata.user_trim = 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));
	}
	
	// User adjustment
	if(sigv2_tag_exists(SigTag_Prox_User_Trim)) {
		sigv2_find_tag(SigTag_Prox_User_Trim, (int16_t*)&(proxdata.user_trim));
	}
	
    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
		}
    }

	if(proxdata.connected)
		was_connected = true;

    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;
}

bool prox_was_connected(void) {
	return was_connected;
}

void prox_clear_was_connected(void) {
	was_connected = false;
}

void prox_update(void) {
	uint16_t temp_distance;
	uint16_t trimmed_threshold;
	
	int32_t temp_threshold = ((int32_t) proxdata.proximity_threshold) + ((int32_t) proxdata.user_trim);
	if(temp_threshold < 0) {
		trimmed_threshold = 0;
	} else {
		trimmed_threshold = (uint16_t) temp_threshold;
	}
	
    if(proxdata.connected) {
        // proxdata.current_prox_data = tmd2635_get_distance(MCU_I2C());
		temp_distance = tmd2635_get_distance(MCU_I2C());
        // if there's an I2C failure, tmd2635 driver will say we are disconnected
        // higher level function will try to reconnect periodically
        if(tmd2635_is_connected()) {
	        was_connected = true;
            // if(proxdata.current_prox_data <= proxdata.programmed_cal) {
			if(temp_distance <= proxdata.programmed_cal) {
                proxdata.current_prox_data = 0;
            } else {
                // proxdata.current_prox_data -= proxdata.programmed_cal;
				proxdata.current_prox_data = temp_distance - proxdata.programmed_cal;
            }
        } else {
            proxdata.connected = false;
        }
    }
	
	// validate the latest sample, determine on/off face by rolling average
	if(proxdata.connected) {
		if((temp_distance >= MIN_PROX_VALUE) && (temp_distance <= MAX_PROX_VALUE)) {

			// 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 <= 
						(trimmed_threshold - proxdata.proximity_hysteresis)) {

					proxdata.person_detected = false;
				}
			} else {
				// has the HMD been placed on?
				if( averaged_prox_data >= 
						(trimmed_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;
}

void prox_set_user_trim(int16_t newtrim) {
	proxdata.user_trim = newtrim;
}

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());
}