/*
 * hal_entory.c
 *
 *  Created on: 2023/10/07
 *      Author: BestTechnology
 */

// No-load RPM   23000rpm/12V
// Stall torque  6.79mN·m/1600mA/12V
// Gear ratio    312.4:1

#include "hal_data.h"

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <math.h>

//#define DBG_TRACE
#include "trace.h"

#include "adc_temp.h"
#include "angle.h"
#include "power_monitor.h"
#include "usart.h"
#include "global_variables.h"
#include "pid.h"
#include "interpolation.h"
#include "control_table.h"
#include "packet.h"
#include "sintable.h"
#include "monitor.h"

FSP_CPP_HEADER
void R_BSP_WarmStart(bsp_warm_start_event_t event);
FSP_CPP_FOOTER

////////////////////////////////////////////////////////////
// Variable & macros
////////////////////////////////////////////////////////////
#define APP_ERR_TRAP(a)     if(a) {__asm("BKPT #0\n");}

#define saturate(value,min,max)  ((value) < (min)) ? (min) : ((value) > (max)) ? (max) : (value)

// Transmit/receive buffer for UART
static uint8_t txb[4000], rxb[4000];

// For angle & speed calculation
#define _HISTORY_DEPTH_ANGLE    32  // value * 1ms
static volatile int32_t prev_ang, now_ang;
static volatile int32_t ang_ex;
static int32_t ang_history[_HISTORY_DEPTH_ANGLE];

static bool led_stat = false;

// Enumerate possible communications based on baud rates supported by FT232H
const uint32_t baudrate_list[15] = {
    9600, 19200, 38400, 57600, 100000, 115200, 200000, 230400, 460800, 500000, 1000000, 2000000, 3000000, 4000000, 6000000
};

void led_toggle (void);
void led_on (void);
void led_off (void);

////////////////////////////////////////////////////////////
// GPIO
////////////////////////////////////////////////////////////
static inline void set_motor_dir (bool cw) {
    R_IOPORT_PinWrite (&g_ioport_ctrl, BSP_IO_PORT_01_PIN_02, cw ? BSP_IO_LEVEL_HIGH : BSP_IO_LEVEL_LOW);
}

static inline void set_motor_enable (bool en) {
    R_IOPORT_PinWrite (&g_ioport_ctrl, BSP_IO_PORT_01_PIN_03, en ? BSP_IO_LEVEL_LOW : BSP_IO_LEVEL_HIGH);
}

void led_toggle (void) {
    led_stat = !led_stat;
    R_IOPORT_PinWrite (&g_ioport_ctrl, BSP_IO_PORT_01_PIN_01, led_stat ? BSP_IO_LEVEL_HIGH : BSP_IO_LEVEL_LOW);
}

void led_on (void) {
    led_stat = true;
    R_IOPORT_PinWrite (&g_ioport_ctrl, BSP_IO_PORT_01_PIN_01, BSP_IO_LEVEL_HIGH);
}

void led_off (void) {
    led_stat = false;
    R_IOPORT_PinWrite (&g_ioport_ctrl, BSP_IO_PORT_01_PIN_01, BSP_IO_LEVEL_LOW);
}

// Low-pass filter for current value
static double lowpass (volatile double *prev, double raw) {
    double k = 0.03;
    *prev += (k * (raw - *prev));
    return *prev;
}

////////////////////////////////////////////////////////////
// Control loop
////////////////////////////////////////////////////////////
// interpolation control
static inline int sline_cos (int Ang) {
  return (my_cos1000 (Ang * 10 / 3 + 1800) + 1000) >> 1;
}

static int32_t CurveInterpolation (uint8_t mode, int32_t st, int32_t en, int32_t interval, int32_t di) {
    switch (mode) {
        case 0: //uniform velocity
            return st + (((en - st) * di) / interval);
            break;
        case 1: //S-curve acceleration/deceleration
            return st + ((en - st) * sline_cos (((di * 540) / interval) % 1080)) / 1000;
            break;
        case 2: //S-curve acceleration
            return st + ((en - st) * sline_cos (((di * 270) / interval) % 1080)) / 500;
            break;
        case 3: //S-curve deceleration
            return st + ((en - st) * (sline_cos (((di * 270) / interval + 270) % 1080) - sline_cos (270))) / 500;
            break;
        default:
            return en;
            break;
    }
}

#define _MON_BLINK_INTERVAL 64

