/****************************************************************************
 * apps/industry/foc/fixed16/foc_align.c
 * This file implements angle sensor alignment routine for fixed16
 *
 * 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 <nuttx/config.h>

#include <assert.h>
#include <errno.h>
#include <stdlib.h>

#include "industry/foc/foc_common.h"
#include "industry/foc/foc_log.h"

#include "industry/foc/fixed16/foc_align.h"

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

/* Align to -90deg = 270deg */

#define FOC_ALIGN_ANGLE        (b16muli(b16HALFPI, 3))

/* Direction alignment */

#define ALIGN_DIR_ANGLE_STEP   (ftob16(0.001f))
#define ALIGN_DIR_ANGLE_HOLD_0 (ftob16(0.0f))
#define ALIGN_DIR_ANGLE_HOLD_1 (b16idiv(b16PI, 3))
#define ALIGN_DIR_ANGLE_HOLD_2 (b16muli(b16idiv(b16PI, 3), 2))
#define ALIGN_DIR_ANGLE_HOLD_3 (b16PI)
#define ALIGN_DIR_ANGLE_HOLD_4 (b16muli(b16idiv(b16PI, 3), 4))
#define ALIGN_DIR_HOLD_CNTR    (10)

/* IDLE steps */

#define IDLE_STEPS             (500)

/****************************************************************************
 * Private Data Types
 ****************************************************************************/

/* Align stage */

enum foc_align_run_stage_e
{
  FOC_ALIGN_RUN_INIT,
  FOC_ALIGN_RUN_OFFSET,
  FOC_ALIGN_RUN_DIR,
  FOC_ALIGN_RUN_IDLE,
  FOC_ALIGN_RUN_DONE
};

/* Align routine private data */

struct foc_align_b16_s
{
  struct foc_routine_align_cfg_b16_s   cfg;
  struct foc_routine_aling_final_b16_s final;
  int                                  cntr;
  int                                  stage;
  int                                  dir_step;
  b16_t                                dir_angle;
  b16_t                                angle_last;
  b16_t                                angle_now;
  b16_t                                diff_cw;
  b16_t                                diff_ccw;
};

/****************************************************************************
 * Private Function Prototypes
 ****************************************************************************/

int foc_routine_align_init_b16(FAR foc_routine_b16_t *r);
void foc_routine_align_deinit_b16(FAR foc_routine_b16_t *r);
int foc_routine_align_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg);
int foc_routine_align_run_b16(FAR foc_routine_b16_t *r,
                              FAR struct foc_routine_in_b16_s *in,
                              FAR struct foc_routine_out_b16_s *out);
int foc_routine_align_final_b16(FAR foc_routine_b16_t *r, FAR void *data);

/****************************************************************************
 * Public Data
 ****************************************************************************/

struct foc_routine_ops_b16_s g_foc_routine_align_b16 =
{
  .init   = foc_routine_align_init_b16,
  .deinit = foc_routine_align_deinit_b16,
  .cfg    = foc_routine_align_cfg_b16,
  .run    = foc_routine_align_run_b16,
  .final  = foc_routine_align_final_b16,
};

/****************************************************************************
 * Private Functions
 ****************************************************************************/

/****************************************************************************
 * Name: foc_align_offset_run_b16
 *
 * Description:
 *   Run offset alignment routine
 *
 * Input Parameter:
 *   align - pointer to FOC align routine
 *   in    - pointer to FOC routine input data
 *   out   - pointer to FOC routine output data
 *
 ****************************************************************************/

static int foc_align_offset_run_b16(FAR struct foc_align_b16_s *align,
                                    FAR struct foc_routine_in_b16_s *in,
                                    FAR struct foc_routine_out_b16_s *out)
{
  int ret = FOC_ROUTINE_RUN_NOTDONE;

  /* Reset Align angle sensor */

  if (align->cntr > align->cfg.offset_steps)
    {
      /* Align angle sensor */

      if (align->cfg.cb.zero != NULL)
        {
          ret = align->cfg.cb.zero(align->cfg.cb.priv, in->angle);
          if (ret < 0)
            {
              FOCLIBERR("ERROR: align offset callback failed %d!\n", ret);
              goto errout;
            }
        }

      /* Offset align done */

      align->cntr = 0;
      ret         = FOC_ROUTINE_RUN_DONE;

      /* Force IDLE mode */

      out->dq_ref.q   = 0;
      out->dq_ref.d   = 0;
      out->vdq_comp.q = 0;
      out->vdq_comp.d = 0;
      out->angle      = 0;
      out->foc_mode   = FOC_HANDLER_MODE_IDLE;
    }
  else
    {
      /* Force DQ voltage vector */

      out->dq_ref.q   = align->cfg.volt;
      out->dq_ref.d   = 0;
      out->vdq_comp.q = 0;
      out->vdq_comp.d = 0;
      out->angle      = FOC_ALIGN_ANGLE;
      out->foc_mode   = FOC_HANDLER_MODE_VOLTAGE;

      /* Increase counter */

      align->cntr += 1;
    }

  /* Store offset if done */

  if (ret == FOC_ROUTINE_RUN_DONE)
    {
      align->final.offset = in->angle;
    }

errout:
  return ret;
}

