/* DCL.h - C2000 Digital Control Library
 *
 */
//#############################################################################
//!
//! Copyright: Copyright (C) 2023 Texas Instruments Incorporated -
//!	All rights reserved not granted herein.
//!	Limited License.
//!
//! Texas Instruments Incorporated grants a world-wide, royalty-free,
//! non-exclusive license under copyrights and patents it now or hereafter
//! owns or controls to make, have made, use, import, offer to sell and sell
//! ("Utilize") this software subject to the terms herein. With respect to the
//! foregoing patent license, such license is granted solely to the extent that
//! any such patent is necessary to Utilize the software alone. The patent
//! license shall not apply to any combinations which include this software,
//! other than combinations with devices manufactured by or for TI 
//! ("TI Devices").
//! No hardware patent is licensed hereunder.
//!
//! Redistributions must preserve existing copyright notices and reproduce this
//! license (including the above copyright notice and the disclaimer and
//! (if applicable) source code license limitations below) in the documentation
//!  and/or other materials provided with the distribution.
//!
//! Redistribution and use in binary form, without modification, are permitted
//! provided that the following conditions are met:
//!
//! * No reverse engineering, decompilation, or disassembly of this software is 
//! permitted with respect to any software provided in binary form.
//! * Any redistribution and use are licensed by TI for use only 
//!   with TI Devices.
//! * Nothing shall obligate TI to provide you with source code for the 
//!   software licensed and provided to you in object code.
//!
//! If software source code is provided to you, modification and redistribution
//! of the source code are permitted provided that the following conditions 
//! are met:
//!
//! * any redistribution and use of the source code, including any resulting
//!   derivative works, are licensed by TI for use only with TI Devices.
//! * any redistribution and use of any object code compiled from the source
//!   code and any resulting derivative works, are licensed by TI for use 
//!   only with TI Devices.
//!
//! Neither the name of Texas Instruments Incorporated nor the names of its
//! suppliers may be used to endorse or promote products derived from this 
//! software without specific prior written permission.
//#############################################################################

#ifndef _C_DCL_H
#define _C_DCL_H

