/****************************************************************************
 * libs/libdsp/lib_motor_b16.c
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.  The
 * ASF licenses this file to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance with the
 * License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
 * License for the specific language governing permissions and limitations
 * under the License.
 *
 ****************************************************************************/

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include <dspb16.h>
#include <string.h>

/****************************************************************************
 * Pre-processor Definitions
 ****************************************************************************/

#define POLE_CNTR_THR (0)

/****************************************************************************
 * Public Functions
 ****************************************************************************/

/****************************************************************************
 * Name: motor_openloop_init_b16
 *
 * Description:
 *  Initialize open-loop data
 *
 * Input Parameters:
 *   op  - (in/out) pointer to the openloop data structure
 *   per - (in) period of the open-loop control
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_openloop_init_b16(FAR struct openloop_data_b16_s *op, b16_t per)
{
  LIBDSP_DEBUGASSERT(op != NULL);
  LIBDSP_DEBUGASSERT(per > 0);

  /* Reset openloop structure */

  memset(op, 0, sizeof(struct openloop_data_b16_s));

  /* Initialize data */

  op->per = per;
}

/****************************************************************************
 * Name: motor_openloop_b16
 *
 * Description:
 *   One step of the open-loop control
 *
 * Input Parameters:
 *   op    - (in/out) pointer to the open-loop data structure
 *   speed - (in) open-loop speed
 *   dir   - (in) rotation direction
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_openloop_b16(FAR struct openloop_data_b16_s *op, b16_t speed,
                        b16_t dir)
{
  LIBDSP_DEBUGASSERT(op != NULL);
  LIBDSP_DEBUGASSERT(speed >= 0);
  LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16);

  b16_t phase_step = 0;
  b16_t tmp = 0;

  /* Get phase step */

  tmp = b16mulb16(dir, speed);
  phase_step =  b16mulb16(tmp, op->per);

  /* Update open-loop angle */

  op->angle += phase_step;

  /* Normalize the open-loop angle to 0.0 - 2PI range */

  angle_norm_2pi_b16(&op->angle, MOTOR_ANGLE_E_MIN_B16,
                     MOTOR_ANGLE_E_MAX_B16);
}

/****************************************************************************
 * Name: motor_openloop_angle_get_b16
 *
 * Description:
 *   Get angle from open-loop controller
 *
 * Input Parameters:
 *   op    - (in/out) pointer to the open-loop data structure
 *
 * Returned Value:
 *   Return angle from open-loop controller
 *
 ****************************************************************************/

b16_t motor_openloop_angle_get_b16(FAR struct openloop_data_b16_s *op)
{
  LIBDSP_DEBUGASSERT(op != NULL);

  return op->angle;
}

/****************************************************************************
 * Name: motor_angle_e_update_b16
 *
 * Description:
 *   Update motor angle structure using electrical motor angle.
 *
 * Input Parameters:
 *   angle     - (in/out) pointer to the motor angle structure
 *   angle_new - (in) new motor electrical angle in range <0.0, 2PI>
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_angle_e_update_b16(FAR struct motor_angle_b16_s *angle,
                              b16_t angle_new, b16_t dir)
{
  LIBDSP_DEBUGASSERT(angle != NULL);
  LIBDSP_DEBUGASSERT(angle_new >= 0 && angle_new <= MOTOR_ANGLE_E_MAX_B16);
  LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16);

  b16_t tmp1 = 0;
  b16_t tmp2 = 0;

  /* Check if we crossed electrical angle boundaries */

  if (dir == DIR_CW_B16)
    {
      /* For CW direction - previous angle is greater than current angle */

      if (angle_new - angle->angle_el.angle < -POLE_CNTR_THR)
        {
          angle->i += 1;
        }
    }

  else if (dir == DIR_CCW_B16)
    {
      /* For CCW direction - previous angle is lower than current angle */

      if (angle_new - angle->angle_el.angle > POLE_CNTR_THR)
        {
          angle->i -= 1;
        }
    }

  /* Reset pole counter if needed */

  if (angle->i >= angle->p)
    {
      angle->i = 0;
    }

  else if (angle->i < 0)
    {
      angle->i = angle->p - 1;
    }

  /* Update electrical angle structure */

  phase_angle_update_b16(&angle->angle_el, angle_new);

  /* Calculate mechanical angle.
   * One electrical angle rotation is equal to one mechanical rotation
   * divided by number of motor pole pairs.
   */

  tmp1 = b16mulb16(MOTOR_ANGLE_E_RANGE_B16, itob16(angle->i));
  tmp2 = tmp1 + angle->angle_el.angle;

  angle->anglem = b16mulb16(tmp2, angle->one_by_p);

  /* Normalize mechanical angle to <0, 2PI> and store */

  angle_norm_2pi_b16(&angle->anglem, MOTOR_ANGLE_M_MIN_B16,
                     MOTOR_ANGLE_M_MAX_B16);
}