/****************************************************************************
 * Name: foc_align_dir_move_b16
 ****************************************************************************/

static void foc_align_dir_move_b16(FAR struct foc_align_b16_s *align,
                                   b16_t dir, b16_t dest)
{
  bool next = false;

  DEBUGASSERT(align);

  /* Update angle */

  align->dir_angle += b16mulb16(dir, ALIGN_DIR_ANGLE_STEP);

  /* Check destination */

  if (dir == DIR_CW_B16)
    {
      if (align->dir_angle >= dest)
        {
          next = true;
        }
    }

  else if (dir == DIR_CCW_B16)
    {
      if (align->dir_angle <= dest)
        {
          next = true;
        }
    }
  else
    {
      DEBUGASSERT(0);
    }

  /* Next step */

  if (next == true)
    {
      align->dir_step += 1;
    }
}

/****************************************************************************
 * Name: foc_align_dir_hold_b16
 ****************************************************************************/

static void foc_align_dir_hold_b16(FAR struct foc_align_b16_s *align,
                                   b16_t dir, bool last, bool diff)
{
  DEBUGASSERT(align);

  /* Lock angle */

  align->dir_angle = align->dir_angle;

  /* Increase counter */

  align->cntr += 1;

  if (align->cntr > ALIGN_DIR_HOLD_CNTR)
    {
      /* Next step */

      align->dir_step += 1;

      /* Accumulate diff */

      if (diff == true)
        {
          if (dir == DIR_CW_B16)
            {
              align->diff_cw += (align->angle_now - align->angle_last);
            }
          else if (dir == DIR_CCW_B16)
            {
              align->diff_ccw += (align->angle_now - align->angle_last);
            }
          else
            {
              DEBUGASSERT(0);
            }
        }

      /* Store last angle */

      if (last == true)
        {
          align->angle_last = align->angle_now;
        }

      /* Reset counter */

      align->cntr = 0;
    }
}

/****************************************************************************
 * Name: foc_align_dir_run_b16
 *
 * Description:
 *   Run direction alignment routine
 *
 * Input Parameter:
 *   align - pointer to FOC align routine
 *   in    - pointer to FOC routine input data
 *   out   - pointer to FOC routine output data
 *
 ****************************************************************************/

int foc_align_dir_run_b16(FAR struct foc_align_b16_s *align,
                          FAR struct foc_routine_in_b16_s *in,
                          FAR struct foc_routine_out_b16_s *out)
{
  int  ret  = FOC_ROUTINE_RUN_NOTDONE;
  bool last = false;

  /* Store angle now */

  align->angle_now = in->angle;

  /* Handle dir align */

  if (align->dir_step == 0)
    {
      /* Start with position 0 */

      align->dir_angle = ALIGN_DIR_ANGLE_HOLD_0;

      /* Step 0 - hold position 0 */

      foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
    }

  else if (align->dir_step == 1)
    {
      /* Step 1 - move motor in CW direction to position 1 */

      foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_1);
    }

  else if (align->dir_step == 2)
    {
      /* Step 2 - hold position 1 */

      foc_align_dir_hold_b16(align, DIR_CW_B16, true, false);
    }

  else if (align->dir_step == 3)
    {
      /* Step 3 - move motor in CW direction to position 2 */

      foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_2);
    }

  else if (align->dir_step == 4)
    {
      /* Step 4 - hold position 2 */

      foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
    }

  else if (align->dir_step == 5)
    {
      /* Step 5 - move motor in CW direction to position 3 */

      foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_3);
    }

  else if (align->dir_step == 6)
    {
      /* Step 6 - hold position 3 */

      foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
    }

  else if (align->dir_step == 6)
    {
      /* Step 6 - move motor in CW direction to position 4 */

      foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_4);
    }

  else if (align->dir_step == 7)
    {
      /* Step 7 - hold position 4 */

      foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
    }

  else if (align->dir_step == 8)
    {
      /* Step 8 - move motor in CCW direction to position 3 */

      foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_3);
    }

  else if (align->dir_step == 9)
    {
      /* Step 9 - hold position 3 */

      foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
    }

  else if (align->dir_step == 10)
    {
      /* Step 10 - move motor in CCW direction to position 2 */

      foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_2);
    }

  else if (align->dir_step == 11)
    {
      /* Step 11 - hold position 2 */

      foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
    }

  else if (align->dir_step == 12)
    {
      /* Step 12 - move motor in CCW direction to position 1 */

      foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_1);
    }

  else if (align->dir_step == 13)
    {
      /* Step 13 - hold position 1 */

      foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
    }

  else if (align->dir_step == 14)
    {
      /* Step 14 - move motor in CCW direction to position 0 */

      foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_0);
    }

  else if (align->dir_step == 15)
    {
      /* Step 15 - hold position 0 */

      foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
    }

  else
    {
      /* Set flag */

      last = true;

      /* Get sensor direction according to sampled data */

      if (align->diff_cw > 0 && align->diff_ccw < 0)
        {
          align->final.dir = DIR_CW_B16;
        }

      else if (align->diff_cw < 0 && align->diff_ccw > 0)
        {
          align->final.dir = DIR_CCW_B16;
        }

      else
        {
          /* Invalid data */

          FOCLIBERR("ERROR: direction align failed !\n");

          align->final.dir = DIR_NONE_B16;
          ret              = -EINVAL;

          goto errout;
        }

      /* Align angle sensor */

      if (align->cfg.cb.dir != NULL)
        {
          ret = align->cfg.cb.dir(align->cfg.cb.priv, align->final.dir);
          if (ret < 0)
            {
              FOCLIBERR("ERROR: align dir callback failed %d!\n", ret);
              goto errout;
            }
        }

      /* Direction alignment done */

      ret = FOC_ROUTINE_RUN_DONE;
    }

  /* Force DQ voltage vector */

  out->dq_ref.q   = align->cfg.volt;
  out->dq_ref.d   = 0;
  out->vdq_comp.q = 0;
  out->vdq_comp.d = 0;
  out->angle      = align->dir_angle;
  out->foc_mode   = FOC_HANDLER_MODE_VOLTAGE;

