/*
 * Copyright (c) 2021-2022, Erich Styger
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include "platform.h"
#include "McuLib.h"
#include "MotOffsetTable.h"

static const struct {
  uint8_t motorID;
  int8_t offset;
} motorOffsets[] =
  { /* table of offset for each motor
    id  offset */
    {0, 35},
    {1, 17},
    {2, 13},
    {3, 17},
    {4,  3},
    {5, 29},
    {6, 10},
    {7, 35},
    {8, 27},
    {9, 23},
    {10,28},
    {11,33},
    {12,40},
    {13,10},
    {14,32},
    {15,37},
    {16,32},
    {17,32},
    {18,32},
    {19,31},
    {21,15},
    {22,17},
    {23,13},
    {24,25},
    {25,10},
    {26,14},
    {27,15},
    {28,16},
    {29,17},
    {30,18},
    {31,22},
    {32,33},
    {33,22},
    {34,23},
    {35,15},
    {36,20},
    {37,15},
    {38,15},
    {50,10},
  };

uint8_t MotOffset_Get(uint8_t motorID, int32_t *offsetp) {
  /* search motor in table */
  for(int i=0; i<sizeof(motorOffsets)/sizeof(motorOffsets[0]); i++) {
    if (motorID==motorOffsets[i].motorID) { /* found */
      *offsetp = motorOffsets[i].offset;
      return ERR_OK;
    }
  }
  /* not found */
  return ERR_FAILED;
}