/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
package com.ctre.phoenix6.configs;

import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.configs.jni.ConfigJNI;
import com.ctre.phoenix6.spns.*;

import edu.wpi.first.units.*;

import edu.wpi.first.units.measure.*;
import static edu.wpi.first.units.Units.*;

/**
 * Configs that affect the ToF Proximity detection
 * <p>
 * Includes proximity mode and the threshold for simple detection
 */
public class ProximityParamsConfigs implements ParentConfiguration
{
    /**
     * Threshold for object detection.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 4
     *   <li> <b>Default Value:</b> 0.4
     *   <li> <b>Units:</b> m
     * </ul>
     */
    public double ProximityThreshold = 0.4;
    /**
     * How far above and below the threshold the distance needs to be to
     * trigger undetected and detected, respectively. This is used to
     * prevent bouncing between the detected and undetected states for
     * objects on the threshold.
     * <p>
     * If the threshold is set to 0.1 meters, and the hysteresis is 0.01
     * meters, then an object needs to be within 0.09 meters to be
     * detected. After the object is first detected, the distance then
     * needs to exceed 0.11 meters to become undetected again.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 1
     *   <li> <b>Default Value:</b> 0.01
     *   <li> <b>Units:</b> m
     * </ul>
     */
    public double ProximityHysteresis = 0.01;
    /**
     * The minimum allowable signal strength before determining the
     * measurement is valid.
     * <p>
     * If the signal strength is particularly low, this typically means
     * the object is far away and there's fewer total samples to derive
     * the distance from. Set this value to be below the lowest strength
     * you see when you're detecting an object with the CANrange; the
     * default of 2500 is typically acceptable in most cases.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 1
     *   <li> <b>Maximum Value:</b> 15000
     *   <li> <b>Default Value:</b> 2500
     *   <li> <b>Units:</b> 
     * </ul>
     */
    public double MinSignalStrengthForValidMeasurement = 2500;
    
    /**
     * Modifies this configuration's ProximityThreshold parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Threshold for object detection.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 4
     *   <li> <b>Default Value:</b> 0.4
     *   <li> <b>Units:</b> m
     * </ul>
     *
     * @param newProximityThreshold Parameter to modify
     * @return Itself
     */
    public ProximityParamsConfigs withProximityThreshold(double newProximityThreshold)
    {
        ProximityThreshold = newProximityThreshold;
        return this;
    }
    
    /**
     * Modifies this configuration's ProximityThreshold parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * Threshold for object detection.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 4
     *   <li> <b>Default Value:</b> 0.4
     *   <li> <b>Units:</b> m
     * </ul>
     *
     * @param newProximityThreshold Parameter to modify
     * @return Itself
     */
    public ProximityParamsConfigs withProximityThreshold(Distance newProximityThreshold)
    {
        ProximityThreshold = newProximityThreshold.in(Meters);
        return this;
    }
    
    /**
     * Helper method to get this configuration's ProximityThreshold parameter converted
     * to a unit type. If not using the Java units library, {@link #ProximityThreshold}
     * can be accessed directly instead.
     * <p>
     * Threshold for object detection.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 4
     *   <li> <b>Default Value:</b> 0.4
     *   <li> <b>Units:</b> m
     * </ul>
     *
     * @return ProximityThreshold
     */
    public Distance getProximityThresholdMeasure()
    {
        return Meters.of(ProximityThreshold);
    }
    
    /**
     * Modifies this configuration's ProximityHysteresis parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * How far above and below the threshold the distance needs to be to
     * trigger undetected and detected, respectively. This is used to
     * prevent bouncing between the detected and undetected states for
     * objects on the threshold.
     * <p>
     * If the threshold is set to 0.1 meters, and the hysteresis is 0.01
     * meters, then an object needs to be within 0.09 meters to be
     * detected. After the object is first detected, the distance then
     * needs to exceed 0.11 meters to become undetected again.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 1
     *   <li> <b>Default Value:</b> 0.01
     *   <li> <b>Units:</b> m
     * </ul>
     *
     * @param newProximityHysteresis Parameter to modify
     * @return Itself
     */
    public ProximityParamsConfigs withProximityHysteresis(double newProximityHysteresis)
    {
        ProximityHysteresis = newProximityHysteresis;
        return this;
    }
    