// Interval timer
void rgt0_callback (timer_callback_args_t *p_args) {
    static bool stall = false;
    static volatile uint8_t isl28022_targetreg = ISLREG_ADDR_BUSVOLTAGE;
    static uint16_t isl28022_read_data;
    static uint8_t angle_depth = 0;
    static double current = 0;
    static volatile uint8_t prevmode = sm_none;

    double calc = 0;
    int32_t lim;
    static int32_t TransitionTime = 0, prev_set_transition_time = 0;
    static uint8_t prev_set_transition_mode = 0;

    static int32_t prev_targetangle, StartAngle, TargetAngle;
    static int32_t prev_targetvelocity, StartVelocity, TargetVelocity;
    static int32_t prev_targetcurrent, StartCurrent, TargetCurrent;

    static int32_t LimitedTargetAngle;

    static int16_t MonCount = 0;

    if (p_args->event == TIMER_EVENT_CYCLE_END) {
        // Counter value to be incremented every 1ms
        ControlTable.SysTickTimee++;

        // Get angle from magnetic sensor
        uint16_t rawpos;
        uint8_t encstat;
        if (tad2141_readstream (&rawpos, &encstat)) {
            ControlTable.CurrentRawPosition = rawpos;
            now_ang = interp1dim ((int32_t)rawpos, interp_tad2141_1d, interp_tad2141_ar_w) + ControlTable.OffsetPresentAngle;
            ControlTable.SystemStatus.bit.encoderstat = encstat;

            // Support for multiple revolutions
            if (abs (prev_ang - now_ang) > 18000) {
                if (now_ang < prev_ang) ang_ex += 36000L;
                else if (now_ang > prev_ang) ang_ex -= 36000L;
            }
            ControlTable.CurrentAngle = (int32_t)now_ang + ang_ex;
            prev_ang = now_ang;
        } else {
            ControlTable.SystemStatus.bit.encoderstat = 0;
            ControlTable.SystemStatus.bit.encodererror = 1;
        }

        // Calculate angular velocity
        ang_history[angle_depth] = ControlTable.CurrentAngle;
        angle_depth = (uint8_t)((angle_depth + 1) % _HISTORY_DEPTH_ANGLE);
        // Convert x10deg/32ms to x1000rot/sec
        ControlTable.CurrentVelocity = (ControlTable.CurrentAngle - ang_history[angle_depth]) * 10000 / _HISTORY_DEPTH_ANGLE / 360;

        // Power Monitor
        //   The I2C flag wait is a lengthy process, so it is handled within the I2C callback.
        static /*volatile*/ double stat_cur = 0;
        if (isl28022_comp_stat () != tisl_busy) {
            switch (isl28022_targetreg) {
                case ISLREG_ADDR_SHUNTVOLTAGE:
//                    isl28022_read_data * 160.0 / 32878.0;
                    // Excite next power measurement
                    isl28022_targetreg = ISLREG_ADDR_BUSVOLTAGE;
                    break;
                case ISLREG_ADDR_BUSVOLTAGE:
                    ControlTable.CurrentBusVoltage = (uint16_t)((isl28022_read_data >> 2) * (0.004 * 1000.0));
                    // Excite next power measurement
                    isl28022_targetreg = ISLREG_ADDR_CURRENT;//ISLREG_ADDR_POWER;
                    break;
                case ISLREG_ADDR_POWER:
//                    (double)isl28022_data * 7.8125 * 5000.0 * 2.0 / 1000000.0;
                    // Excite next power measurement
                    isl28022_targetreg = ISLREG_ADDR_CURRENT;
                    break;
                case ISLREG_ADDR_CURRENT:
                    // Noise is excessive in the rotational direction when the PWM waveform appears large on the sense resistor side.
                    // A lax low-pass filter could not remove the noise, so it was dealt with software.
                    if (ControlTable.Excitation != sm_none) {
                        double n = (int16_t)isl28022_read_data;
                        if ((n > -20000) && (n < 20000)) stat_cur = -n;
                        current = (int32_t)lowpass (&current, stat_cur * ((0.160 / 0.030) / 32768.0) * 1000.0);
                        ControlTable.CurrentCurrent = (int32_t)current;
                    } else {
                        stat_cur = 0;
                        ControlTable.CurrentCurrent = (int32_t)(current = 0);
                    }
                    // Excite next power measurement
                    isl28022_targetreg = ISLREG_ADDR_BUSVOLTAGE;//ISLREG_ADDR_SHUNTVOLTAGE;
                    break;
                default:
                    isl28022_force_reset_stat ();
                    isl28022_targetreg = ISLREG_ADDR_BUSVOLTAGE;//ISLREG_ADDR_SHUNTVOLTAGE;
                    break;
            }
            isl28022_read_ex (isl28022_targetreg, &isl28022_read_data);
            ControlTable.SystemStatus.bit.powmonerror = 0;
        } else {
            isl28022_force_reset_stat ();
            ControlTable.SystemStatus.bit.powmonerror = 1;
            isl28022_targetreg = ISLREG_ADDR_SHUNTVOLTAGE;
//            isl28022_targetreg = ISLREG_ADDR_BUSVOLTAGE;
            isl28022_read_ex (isl28022_targetreg, &isl28022_read_data);
        }

        // Filtering ADC values of MCU's
        update_adcvalue ();

        // Control operation
        // Initialize gain by updating excitation
        if (prevmode != ControlTable.Excitation) {
            PIDReset_real (&pidparam_ang);
            PIDReset_real (&pidparam_velo);
            PIDReset_real (&pidparam_cur);
        }
        prevmode = ControlTable.Excitation;

        // Stop control if a serious condition of the magnetic sensor is detected.
        if (ControlTable.SystemStatus.bit.encodererror || (ControlTable.SystemStatus.bit.encoderstat & ~0x01)) {
            stall = true;
        }

        double tmp1, tmp2;
        // In any of the controls, the last stage is saturated with Target PWM.
        // In the case of cascade, the Target value of the minor controller is used as the absolute value.
        switch (ControlTable.Excitation) {
            case sm_none:
            case (sm_angle | sm_velocity  | sm_current | sm_pwm):
                UpdateTargetValue = false;
                TargetAngle = ControlTable.CurrentAngle;
                TargetVelocity = ControlTable.CurrentVelocity;
                TargetCurrent = ControlTable.CurrentCurrent;
                ControlTable.ControlDurationTime = 0;
                ControlTable.AngleViation = 0;
                ControlTable.VelocityViation = 0;
                ControlTable.CurrentViation = 0;
                break;
            case sm_angle:
                // interpolation control
                if (((ControlTable.TargetAngle != prev_targetangle) || (ControlTable.TransitionTime != prev_set_transition_time) || (ControlTable.TransitionMode != prev_set_transition_mode) || UpdateTargetValue) && (ControlTable.TransitionTime > 0)) {
                    TransitionTime = 0;
                    StartAngle = TargetAngle;
                } else if (ControlTable.TransitionTime > TransitionTime)
                    TargetAngle = CurveInterpolation (ControlTable.TransitionMode, StartAngle, ControlTable.TargetAngle, ControlTable.TransitionTime, ++TransitionTime);
                else
                    TargetAngle = ControlTable.TargetAngle;
                LimitedTargetAngle = saturate(TargetAngle + ControlTable.OffsetTargetAngle, ControlTable.AngleLimit.min, ControlTable.AngleLimit.max);
                // PID operation
                pidparam_ang.min = -abs (ControlTable.TargetPWM);
                pidparam_ang.max =  abs (ControlTable.TargetPWM);
                calc = PIDUpdate_real (&pidparam_ang, ControlTable.CurrentAngle, LimitedTargetAngle);
                // stiffness control
                lim = interp1dim ((int32_t)pidparam_ang.err[1], interp_angle_compliance_1d, interp_angle_compliance_ar_w);
                if (fabs(calc) > abs(lim)) {
                    calc = abs(lim) * ((calc >= 0) ? 1.0 : -1.0);
                    if (lim == 0) PIDReset_real (&pidparam_ang);
                }
                ControlTable.AngleViation = LimitedTargetAngle - ControlTable.CurrentAngle;
                ControlTable.VelocityViation = 0;
                ControlTable.CurrentViation = 0;
                if (UpdateTargetValue) UpdateTargetValue = false;
                ControlTable.ControlDurationTime++;
                break;
            case sm_velocity:
                // interpolation control
                if (((ControlTable.TargetVelocity != prev_targetvelocity) || (ControlTable.TransitionTime != prev_set_transition_time) || (ControlTable.TransitionMode != prev_set_transition_mode) || UpdateTargetValue) && (ControlTable.TransitionTime > 0)) {
                    TransitionTime = 0;
                    StartVelocity = TargetVelocity;
                } else if (ControlTable.TransitionTime > TransitionTime)
                    TargetVelocity = CurveInterpolation (ControlTable.TransitionMode, StartVelocity, ControlTable.TargetVelocity, ControlTable.TransitionTime, ++TransitionTime);
                else
                    TargetVelocity = ControlTable.TargetVelocity;
                TargetVelocity += ControlTable.OffsetTargetVelocity;
                // PID operation
                pidparam_velo.min = -abs (ControlTable.TargetPWM);
                pidparam_velo.max =  abs (ControlTable.TargetPWM);
                calc = PIDUpdate_real (&pidparam_velo, ControlTable.CurrentVelocity, TargetVelocity);
                ControlTable.AngleViation = 0;
                ControlTable.VelocityViation = TargetVelocity - ControlTable.CurrentVelocity;
                ControlTable.CurrentViation = 0;
                if (UpdateTargetValue) UpdateTargetValue = false;
                ControlTable.ControlDurationTime++;
                break;
            case (sm_angle | sm_velocity):
                // interpolation control
                if (((ControlTable.TargetAngle != prev_targetangle) || (ControlTable.TransitionTime != prev_set_transition_time) || (ControlTable.TransitionMode != prev_set_transition_mode) || UpdateTargetValue) && (ControlTable.TransitionTime > 0)) {
                    TransitionTime = 0;
                    StartAngle = TargetAngle;
                } else if (ControlTable.TransitionTime > TransitionTime)
                    TargetAngle = CurveInterpolation (ControlTable.TransitionMode, StartAngle, ControlTable.TargetAngle, ControlTable.TransitionTime, ++TransitionTime);
                else
                    TargetAngle = ControlTable.TargetAngle;
                LimitedTargetAngle = saturate(TargetAngle + ControlTable.OffsetTargetAngle, ControlTable.AngleLimit.min, ControlTable.AngleLimit.max);
                // PID operation
                pidparam_ang.min = -abs (ControlTable.TargetVelocity);
                pidparam_ang.max =  abs (ControlTable.TargetVelocity);
                calc = PIDUpdate_real (&pidparam_ang, ControlTable.CurrentAngle, LimitedTargetAngle);
                pidparam_velo.min = -abs (ControlTable.TargetPWM);
                pidparam_velo.max =  abs (ControlTable.TargetPWM);
                calc = PIDUpdate_real (&pidparam_velo, ControlTable.CurrentVelocity, (tmp1 = -calc));
                // stiffness control
                lim = interp1dim ((int32_t)pidparam_ang.err[1], interp_angle_compliance_1d, interp_angle_compliance_ar_w);
                if (fabs(calc) > abs(lim)) {
                    calc = abs(lim) * ((calc >= 0) ? 1.0 : -1.0);
                    if (lim == 0) {
                        PIDReset_real (&pidparam_ang);
                        PIDReset_real (&pidparam_velo);
                    }
                }
                ControlTable.AngleViation = LimitedTargetAngle - ControlTable.CurrentAngle;
                ControlTable.VelocityViation = tmp1 - ControlTable.CurrentVelocity;
                ControlTable.CurrentViation = 0;
                if (UpdateTargetValue) UpdateTargetValue = false;
                ControlTable.ControlDurationTime++;
                break;
            case sm_current:
                // interpolation control
                if (((ControlTable.TargetCurrent != prev_targetcurrent) || (ControlTable.TransitionTime != prev_set_transition_time) || (ControlTable.TransitionMode != prev_set_transition_mode) || UpdateTargetValue) && (ControlTable.TransitionTime > 0)) {
                    TransitionTime = 0;
                    StartCurrent = TargetCurrent;
                } else if (ControlTable.TransitionTime > TransitionTime)
                    TargetCurrent = CurveInterpolation (ControlTable.TransitionMode, StartCurrent, ControlTable.TargetCurrent, ControlTable.TransitionTime, ++TransitionTime);
                else
                    TargetCurrent = ControlTable.TargetCurrent;
                TargetCurrent += ControlTable.OffsetTargetCurrent;
                // PID operation
                UpdateTargetValue = false;
                pidparam_cur.min = -abs (ControlTable.TargetPWM);
                pidparam_cur.max =  abs (ControlTable.TargetPWM);
                calc = PIDUpdate_real (&pidparam_cur, ControlTable.CurrentCurrent, TargetCurrent);
                // stiffness control
                lim = interp1dim ((int32_t)pidparam_cur.err[1], interp_current_compliance_1d, interp_current_compliance_ar_w);
                if (fabs(calc) > abs(lim)) {
                    calc = abs(lim) * ((calc >= 0) ? 1.0 : -1.0);
                }
                ControlTable.AngleViation = 0;
                ControlTable.VelocityViation = 0;
                ControlTable.CurrentViation = TargetCurrent - ControlTable.CurrentCurrent;
                if (UpdateTargetValue) UpdateTargetValue = false;
                ControlTable.ControlDurationTime++;
                break;
            case (sm_angle | sm_current):
                // interpolation control
                if (((ControlTable.TargetAngle != prev_targetangle) || (ControlTable.TransitionTime != prev_set_transition_time) || (ControlTable.TransitionMode != prev_set_transition_mode) || UpdateTargetValue) && (ControlTable.TransitionTime > 0)) {
                    TransitionTime = 0;
                    StartAngle = TargetAngle;
                } else if (ControlTable.TransitionTime > TransitionTime)
                    TargetAngle = CurveInterpolation (ControlTable.TransitionMode, StartAngle, ControlTable.TargetAngle, ControlTable.TransitionTime, ++TransitionTime);
                else
                    TargetAngle = ControlTable.TargetAngle;
                LimitedTargetAngle = saturate(TargetAngle + ControlTable.OffsetTargetAngle, ControlTable.AngleLimit.min, ControlTable.AngleLimit.max);
                // PID operation
                pidparam_ang.min = -abs (ControlTable.TargetCurrent);
                pidparam_ang.max =  abs (ControlTable.TargetCurrent);
                calc = PIDUpdate_real (&pidparam_ang, ControlTable.CurrentAngle, LimitedTargetAngle);
                pidparam_cur.min = -abs (ControlTable.TargetPWM);
                pidparam_cur.max =  abs (ControlTable.TargetPWM);
                calc = PIDUpdate_real (&pidparam_cur, ControlTable.CurrentCurrent, (tmp1 = -calc));
                // stiffness control
                lim = interp1dim ((int32_t)pidparam_ang.err[1], interp_angle_compliance_1d, interp_angle_compliance_ar_w);
                if (fabs(calc) > abs(lim)) {
                    calc = abs(lim) * ((calc >= 0) ? 1.0 : -1.0);
                    if (lim == 0) {
                        PIDReset_real (&pidparam_ang);
                        PIDReset_real (&pidparam_cur);
                    }
                }
                ControlTable.AngleViation = LimitedTargetAngle - ControlTable.CurrentAngle;
                ControlTable.VelocityViation = 0;
                ControlTable.CurrentViation = tmp1 - ControlTable.CurrentCurrent;
                if (UpdateTargetValue) UpdateTargetValue = false;
                ControlTable.ControlDurationTime++;
                break;
            case (sm_velocity | sm_current):
                // interpolation control
                if (((ControlTable.TargetVelocity != prev_targetvelocity) || (ControlTable.TransitionTime != prev_set_transition_time) || (ControlTable.TransitionMode != prev_set_transition_mode) || UpdateTargetValue) && (ControlTable.TransitionTime > 0)) {
                    TransitionTime = 0;
                    StartVelocity = TargetVelocity;
                } else if (ControlTable.TransitionTime > TransitionTime)
                    TargetVelocity = CurveInterpolation (ControlTable.TransitionMode, StartVelocity, ControlTable.TargetVelocity, ControlTable.TransitionTime, ++TransitionTime);
                else
                    TargetVelocity = ControlTable.TargetVelocity;
                TargetVelocity += ControlTable.OffsetTargetVelocity;
                // PID operation
                pidparam_velo.min = -abs (ControlTable.TargetCurrent);
                pidparam_velo.max =  abs (ControlTable.TargetCurrent);
                calc = PIDUpdate_real (&pidparam_velo, ControlTable.CurrentVelocity, TargetVelocity);
                pidparam_cur.min = -abs (ControlTable.TargetPWM);
                pidparam_cur.max =  abs (ControlTable.TargetPWM);
                calc = PIDUpdate_real (&pidparam_cur, ControlTable.CurrentCurrent, (tmp1 = -calc));
                ControlTable.AngleViation = 0;
                ControlTable.VelocityViation = TargetVelocity - ControlTable.CurrentVelocity;
                ControlTable.CurrentViation = tmp1 - ControlTable.CurrentCurrent;
                if (UpdateTargetValue) UpdateTargetValue = false;
                ControlTable.ControlDurationTime++;
                break;
            case (sm_angle | sm_velocity  | sm_current):
                // interpolation control
                if (((ControlTable.TargetAngle != prev_targetangle) || (ControlTable.TransitionTime != prev_set_transition_time) || (ControlTable.TransitionMode != prev_set_transition_mode) || UpdateTargetValue) && (ControlTable.TransitionTime > 0)) {
                    TransitionTime = 0;
                    StartAngle = TargetAngle;
                } else if (ControlTable.TransitionTime > TransitionTime)
                    TargetAngle = CurveInterpolation (ControlTable.TransitionMode, StartAngle, ControlTable.TargetAngle, ControlTable.TransitionTime, ++TransitionTime);
                else
                    TargetAngle = ControlTable.TargetAngle;
                LimitedTargetAngle = saturate(TargetAngle + ControlTable.OffsetTargetAngle, ControlTable.AngleLimit.min, ControlTable.AngleLimit.max);
                // PID operation
                pidparam_ang.min = -abs (ControlTable.TargetVelocity);
                pidparam_ang.max =  abs (ControlTable.TargetVelocity);
                calc = PIDUpdate_real (&pidparam_ang, ControlTable.CurrentAngle, LimitedTargetAngle);
                pidparam_velo.min = -abs (ControlTable.TargetCurrent);
                pidparam_velo.max =  abs (ControlTable.TargetCurrent);
                calc = PIDUpdate_real (&pidparam_velo, ControlTable.CurrentVelocity, (tmp1 = -calc));
                pidparam_cur.min = -abs (ControlTable.TargetPWM);
                pidparam_cur.max =  abs (ControlTable.TargetPWM);
                calc = PIDUpdate_real (&pidparam_cur, ControlTable.CurrentCurrent, (tmp2 = -calc));
                // stiffness control
                lim = interp1dim ((int32_t)pidparam_ang.err[1], interp_angle_compliance_1d, interp_angle_compliance_ar_w);
                if (fabs(calc) > abs(lim)) {
                    calc = abs(lim) * ((calc >= 0) ? 1.0 : -1.0);
                    if (lim == 0) {
                        PIDReset_real (&pidparam_ang);
                        PIDReset_real (&pidparam_velo);
                        PIDReset_real (&pidparam_cur);
                    }
                }
                ControlTable.AngleViation = LimitedTargetAngle - ControlTable.CurrentAngle;
                ControlTable.VelocityViation = tmp1 - ControlTable.CurrentVelocity;
                ControlTable.CurrentViation = tmp2 - ControlTable.CurrentCurrent;
                if (UpdateTargetValue) UpdateTargetValue = false;
                ControlTable.ControlDurationTime++;
                break;
            case sm_pwm:
                calc = -ControlTable.TargetPWM;
                ControlTable.AngleViation = 0;
                ControlTable.VelocityViation = 0;
                ControlTable.CurrentViation = 0;
                if (UpdateTargetValue) UpdateTargetValue = false;
                ControlTable.ControlDurationTime++;
                break;
        }
        ControlTable.CurrentPWM = -(int32_t)calc;
        prev_targetangle = ControlTable.TargetAngle;
        prev_targetvelocity = ControlTable.TargetVelocity;
        prev_targetcurrent = ControlTable.TargetCurrent;
        prev_set_transition_mode = ControlTable.TransitionMode;
        prev_set_transition_time = ControlTable.TransitionTime;

        tmonproc moncon = MonConditions ();
        static tmonproc prev_moncon = mpc_idle;
        if (moncon == mpc_stall) stall = true;
        static const int pwm_gain[] = {75, 50, 25, 10, 0};
        if (stall) {
            ControlTable.Excitation = 0;
            set_motor_enable (false);
            R_GPT_DutyCycleSet (&g_timer5_ctrl, 0, GPT_IO_PIN_GTIOCB);
            ControlTable.SystemStatus.bit.other = 1;
            if (MonCount++ % _MON_BLINK_INTERVAL == 0) led_toggle();
        } else {
            switch (moncon) {
                case mpc_idle:
                case mpc_none:
                    // Update output to amplifier
                    set_motor_enable (ControlTable.Excitation != sm_none);
                    set_motor_dir (calc < 0);
                    R_GPT_DutyCycleSet (&g_timer5_ctrl, (uint32_t)fabs(calc), GPT_IO_PIN_GTIOCB);
                    ControlTable.SystemStatus.bit.other = 0;
                    break;
                case mpc_pwm75:
                case mpc_pwm50:
                case mpc_pwm25:
                case mpc_pwm10:
                case mpc_pwm0:
                    // Update output to amplifier
                    set_motor_enable (ControlTable.Excitation != sm_none);
                    set_motor_dir (calc < 0);
                    R_GPT_DutyCycleSet (&g_timer5_ctrl, (uint32_t)fabs((calc * pwm_gain[moncon - mpc_pwm75]) / 100), GPT_IO_PIN_GTIOCB);
                    ControlTable.SystemStatus.bit.other = 1;
                    break;
                case mpc_free:
                    ControlTable.Excitation = 0;
                    set_motor_enable (false);
                    ControlTable.SystemStatus.bit.other = 1;
                    break;
                case mpc_stall:
                    ControlTable.SystemStatus.bit.other = 1;
                    break;
            }
            if (moncon != mpc_idle) {
                if (MonCount++ % _MON_BLINK_INTERVAL == 0) led_toggle();
            }
            else if ((moncon == mpc_idle) && (moncon != prev_moncon)) led_off();
        }
        prev_moncon = moncon;

        ControlTable.NoCommTime++;
    }
}