errout:

  /* Force current to zero if alignment done */

  if (last == true)
    {
      out->dq_ref.q = 0;
      out->dq_ref.d = 0;
    }

  return ret;
}

/****************************************************************************
 * Name: foc_align_idle_run_b16
 *
 * Description:
 *   Force IDLE state
 *
 * Input Parameter:
 *   align - pointer to FOC align routine
 *   in    - pointer to FOC routine input data
 *   out   - pointer to FOC routine output data
 *
 ****************************************************************************/

static int foc_align_idle_run_b16(FAR struct foc_align_b16_s *align,
                                    FAR struct foc_routine_in_b16_s *in,
                                    FAR struct foc_routine_out_b16_s *out)
{
  int ret = FOC_ROUTINE_RUN_NOTDONE;

  /* Get output */

  out->dq_ref.q   = 0;
  out->dq_ref.d   = 0;
  out->vdq_comp.q = 0;
  out->vdq_comp.d = 0;
  out->angle      = 0;
  out->foc_mode   = FOC_HANDLER_MODE_IDLE;

  /* Increase counter */

  align->cntr += 1;

  if (align->cntr > IDLE_STEPS)
    {
      /* Done */

      ret = FOC_ROUTINE_RUN_DONE;

      /* Reset counter */

      align->cntr = 0;
    }

  return ret;
}

/****************************************************************************
 * Name: foc_routine_align_init_b16
 *
 * Description:
 *   Initialize the FOC align routine (fixed16)
 *
 * Input Parameter:
 *   r - pointer to FOC routine
 *
 ****************************************************************************/

int foc_routine_align_init_b16(FAR foc_routine_b16_t *r)
{
  int ret = OK;

  DEBUGASSERT(r);

  /* Connect angle data */

  r->data = zalloc(sizeof(struct foc_align_b16_s));
  if (r->data == NULL)
    {
      ret = -ENOMEM;
      goto errout;
    }

errout:
  return ret;
}

/****************************************************************************
 * Name: foc_routine_align_deinit_b16
 *
 * Description:
 *   Deinitialize the FOC align routine (fixed16)
 *
 * Input Parameter:
 *   r - pointer to FOC routine
 *
 ****************************************************************************/

void foc_routine_align_deinit_b16(FAR foc_routine_b16_t *r)
{
  DEBUGASSERT(r);

  if (r->data)
    {
      /* Free routine data */

      free(r->data);
    }
}

/****************************************************************************
 * Name: foc_routine_align_cfg_b16
 *
 * Description:
 *   Configure the FOC align routine (fixed16)
 *
 * Input Parameter:
 *   r   - pointer to FOC routine
 *   cfg - pointer to align routine configuration data
 *
 ****************************************************************************/