/****************************************************************************
 * Name: motor_angle_m_update_b16
 *
 * Description:
 *   Update motor angle structure using mechanical motor angle
 *
 * Input Parameters:
 *   angle     - (in/out) pointer to the motor angle structure
 *   angle_new - (in) new motor mechanical angle in range <0.0, 2PI>
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_angle_m_update_b16(FAR struct motor_angle_b16_s *angle,
                              b16_t angle_new, b16_t dir)
{
  LIBDSP_DEBUGASSERT(angle != NULL);
  LIBDSP_DEBUGASSERT(angle_new >= 0 && angle_new <= MOTOR_ANGLE_E_MAX_B16);
  LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16);

  b16_t angle_el = 0;
  b16_t tmp1     = 0;
  b16_t tmp2     = 0;

  /* Store new mechanical angle */

  angle->anglem = angle_new;

  /* Update pole counter */

  tmp1 = b16mulb16(angle->anglem, itob16(angle->p));

  angle->i = (uint8_t) b16toi(b16divb16(tmp1, MOTOR_ANGLE_M_MAX_B16));

  /* Get electrical angle */

  tmp1 = b16mulb16(angle->anglem, itob16(angle->p));
  tmp2 = b16mulb16(MOTOR_ANGLE_E_MAX_B16, itob16(angle->i));

  angle_el = (tmp1 - tmp2);

  /* Update electrical angle structure */

  phase_angle_update_b16(&angle->angle_el, angle_el);
}

/****************************************************************************
 * Name: motor_angle_m_get_b16
 *
 * Description:
 *   Get motor mechanical angle
 *
 * Input Parameters:
 *   angle - (in/out) pointer to the motor angle structure
 *
 * Returned Value:
 *   Return motor mechanical angle
 *
 ****************************************************************************/

b16_t motor_angle_m_get_b16(FAR struct motor_angle_b16_s *angle)
{
  LIBDSP_DEBUGASSERT(angle != NULL);

  return angle->anglem;
}

/****************************************************************************
 * Name: motor_angle_e_get_b16
 *
 * Description:
 *   Get motor electrical angle
 *
 * Input Parameters:
 *   angle - (in/out) pointer to the motor angle structure
 *
 * Returned Value:
 *   Return motor electrical angle
 *
 ****************************************************************************/

b16_t motor_angle_e_get_b16(FAR struct motor_angle_b16_s *angle)
{
  LIBDSP_DEBUGASSERT(angle != NULL);

  return angle->angle_el.angle;
}

/****************************************************************************
 * Name: motor_phy_params_init_b16
 *
 * Description:
 *   Initialize motor physical parameters
 *
 * Input Parameters:
 *   phy   - (in/out) pointer to the motor physical parameters
 *   poles - (in) number of the motor pole pairs
 *   res   - (in) average phase-to-neutral base motor resistance
 *                (without temperature compensation)
 *   ind   - (in) average phase-to-neutral motor inductance
 *   flux  - (in) flux linkage
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_phy_params_init_b16(FAR struct motor_phy_params_b16_s *phy,
                               uint8_t poles, b16_t res, b16_t ind,
                               b16_t flux_link)
{
  LIBDSP_DEBUGASSERT(phy != NULL);

  memset(phy, 0, sizeof(struct motor_phy_params_b16_s));

  phy->p          = poles;
  phy->flux_link  = flux_link;
  phy->res        = res;
  phy->ind        = ind;
  phy->one_by_ind = b16divb16(b16ONE, ind);
  phy->one_by_p   = b16divb16(b16ONE, poles);
}

/****************************************************************************
 * Name: pmsm_phy_params_init_b16
 *
 * Description:
 *   Initialize PMSM physical parameters
 *
 * Input Parameters:
 *   phy   - (in/out) pointer to the PMSM physical parameters
 *   poles - (in) number of the motor pole pairs
 *   res   - (in) average phase-to-neutral base motor resistance
 *                (without temperature compensation)
 *   ind   - (in) average phase-to-neutral motor inductance
 *   iner  - (in) rotor inertia (J)
 *   flux  - (in) flux linkage
 *   ind_d - (in) d-inductance
 *   ind_q - (in) q-inductance
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void pmsm_phy_params_init_b16(FAR struct pmsm_phy_params_b16_s *phy,
                              uint8_t poles, b16_t res, b16_t ind,
                              b16_t iner, b16_t flux,
                              b16_t ind_d, b16_t ind_q)
{
  LIBDSP_DEBUGASSERT(phy != NULL);

  /* Initialize motor phy */

  motor_phy_params_init_b16(&phy->motor, poles, res, ind, flux);

  /* Iniitalize PMSM specific data */

  phy->iner        = iner;
  phy->ind_d       = ind_d;
  phy->ind_q       = ind_q;
  phy->one_by_iner = b16divb16(b16ONE, iner);
  phy->one_by_indd = b16divb16(b16ONE, ind_d);
  phy->one_by_indq = b16divb16(b16ONE, ind_q);
}