static void ReqNumConversionItems (void) {
    for (uint16_t i = 0; i < 37; i++) write_item_callback (i + _ITEM_IDX_ANGLECORRECTION, ControlTable.AngleCorrection[i]);
    write_item_callback (_ITEM_IDX_ANGLECTRLGAIN    , ControlTable.AngleCtrlGain.Kp);
    write_item_callback (_ITEM_IDX_ANGLECTRLGAIN + 1, ControlTable.AngleCtrlGain.Ki);
    write_item_callback (_ITEM_IDX_ANGLECTRLGAIN + 2, ControlTable.AngleCtrlGain.Kd);
    write_item_callback (_ITEM_IDX_ANGLECTRLGAIN + 3, ControlTable.AngleCtrlGain.Kd);
    write_item_callback (_ITEM_IDX_ANGLECTRLGAIN + 4, ControlTable.AngleCtrlGain.delta_t);
    write_item_callback (_ITEM_IDX_VELOCITYCTRLGAIN    , ControlTable.VelocityCtrlGain.Kp);
    write_item_callback (_ITEM_IDX_VELOCITYCTRLGAIN + 1, ControlTable.VelocityCtrlGain.Ki);
    write_item_callback (_ITEM_IDX_VELOCITYCTRLGAIN + 2, ControlTable.VelocityCtrlGain.Kd);
    write_item_callback (_ITEM_IDX_VELOCITYCTRLGAIN + 3, ControlTable.VelocityCtrlGain.Kf);
    write_item_callback (_ITEM_IDX_VELOCITYCTRLGAIN + 4, ControlTable.VelocityCtrlGain.delta_t);
    write_item_callback (_ITEM_IDX_CURRENTCTRLGAIN    , ControlTable.CurrentCtrlGain.Kp);
    write_item_callback (_ITEM_IDX_CURRENTCTRLGAIN + 1, ControlTable.CurrentCtrlGain.Ki);
    write_item_callback (_ITEM_IDX_CURRENTCTRLGAIN + 2, ControlTable.CurrentCtrlGain.Kd);
    write_item_callback (_ITEM_IDX_CURRENTCTRLGAIN + 3, ControlTable.CurrentCtrlGain.Kf);
    write_item_callback (_ITEM_IDX_CURRENTCTRLGAIN + 4, ControlTable.CurrentCtrlGain.delta_t);
    PIDReset_real (&pidparam_ang);
    PIDReset_real (&pidparam_velo);
    PIDReset_real (&pidparam_cur);
    write_item_callback (_ITEM_IDX_ANGFLEX_NCOMP, ControlTable.AngleFlexibility.n_compliance);
    write_item_callback (_ITEM_IDX_ANGFLEX_NDEAD, ControlTable.AngleFlexibility.n_deadzone);
    write_item_callback (_ITEM_IDX_ANGFLEX_PDEAD, ControlTable.AngleFlexibility.p_deadzone);
    write_item_callback (_ITEM_IDX_ANGFLEX_PCOMP, ControlTable.AngleFlexibility.p_compliance);
    write_item_callback (_ITEM_IDX_CURFLEX_NCOMP, ControlTable.CurrentFlexibility.n_compliance);
    write_item_callback (_ITEM_IDX_CURFLEX_NDEAD, ControlTable.CurrentFlexibility.n_deadzone);
    write_item_callback (_ITEM_IDX_CURFLEX_PDEAD, ControlTable.CurrentFlexibility.p_deadzone);
    write_item_callback (_ITEM_IDX_CURFLEX_PCOMP, ControlTable.CurrentFlexibility.p_compliance);
}