int foc_routine_align_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg)
{
  FAR struct foc_align_b16_s *a   = NULL;
  int                         ret = OK;

  DEBUGASSERT(r);
  DEBUGASSERT(cfg);

  /* Get aling data */

  DEBUGASSERT(r->data);
  a = r->data;

  /* Copy configuration */

  memcpy(&a->cfg, cfg, sizeof(struct foc_routine_align_cfg_b16_s));

  /* Verify configuration */

  if (a->cfg.volt <= 0)
    {
      ret = -EINVAL;
      goto errout;
    }

  if (a->cfg.offset_steps <= 0)
    {
      ret = -EINVAL;
      goto errout;
    }

  if (a->cfg.cb.zero == NULL || a->cfg.cb.dir == NULL ||
      a->cfg.cb.priv == NULL)
    {
      FOCLIBWARN("WARN: no align cb data!\n");
    }

errout:
  return ret;
}

/****************************************************************************
 * Name: foc_routine_align_run_b16
 *
 * Description:
 *   Run the FOC align routine step (fixed16)
 *
 * Input Parameter:
 *   r   - pointer to FOC routine
 *   in  - pointer to FOC routine input data
 *   out - pointer to FOC routine output data
 *
 ****************************************************************************/

int foc_routine_align_run_b16(FAR foc_routine_b16_t *r,
                              FAR struct foc_routine_in_b16_s *in,
                              FAR struct foc_routine_out_b16_s *out)
{
  FAR struct foc_align_b16_s *a   = NULL;
  int                         ret = FOC_ROUTINE_RUN_NOTDONE;

  DEBUGASSERT(r);
  DEBUGASSERT(in);
  DEBUGASSERT(out);

  /* Get aling data */

  DEBUGASSERT(r->data);
  a = r->data;

  /* Force IDLE state at default */

  out->dq_ref.q   = 0;
  out->dq_ref.d   = 0;
  out->vdq_comp.q = 0;
  out->vdq_comp.d = 0;
  out->angle      = 0;
  out->foc_mode   = FOC_HANDLER_MODE_IDLE;

  /* Handle alignment stage */

  switch (a->stage)
    {
      case FOC_ALIGN_RUN_INIT:
        {
          a->stage += 1;
          ret = FOC_ROUTINE_RUN_NOTDONE;

          break;
        }

    case FOC_ALIGN_RUN_OFFSET:
      {
        /* Align zero procedure */

        ret = foc_align_offset_run_b16(a, in, out);
        if (ret < 0)
          {
            goto errout;
          }

        if (ret == FOC_ROUTINE_RUN_DONE)
          {
            FOCLIBLOG("ALIGN OFFSET done!\n");

            a->stage += 1;
            ret = FOC_ROUTINE_RUN_NOTDONE;
          }

        break;
      }

      case FOC_ALIGN_RUN_DIR:
        {
          /* Align direction procedure */

          ret = foc_align_dir_run_b16(a, in, out);
          if (ret < 0)
            {
              goto errout;
            }

          if (ret == FOC_ROUTINE_RUN_DONE)
            {
              FOCLIBLOG("ALIGN DIR done!\n");

              a->stage += 1;
              ret = FOC_ROUTINE_RUN_NOTDONE;
            }

          break;
        }

      case FOC_ALIGN_RUN_IDLE:
        {
          /* De-energetize motor */

          ret = foc_align_idle_run_b16(a, in, out);
          if (ret < 0)
            {
              goto errout;
            }

          if (ret == FOC_ROUTINE_RUN_DONE)
            {
              FOCLIBLOG("ALIGN IDLE done!\n");

              a->stage += 1;
              ret = FOC_ROUTINE_RUN_NOTDONE;
            }

          break;
        }

      case FOC_ALIGN_RUN_DONE:
        {
          ret = FOC_ROUTINE_RUN_DONE;

          break;
        }

      default:
        {
          FOCLIBERR("ERROR: invalid alignment stage %d\n", a->stage);
          ret = -EINVAL;
          goto errout;
        }
    }

errout:
  return ret;
}

/****************************************************************************
 * Name: foc_routine_align_final_b16
 *
 * Description:
 *   Finalize the FOC routine data (fixed16)
 *
 * Input Parameter:
 *   r    - pointer to FOC routine
 *   data - pointer to FOC align routine final data
 *
 ****************************************************************************/

int foc_routine_align_final_b16(FAR foc_routine_b16_t *r, FAR void *data)
{
  FAR struct foc_align_b16_s *a = NULL;

  DEBUGASSERT(r);
  DEBUGASSERT(data);

  /* Get aling data */

  DEBUGASSERT(r->data);
  a = r->data;

  /* Get final data */

  memcpy(data, &a->final, sizeof(struct foc_routine_aling_final_b16_s));

  return OK;
}