#ifdef __cplusplus
extern "C" {
#endif

//! \file           DCL.h
//! \brief          Contains the public interface to the common
//!                 Digital Controller Library functions

//! \brief          Library version number formatted for numerical comparison
//!
#define DCL_VERSION 3040000

#ifndef C2000_IEEE754_TYPES
#define C2000_IEEE754_TYPES
#ifdef __TI_EABI__
typedef float float32_t;
typedef double float64_t;
#else
typedef float float32_t;
typedef long double float64_t;
#endif      // __TI_EABI__
#endif      // C2000_IEEE754_TYPES

#ifndef __TMS320C28XX_CLA__

#include <complex.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>

//! \brief          Local definitions of mathematical constants
//!
#define CONST_PI_32     3.14159265358979323846f
#define CONST_2PI_32    2.0f * CONST_PI_32

#define CONST_PI_64     3.1415926535897932384626433832795028841971693993751058209749445923078164062L
#define CONST_2PI_64    2.0L * CONST_PI_64

//! \brief          Defines 32-bit null address for pointer initialization
//!
#define NULL_ADDR   0x00000000

//! \brief          Define the acceptable FPU numerical tolerances
//!
#define DEFAULT_FPU32_TOL   1.0e-06f
#define DEFAULT_FPU64_TOL   1.0e-10f

//! \brief          Determines numerical proximity to specified value
//!
#define F32_IS_VALUE(x,y)   (((x < (y + DEFAULT_FPU32_TOL)) && (x > (y - DEFAULT_FPU32_TOL))) ? 1 : 0)
#define F64_IS_VALUE(x,y)   (((x < (y + DEFAULT_FPU64_TOL)) && (x > (y - DEFAULT_FPU64_TOL))) ? 1 : 0)

//! \brief          Determines numerical proximity to zero
//!
#define F32_IS_ZERO(x)      F32_IS_VALUE(x,0.0f)
#define F64_IS_ZERO(x)      F64_IS_VALUE(x,0.0L)

//! \brief          Returns a random floating point result between -1.0 and +1.0
//!                 where 'a' is a single or double precision float.
//!                 Useful for initialization of arrays and matrices during test.
//!					Ensure compiler switch "--fp_mode = relaxed" is set to ensure h/w division.
//!
//! \code
//! float32_t s = DCL_randf(1.0f);
//!
#define DCL_randf(a)         (a * ((float32_t) rand() / (float32_t) (RAND_MAX >> 1) - 1.0f))
#define DCL_randf64(a)       (a * ((float64_t) rand() / (float64_t) (RAND_MAX >> 1) - 1.0L))

//! \brief          Defines the lower limit on derivative filter coefficient c2
//!                 in order for fc to lie below the Nyquist frequency
//!
#define DCL_C2_LIMIT_32    ((2.0f - CONST_PI_32) / (2.0f + CONST_PI_32))
#define DCL_C2_LIMIT_64    ((2.0L - CONST_PI_64) / (2.0L + CONST_PI_64))

//! \brief          Define the default control period in seconds
//!
#define DCL_DEFAULT_T_F32 100.0e-06f
#define DCL_DEFAULT_T_F64 100.0e-06L

//! \brief          Build the library with controller test points enabled
//!
#define DCL_TESTPOINTS_ENABLED

//! \brief          Build the library with break points enabled
//!
#define DCL_BREAK_POINTS_ENABLED

//! \brief          Defines a debug break point
//!
#ifdef DCL_BREAK_POINTS_ENABLED
#define DCL_BREAK_POINT asm(" ESTOP0")
#else
#define DCL_BREAK_POINT ;
#endif

//! \brief          Boundary instructions for atomic code blocks
//!                 Global interrupt mask is set at start of atomic block and
//!                 restored to its' original value at the end
//!
#ifdef __TMS320C28XX__
#define DCL_DISABLE_INTS    DCL_disableInts()
#define DCL_RESTORE_INTS(v) DCL_restoreInts(v)

//! \brief          Sets global interrupts mask
//! \return         Original ST1 register value
//!
static inline int16_t DCL_disableInts(void)
{
    return __disable_interrupts();
}

//! \brief          Restores global interrupts if they were previously enabled
//! \param[in] v    Original ST1 register value
//! \return         None
//!
static inline void DCL_restoreInts(int16_t v)
{
    if (0 == (v & 0x1))
    {
        __enable_interrupts();
    }
}

#else
#define DCL_DISABLE_INTS    0
#define DCL_RESTORE_INTS(v) ;
#warning "CPU not recognized.  Parameter updates may not be atomic."
#endif

//--- Controller Common Support Structure ------------------------------------

//! \brief          Defines the controller common support structure
//!
//! \details        The CSS is accessed via a pointer in most of the DCL
//!                 controller structs.  It contains data used for testing and
//!                 configuring the controller, as well as for error checking.
//!
typedef struct dcl_css {
    float32_t tpt;      //!< Test point
    float32_t T;        //!< Controller period in seconds
    uint32_t sts;       //!< Status word
    uint32_t err;       //!< Error code
    uint32_t loc;       //!< Error location
} DCL_CSS;

//! \brief          Default values to initialize the CSS structure
//!
#define DCL_CSS_DEFAULTS { 0.0f, DCL_DEFAULT_T_F32, 0UL, 0UL, 0UL }

//! \brief          Loads the controller period in the CSS
//!                 CSS pointer must be configured first
//! \param[in] p    Pointer to the controller structure
//! \param[in] a    Sample period in seconds
//! \return         None
//!
#define DCL_SET_CONTROLLER_PERIOD(p,a)  ((p)->css->T = a)

//! \brief          Re-definition of legacy controller period call
//!
#define	DCL_SET_SAMPLE_RATE		DCL_SET_CONTROLLER_PERIOD


//--- Status word ------------------------------------------------------------

//! \brief          Defines the library enumerated status bits
//!
//! \details        To perform a safe parameter update, the user first loads new
//!                 parameters into the controller shadow parameter set, then sets
//!                 the STS_UPDATE_PENDING bit in the controller status word.  The
//!                 next call to the update function performs the "shadow-to-
//!                 active" set copy while global interrupts are disabled.
//!
enum dcl_status_bits {
    STS_NONE = 0UL,                     //!< Status empty
    STS_UPDATE_PENDING = (1U << 0),    //!< Parameter update pending
    STS_CONTROLLER_RUNNING = (1U << 1), //!< Controller operation in progress
    STS_ADJUSTMENT_RUNNING = (1U << 2) //!< Parameter adjustment in progress
};

//! \brief          Macros to set the update flag in the status word to initiate
//!                 controller parameter update on next DCL_update() call, and to
//!                 clear the flag on completion.
//!
#define DCL_REQUEST_UPDATE(p)           ((p)->css->sts |= STS_UPDATE_PENDING)
#define DCL_CLEAR_UPDATE_REQUEST(p)     ((p)->css->sts &= ~STS_UPDATE_PENDING)

//! \brief          Macro to determine whether a parameter update is pending
//!                 based on the STS_UPDATE_PENDING bit in the status word.
#define DCL_UPDATE_WAITING(p)           (0U != ((p)->css->sts & STS_UPDATE_PENDING))

//! \brief          Macros placed at the beginning and end of the controller
//!                 so that other functions know a control operation is in
//!                 progress.  Typically only used with complex controllers
//!                 which may not be atomic.
//!
#define DCL_CONTROLLER_BEGIN(p)         ((p)->css->sts |= STS_CONTROLLER_RUNNING)
#define DCL_CONTROLLER_END(p)           ((p)->css->sts &= ~STS_CONTROLLER_RUNNING)

//! \brief          Macro to determine whether a controller is being executed
//!                 based on the DCL_CONTROLLER_RUNNING bit in the status word.
#define DCL_CONTROLLER_IN_PROGRESS(p)   (0U != ((p)->css->sts & STS_CONTROLLER_RUNNING))

//! \brief          Macros to set the flag in the status word to denote
//!                 that parameter adjustment is in progress, and to
//!                 clear the flag when the target is reached.
//!
#define DCL_ADJUSTMENT_RUNNING(p)       ((p)->css->sts |= STS_ADJUSTMENT_RUNNING)
#define DCL_ADJUSTMENT_COMPLETE(p)      ((p)->css->sts &= ~STS_ADJUSTMENT_RUNNING)

//! \brief          Macro to determine whether parameter adjustment is underway
//!                 based on the DCL_ADJUSTMENT_RUNNING bit in the status word.
#define DCL_ADJUSTMENT_IN_PROGRESS(p)   (0U != ((p)->css->sts & STS_ADJUSTMENT_RUNNING))


//--- Error handling ---------------------------------------------------------

//! \brief          Build the library with error handling enabled
//!
#define DCL_ERROR_HANDLING_ENABLED

//! \brief          Defines the library enumerated error codes
//!                 These will be applied as bit masks in the error handler
//!
enum dcl_error_codes {
    ERR_NONE = 0U,                     //!< No error
    ERR_PARAM_RANGE = (1U << 0),       //!< Parameter range exceeded
    ERR_PARAM_INVALID = (1U << 1),     //!< Parameter not valid
    ERR_PARAM_WARN = (1U << 2),        //!< Parameter warning
    ERR_INPUT_RANGE = (1U << 3),       //!< Input range exceeded
    ERR_OVERFLOW = (1U << 4),          //!< Numerical overflow
    ERR_UNDERFLOW = (1U << 5),         //!< Numerical underflow
    ERR_VERSION = (1U << 6),           //!< Incorrect DCL version
    ERR_DEVICE = (1U << 7),            //!< Device not supported
    ERR_CONTROLLER = (1U << 8),        //!< Controller operation not completed
    ERR_TIMING = (1U << 9),            //!< Timing error
    ERR_COMPUTATION = (1U << 10)       //!< Computation error
};

//! \brief          Macro to clear stored error code in CSS
//!
#define DCL_CLEAR_ERROR_CODE(p)     (p->css->err = ERR_NONE)

//! \brief          Macro to store code location of error in CSS
//!
#define DCL_GET_ERROR_LOC(n)        (n->loc = (ERR_NONE == n->err) ? NULL_ADDR : __LINE__)

//! \brief          Define error handling routine
//!
#define DCL_RUN_ERROR_HANDLER(n)    DCL_runErrorHandler(n)

//! \brief          Prototype for external basic error handler [DCL_error.c]
//! \param[in] p    Pointer to DCL_CSS structure
//! \return         None
//!
extern void DCL_runErrorHandler(DCL_CSS *p);


//--- Polynomial stability functions -----------------------------------------

//! \brief          Determines stability of a first order real polynomial
//!                 P(z) = z + a1
//! \param[in] a1   Coefficient a1
//! \return         'true' if the root has magnitude less than 1, 'false' otherwise
//!
static inline bool DCL_isStablePn1(float32_t a1)
{
    return(((a1 * a1) < 1.0f) ? true : false);
}

//! \brief          Determines stability of a second order polynomial with real coefficients
//!                 P(z) = a0 z^2 + a1 z + a2
//! \param[in] a1   Coefficient a1
//! \param[in] a2   Coefficient a2
//! \return         'true' if both roots have magnitude less than 1, 'false' otherwise
//!
static inline bool DCL_isStablePn2(float32_t a0, float32_t a1, float32_t a2)
{
    float32_t b0, b1, c0;

    b0 = a0 - a2 * a2 / a0;
    b1 = a1 - a1 * a2 / a0;
    c0 = b0 - b1 * b1 / b0;

    if ((a0 > 0.0f) && (b0 > 0.0f) && (c0 > 0.0f))
    {
        return(true);
    }
    else
    {
        return(false);
    }
}

//! \brief          Determines stability of a third order polynomial with real coefficients
//!                 P(z) = a0 z^3 + a1 z^2 + a2 z + a3
//! \param[in] a1   Coefficient a1
//! \param[in] a2   Coefficient a2
//! \param[in] a3   Coefficient a3
//! \return         'true' if all roots have magnitude less than 1, 'false' otherwise
//!
static inline bool DCL_isStablePn3(float32_t a0, float32_t a1, float32_t a2, float32_t a3)
{
    float32_t b0, b1, b2, c0, c1, d0;

    b0 = a0 - a3 * a3 / a0;
    b1 = a1 - a2 * a3 / a0;
    b2 = a2 - a1 * a3 / a0;
    c0 = b0 - b2 * b2 / b0;
    c1 = b1 - b1 * b2 / b0;
    d0 = c0 - c1 * c1 / c0;

    if ((a0 > 0.0f) && (b0 > 0.0f) && (c0 > 0.0f) && (d0 > 0.0f))
    {
        return(true);
    }
    else
    {
        return(false);
    }
}


//--- ZPK3 structure ---------------------------------------------------------

//! \brief          Defines the DCL_ZPK3 controller structure.
//!
//! \details        Allows controllers to be defined in terms of complex pole
//!                 and zero frequencies.  The common structure consists of
//!                 three complex zeros, three complex poles, and a real gain.
//!                 All frequencies must be specified in radians/sec.
//!
typedef struct dcl_zpk3 {
    float complex z1;
    float complex z2;
    float complex z3;
    float complex p1;
    float complex p2;
    float complex p3;
    float32_t K;
} DCL_ZPK3;

//! \brief          Defines default values to initialize the DCL_ZPK3 structure
//!
#define ZPK3_DEFAULTS { 0.0f + 0.0f*I, 0.0f + 0.0f*I, 0.0f + 0.0f*I, \
                        0.0f + 0.0f*I, 0.0f + 0.0f*I, 0.0f + 0.0f*I, \
                        1.0f }

//! \brief          Determines stability of a ZPK3 representation by checking pole magnitude
//! \param[in] q    Pointer to DCL_ZPK3 structure
//! \return         'true' if all poles have magnitude less than 1, 'false' otherwise
//!
static inline bool DCL_isStableZpk3(DCL_ZPK3 *q)
{
    return(((cabsf(q->p1) < 1.0f) && (cabsf(q->p2) < 1.0f) && (cabsf(q->p3) < 1.0f)) ? true : false);
}


//----------------------------------------------------------------------------

#endif // __TMS320C28XX_CLA__

#ifdef __cplusplus
}
#endif // extern "C"

#endif // _C_DCL_H

/* end of file */