//#define ISLTEST

/*******************************************************************************************************************//**
 * main() is generated by the RA Configuration editor and is used to generate threads if an RTOS is used.  This function
 * is called by main() when no RTOS is used.
 **********************************************************************************************************************/
void hal_entry(void) {
#if BSP_TZ_SECURE_BUILD
    /* Enter non-secure code */
    R_BSP_NonSecureEnter();
#endif

    // Init UART
    us_init (3000000, USIP_8N1, txb, sizeof (txb), rxb, sizeof (rxb));

    // Reads control table values from NVM.
    if (!LoadCcontroltableFromNVM ()) RestoreControlTable2FactoryDefaults ();

    // Initialize variables that do not directly use values from the control table.
    ReqNumConversionItems ();

    // Init GPIO
    if (R_IOPORT_Open (&g_ioport_ctrl, &g_bsp_pin_cfg) == FSP_SUCCESS) {
        set_motor_enable (false);
    } else {
        DBG ("ERR:R_IOPORT_Open\n");
        WAITFOR_TXCMP();
        NVIC_SystemReset ();
    }

    // Init ADC
    if (R_ADC_Open (&g_adc0_ctrl, &g_adc0_cfg) == FSP_SUCCESS) {
        R_ADC_ScanCfg (&g_adc0_ctrl, &g_adc0_channel_cfg);
        R_ADC_ScanStart (&g_adc0_ctrl);
    } else {
        DBG ("ERR:R_ADC_Open\n");
        WAITFOR_TXCMP();
        NVIC_SystemReset ();
    }

    // Init SPI
    uint16_t rawpos;
    if (tad2141_init (&rawpos, NULL)) {
        prev_ang = now_ang = interp1dim (rawpos, interp_tad2141_1d, interp_tad2141_ar_w) + ControlTable.OffsetPresentAngle;
    } else {
        DBG ("ERR:tad2141_init\n");
        WAITFOR_TXCMP();
        NVIC_SystemReset ();
    }

    // Init I2C
    //  CurrnetFS = 0.160[V] /0.030[ohm] = 5.333333333333333
    //  CurrentLSB = CurrnetFS / 32768 = 0.1627604166666667
    //  CalReg = 0.04096 / (CurrentLSB * 0.030[ohm]) = 8388.608
    if (!isl28022_init (ISLREG_BRNG_16V, ISLREG_PGA_160mV, ISLREG_ADC_15b_508us, ISLREG_ADC_128S_64ms, ISLREG_MODE_SHTBUSCONT, 8389)) {
        DBG ("ERR:isl28022_init\n");
        WAITFOR_TXCMP();
        NVIC_SystemReset ();
    }

    // Init PWM
    if (R_GPT_Open (&g_timer5_ctrl, &g_timer5_cfg) == FSP_SUCCESS) {
        R_GPT_Start (&g_timer5_ctrl);
        R_GPT_DutyCycleSet (&g_timer5_ctrl, 0, GPT_IO_PIN_GTIOCB);
    } else {
        DBG ("ERR:R_GPT_Open\n");
        WAITFOR_TXCMP();
        NVIC_SystemReset ();
    }

    // Update to the baud rate set in the control table
    us_setbaudrate (baudrate_list[ControlTable.Baudrate]);

#ifdef CALTEST
    DBG ("\nstart cal\n");
    tad2141_EOL_Programming ();
    DBG ("\nbye\n");

    R_BSP_SoftwareDelay (50, BSP_DELAY_UNITS_MILLISECONDS);
    NVIC_SystemReset ();
#else
    // Start Interval Timer
    if (R_AGT_Open (&g_timer0_ctrl, &g_timer0_cfg) == FSP_SUCCESS) {
        R_AGT_Enable (&g_timer0_ctrl);
        static char packet[2000];
        for (;;) {
            if (packet_receive (packet) > 0) {
                packet_handling (packet);
                if (packet_req_init_id_baud) {
                    while (us_tx_buff ()) __WFI();
                    us_setbaudrate (baudrate_list[ControlTable.Baudrate]);
                    packet_req_init_id_baud = false;
                } else if (packet_req_initalize) {
                    ControlTable.Excitation = sm_none;
                    while (us_tx_buff ()) __WFI();
                    RestoreControlTable2FactoryDefaults ();
                    // Initialize variables that do not directly use values from the control table
                    ReqNumConversionItems ();

                    us_setbaudrate (baudrate_list[ControlTable.Baudrate]);
                    packet_req_initalize = false;
                }
            }
            __WFI();
        }
    } else NVIC_SystemReset ();
#endif
}