    /**
     * Modifies this configuration's ProximityHysteresis parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * How far above and below the threshold the distance needs to be to
     * trigger undetected and detected, respectively. This is used to
     * prevent bouncing between the detected and undetected states for
     * objects on the threshold.
     * <p>
     * If the threshold is set to 0.1 meters, and the hysteresis is 0.01
     * meters, then an object needs to be within 0.09 meters to be
     * detected. After the object is first detected, the distance then
     * needs to exceed 0.11 meters to become undetected again.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 1
     *   <li> <b>Default Value:</b> 0.01
     *   <li> <b>Units:</b> m
     * </ul>
     *
     * @param newProximityHysteresis Parameter to modify
     * @return Itself
     */
    public ProximityParamsConfigs withProximityHysteresis(Distance newProximityHysteresis)
    {
        ProximityHysteresis = newProximityHysteresis.in(Meters);
        return this;
    }
    
    /**
     * Helper method to get this configuration's ProximityHysteresis parameter converted
     * to a unit type. If not using the Java units library, {@link #ProximityHysteresis}
     * can be accessed directly instead.
     * <p>
     * How far above and below the threshold the distance needs to be to
     * trigger undetected and detected, respectively. This is used to
     * prevent bouncing between the detected and undetected states for
     * objects on the threshold.
     * <p>
     * If the threshold is set to 0.1 meters, and the hysteresis is 0.01
     * meters, then an object needs to be within 0.09 meters to be
     * detected. After the object is first detected, the distance then
     * needs to exceed 0.11 meters to become undetected again.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 0
     *   <li> <b>Maximum Value:</b> 1
     *   <li> <b>Default Value:</b> 0.01
     *   <li> <b>Units:</b> m
     * </ul>
     *
     * @return ProximityHysteresis
     */
    public Distance getProximityHysteresisMeasure()
    {
        return Meters.of(ProximityHysteresis);
    }
    
    /**
     * Modifies this configuration's MinSignalStrengthForValidMeasurement parameter and returns itself for
     * method-chaining and easier to use config API.
     * <p>
     * The minimum allowable signal strength before determining the
     * measurement is valid.
     * <p>
     * If the signal strength is particularly low, this typically means
     * the object is far away and there's fewer total samples to derive
     * the distance from. Set this value to be below the lowest strength
     * you see when you're detecting an object with the CANrange; the
     * default of 2500 is typically acceptable in most cases.
     * 
     * <ul>
     *   <li> <b>Minimum Value:</b> 1
     *   <li> <b>Maximum Value:</b> 15000
     *   <li> <b>Default Value:</b> 2500
     *   <li> <b>Units:</b> 
     * </ul>
     *
     * @param newMinSignalStrengthForValidMeasurement Parameter to modify
     * @return Itself
     */
    public ProximityParamsConfigs withMinSignalStrengthForValidMeasurement(double newMinSignalStrengthForValidMeasurement)
    {
        MinSignalStrengthForValidMeasurement = newMinSignalStrengthForValidMeasurement;
        return this;
    }

    

    @Override
    public String toString()
    {
        String ss = "Config Group: ProximityParams\n";
        ss += "    ProximityThreshold: " + ProximityThreshold + " m" + "\n";
        ss += "    ProximityHysteresis: " + ProximityHysteresis + " m" + "\n";
        ss += "    MinSignalStrengthForValidMeasurement: " + MinSignalStrengthForValidMeasurement + "\n";
        return ss;
    }

    /**
     *
     */
    public StatusCode deserialize(String to_deserialize)
    {
        ProximityThreshold = ConfigJNI.Deserializedouble(SpnValue.CANrange_ProximityThreshold.value, to_deserialize);
        ProximityHysteresis = ConfigJNI.Deserializedouble(SpnValue.CANrange_ProximityHysteresis.value, to_deserialize);
        MinSignalStrengthForValidMeasurement = ConfigJNI.Deserializedouble(SpnValue.CANrange_MinSigStrengthForValidMeas.value, to_deserialize);
        return  StatusCode.OK;
    }

    /**
     *
     */
    public String serialize()
    {
        String ss = "";
        ss += ConfigJNI.Serializedouble(SpnValue.CANrange_ProximityThreshold.value, ProximityThreshold);
        ss += ConfigJNI.Serializedouble(SpnValue.CANrange_ProximityHysteresis.value, ProximityHysteresis);
        ss += ConfigJNI.Serializedouble(SpnValue.CANrange_MinSigStrengthForValidMeas.value, MinSignalStrengthForValidMeasurement);
        return ss;
    }
}