/*******************************************************************************************************************//**
 * This function is called at various points during the startup process.  This implementation uses the event that is
 * called right before main() to set up the pins.
 *
 * @param[in]  event    Where at in the start up process the code is currently at
 **********************************************************************************************************************/
void R_BSP_WarmStart(bsp_warm_start_event_t event)
{
    if (BSP_WARM_START_RESET == event)
    {
#if BSP_FEATURE_FLASH_LP_VERSION != 0

        /* Enable reading from data flash. */
        R_FACI_LP->DFLCTL = 1U;

        /* Would normally have to wait tDSTOP(6us) for data flash recovery. Placing the enable here, before clock and
         * C runtime initialization, should negate the need for a delay since the initialization will typically take more than 6us. */
#endif
    }

    if (BSP_WARM_START_POST_C == event)
    {
        /* C runtime environment and system clocks are setup. */

        /* Configure pins. */
        R_IOPORT_Open (&g_ioport_ctrl, g_ioport.p_cfg);
    }
}

#if BSP_TZ_SECURE_BUILD

BSP_CMSE_NONSECURE_ENTRY void template_nonsecure_callable ();

/* Trustzone Secure Projects require at least one nonsecure callable function in order to build (Remove this if it is not required to build). */
BSP_CMSE_NONSECURE_ENTRY void template_nonsecure_callable ()
{

}
#endif
