/*
 * =======================================================================================================
 * -------------------------------------------------------------------------------------------------------
 * ---####################-----###########-------###########-----############--############-############--
 * --######################---#############-----#############----            --            -           ---
 * --######              ##---#####     ###-----###     #####---------##-------#######------#-------------
 * --      --------------  ---     -----    ---    -----     ---------##-------#------------#-------------
 * --#####--------------------#####------####-####------#####---------##-------###########--############--
 * --     --------------------     ------         ------     ---------  -------           --            --
 * --#####--------------------#####--------#####--------#####---------------------------------------------
 * --     --------------------     --------     --------     ---------------------------------------------
 * --######--------------##---#####---------------------#####---------- CMtec CMDR Joystick RC -----------
 * --##################### ---#####---------------------#####---------------------------------------------
 * ---################### ----#####---------------------#####---------------------------------------------
 * ---                   -----     ---------------------     ---------------------------------------------
 * -------------------------------------------------------------------------------------------------------
 * =======================================================================================================
 *
 *  Copyright 2022 Christoffer Martinsson <cm@cmtec.se>
 *
 *  CMtec CMDR Joystick RC can be redistributed and/or modified under the terms of the GNU General
 *  Public License (Version 2), as published by the Free Software Foundation.
 *  A copy of the license can be found online at www.gnu.o urg/licenses.
 *
 *  CMtec CMDR Joystick RC is distributed in the hope that it will be useful, but WITHOUT ANY
 *  WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
 *  A PARTICULAR PURPOSE. See the GNU General Public License for more details.
 *
 *  Joystick based on standard teensy "usb_joystick" library for HID joystick usb data communication.
 */

#include <Arduino.h>
#include <EEPROM.h>
#include "erls.h"
#include "ResponsiveAnalogRead.h"

#define LED_MODE_OFF 0
#define LED_MODE_ON 1
#define LED_MODE_BLINK 2
#define LED_ON HIGH
#define LED_OFF LOW

const int STATUS_LED_PIN = 13;
bool status_led_on = false;
int status_led_mode = LED_MODE_BLINK;

const int BUTTON_LED_1_PIN = 22;
bool button_led_1_on = false;
int button_led_1_mode = LED_MODE_OFF;

const int BUTTON_LED_2_PIN = 23;
bool button_led_2_on = false;
int button_led_2_mode = LED_MODE_OFF;

#define TIME_US_1ms 1000
#define TIME_US_5ms 5000
#define TIME_US_200ms 200000
unsigned long current_timestamp_micros = 0;
unsigned long process_data_timestamp_micros = 0;
unsigned long indicator_timestamp_micros = 0;
unsigned long send_usb_timestamp_micros = 0;
unsigned long send_elrs_timestamp_micros = 0;

#define BUTTON_PRESSED LOW
#define BUTTON_NOT_PRESSED HIGH
const int BUTTON_FRONT_LEFT_UPPER_PIN = 4;
const int BUTTON_FRONT_LEFT_LOWER_PIN = 5;
const int BUTTON_FRONT_RIGHT_UPPER_PIN = 6;
const int BUTTON_FRONT_RIGHT_LOWER_PIN = 7;
const int BUTTON_TOP_LEFT_UPPER_PIN = 9;
const int BUTTON_TOP_RIGHT_UPPER_PIN = 10;
const int BUTTON_TOP_LEFT_LOWER_PIN = 8;
const int BUTTON_TOP_RIGHT_LOWER_PIN = 11;

#define GIMBAL_MODE_FRSKY_M7 0
#define GIMBAL_MODE_FRSKY_M10 1
const int GIMBAL_MODE_PIN = 21;
int gimbal_mode = GIMBAL_MODE_FRSKY_M7;

bool toggle_button_arm = false;
bool toggle_button_mode = false;
int toggle_button_arm_previous_value = HIGH;

#define AXIS_10BIT_MAX 1023
#define AXIS_10BIT_MIN 0
#define AXIS_10BIT_CENTER 512
#define AXIS_12BIT_MAX 4096
#define AXIS_12BIT_MIN 0
#define AXIS_12BIT_CENTER 2048
#define DEADZONE_X 50
#define DEADZONE_Y 50
#define JOYSTICK_HAT_CENTER -1

enum EEPROM_ADR
{
    MAX_X1_ADR_HIGH,
    MAX_X1_ADR_LOW,
    MIN_X1_ADR_HIGH,
    MIN_X1_ADR_LOW,
    CNT_X1_ADR_HIGH,
    CNT_X1_ADR_LOW,
    MAX_Y1_ADR_HIGH,
    MAX_Y1_ADR_LOW,
    MIN_Y1_ADR_HIGH,
    MIN_Y1_ADR_LOW,
    CNT_Y1_ADR_HIGH,
    CNT_Y1_ADR_LOW,
    MAX_X2_ADR_HIGH,
    MAX_X2_ADR_LOW,
    MIN_X2_ADR_HIGH,
    MIN_X2_ADR_LOW,
    CNT_X2_ADR_HIGH,
    CNT_X2_ADR_LOW,
    MAX_Y2_ADR_HIGH,
    MAX_Y2_ADR_LOW,
    MIN_Y2_ADR_HIGH,
    MIN_Y2_ADR_LOW,
    CNT_Y2_ADR_HIGH,
    CNT_Y2_ADR_LOW,
    EEPROM_ADR_NBR_OF_BYTES
};

int joystick_counter = 0;
int joystick_x1_12bit = 0;
int joystick_x1_12bit_max = 4096;
int joystick_x1_12bit_min = 0;
int joystick_x1_12bit_center = joystick_x1_12bit_max / 2;
int joystick_y1_12bit = 0;
int joystick_y1_12bit_max = 4096;
int joystick_y1_12bit_min = 0;
int joystick_y1_12bit_center = joystick_y1_12bit_max / 2;
int joystick_x2_12bit = 0;
int joystick_x2_12bit_max = 4096;
int joystick_x2_12bit_min = 0;
int joystick_x2_12bit_center = joystick_x2_12bit_max / 2;
int joystick_y2_12bit = 0;
int joystick_y2_12bit_max = 4096;
int joystick_y2_12bit_min = 0;
int joystick_y2_12bit_center = joystick_y2_12bit_max / 2;

float exp_constant = 0.2;

int fn_mode = 0;

ResponsiveAnalogRead analog_x1(A0, true);
ResponsiveAnalogRead analog_y1(A1, true);
ResponsiveAnalogRead analog_x2(A3, true);
ResponsiveAnalogRead analog_y2(A2, true);

#define CALIBRATION_OFF 0
#define CALIBRATION_INIT 1
#define CALIBRATION_CENTER 2
#define CALIBRATION_MINMAX 3
int joystick_calibration_mode = CALIBRATION_OFF;

/**
 * @brief Save calibration data to EEPROM
 */
void save_to_eeprom()
{
    EEPROM.write(MAX_X1_ADR_LOW, joystick_x1_12bit_max);
    EEPROM.write(MAX_X1_ADR_HIGH, joystick_x1_12bit_max >> 8);
    EEPROM.write(MIN_X1_ADR_LOW, joystick_x1_12bit_min);
    EEPROM.write(MIN_X1_ADR_HIGH, joystick_x1_12bit_min >> 8);
    EEPROM.write(CNT_X1_ADR_LOW, joystick_x1_12bit_center);
    EEPROM.write(CNT_X1_ADR_HIGH, joystick_x1_12bit_center >> 8);

    EEPROM.write(MAX_Y1_ADR_LOW, joystick_y1_12bit_max);
    EEPROM.write(MAX_Y1_ADR_HIGH, joystick_y1_12bit_max >> 8);
    EEPROM.write(MIN_Y1_ADR_LOW, joystick_y1_12bit_min);
    EEPROM.write(MIN_Y1_ADR_HIGH, joystick_y1_12bit_min >> 8);
    EEPROM.write(CNT_Y1_ADR_LOW, joystick_y1_12bit_center);
    EEPROM.write(CNT_Y1_ADR_HIGH, joystick_y1_12bit_center >> 8);

    EEPROM.write(MAX_X2_ADR_LOW, joystick_x2_12bit_max);
    EEPROM.write(MAX_X2_ADR_HIGH, joystick_x2_12bit_max >> 8);
    EEPROM.write(MIN_X2_ADR_LOW, joystick_x2_12bit_min);
    EEPROM.write(MIN_X2_ADR_HIGH, joystick_x2_12bit_min >> 8);
    EEPROM.write(CNT_X2_ADR_LOW, joystick_x2_12bit_center);
    EEPROM.write(CNT_X2_ADR_HIGH, joystick_x2_12bit_center >> 8);

    EEPROM.write(MAX_Y2_ADR_LOW, joystick_y2_12bit_max);
    EEPROM.write(MAX_Y2_ADR_HIGH, joystick_y2_12bit_max >> 8);
    EEPROM.write(MIN_Y2_ADR_LOW, joystick_y2_12bit_min);
    EEPROM.write(MIN_Y2_ADR_HIGH, joystick_y2_12bit_min >> 8);
    EEPROM.write(CNT_Y2_ADR_LOW, joystick_y2_12bit_center);
    EEPROM.write(CNT_Y2_ADR_HIGH, joystick_y2_12bit_center >> 8);
}

/**
 * @brief Load calibration data from EEPROM
 */
void load_from_eeprom()
{
    joystick_x1_12bit_max = (EEPROM.read(MAX_X1_ADR_HIGH) << 8);
    joystick_x1_12bit_max |= EEPROM.read(MAX_X1_ADR_LOW);
    joystick_x1_12bit_min = (EEPROM.read(MIN_X1_ADR_HIGH) << 8);
    joystick_x1_12bit_min |= EEPROM.read(MIN_X1_ADR_LOW);
    joystick_x1_12bit_center = (EEPROM.read(CNT_X1_ADR_HIGH) << 8);
    joystick_x1_12bit_center |= EEPROM.read(CNT_X1_ADR_LOW);

    joystick_y1_12bit_max = (EEPROM.read(MAX_Y1_ADR_HIGH) << 8);
    joystick_y1_12bit_max |= EEPROM.read(MAX_Y1_ADR_LOW);
    joystick_y1_12bit_min = (EEPROM.read(MIN_Y1_ADR_HIGH) << 8);
    joystick_y1_12bit_min |= EEPROM.read(MIN_Y1_ADR_LOW);
    joystick_y1_12bit_center = (EEPROM.read(CNT_Y1_ADR_HIGH) << 8);
    joystick_y1_12bit_center |= EEPROM.read(CNT_Y1_ADR_LOW);

    joystick_x2_12bit_max = (EEPROM.read(MAX_X2_ADR_HIGH) << 8);
    joystick_x2_12bit_max |= EEPROM.read(MAX_X2_ADR_LOW);
    joystick_x2_12bit_min = (EEPROM.read(MIN_X2_ADR_HIGH) << 8);
    joystick_x2_12bit_min |= EEPROM.read(MIN_X2_ADR_LOW);
    joystick_x2_12bit_center = (EEPROM.read(CNT_X2_ADR_HIGH) << 8);
    joystick_x2_12bit_center |= EEPROM.read(CNT_X2_ADR_LOW);

    joystick_y2_12bit_max = (EEPROM.read(MAX_Y2_ADR_HIGH) << 8);
    joystick_y2_12bit_max |= EEPROM.read(MAX_Y2_ADR_LOW);
    joystick_y2_12bit_min = (EEPROM.read(MIN_Y2_ADR_HIGH) << 8);
    joystick_y2_12bit_min |= EEPROM.read(MIN_Y2_ADR_LOW);
    joystick_y2_12bit_center = (EEPROM.read(CNT_Y2_ADR_HIGH) << 8);
    joystick_y2_12bit_center |= EEPROM.read(CNT_Y2_ADR_LOW);
}

/**
 * @brief Calibrate all joystick axis
 *
 * @param analog_x1
 * @param analog_y1
 * @param analog_x2
 * @param analog_y2
 */
void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2)
{

    joystick_x1_12bit = AXIS_12BIT_CENTER;
    joystick_y1_12bit = AXIS_12BIT_CENTER;
    joystick_x2_12bit = AXIS_12BIT_CENTER;
    joystick_y2_12bit = AXIS_12BIT_CENTER;

    // Check for calibration mode
    if (joystick_calibration_mode == CALIBRATION_INIT)
    {
        if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_NOT_PRESSED)
        {
            joystick_calibration_mode = CALIBRATION_CENTER;
        }
    }

    // Calibrate joystick center values
    else if (joystick_calibration_mode == CALIBRATION_CENTER)
    {
        joystick_x1_12bit_center = analog_x1;
        joystick_y1_12bit_center = analog_y1;
        joystick_x1_12bit_max = joystick_x1_12bit_center;
        joystick_x1_12bit_min = joystick_x1_12bit_center;
        joystick_y1_12bit_max = joystick_y1_12bit_center;
        joystick_y1_12bit_min = joystick_y1_12bit_center;

        joystick_x2_12bit_center = analog_x2;
        joystick_y2_12bit_center = analog_y2;
        joystick_x2_12bit_max = joystick_x2_12bit_center;
        joystick_x2_12bit_min = joystick_x2_12bit_center;
        joystick_y2_12bit_max = joystick_y2_12bit_center;
        joystick_y2_12bit_min = joystick_y2_12bit_center;

        if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED)
        {
            joystick_calibration_mode = CALIBRATION_MINMAX;
        }
    }

    // Calibrate joystick min/max values
    else if (joystick_calibration_mode == CALIBRATION_MINMAX)
    {
        if (analog_x1 > joystick_x1_12bit_max) joystick_x1_12bit_max = analog_x1;
        if (analog_x1 < joystick_x1_12bit_min) joystick_x1_12bit_min = analog_x1;
        if (analog_y1 > joystick_y1_12bit_max) joystick_y1_12bit_max = analog_y1;
        if (analog_y1 < joystick_y1_12bit_min) joystick_y1_12bit_min = analog_y1;
        if (analog_x2 > joystick_x2_12bit_max) joystick_x2_12bit_max = analog_x2;
        if (analog_x2 < joystick_x2_12bit_min) joystick_x2_12bit_min = analog_x2;
        if (analog_y2 > joystick_y2_12bit_max) joystick_y2_12bit_max = analog_y2;
        if (analog_y2 < joystick_y2_12bit_min) joystick_y2_12bit_min = analog_y2;

        if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED)
        {
            joystick_calibration_mode = CALIBRATION_OFF;
            save_to_eeprom();
        }
    }
}

/**
 * @brief Save joystick calibration values to EEPROM
 */
void send_erls_data()
{

    // Set ELRS analog channels
    erls_set_data(map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 0);
    erls_set_data(map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 1);
    erls_set_data(map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 2);
    erls_set_data(map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 3);

    // Set ELRS digital channels
    for (int i = 4; i < CRSF_MAX_CHANNEL; i++)
    {
        erls_set_data(CRSF_DIGITAL_CHANNEL_MIN, i);
    }

    if (toggle_button_arm) erls_set_data(CRSF_DIGITAL_CHANNEL_MAX, 4);
    if (toggle_button_mode) erls_set_data(CRSF_DIGITAL_CHANNEL_MAX, 5);
    if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) erls_set_data(CRSF_DIGITAL_CHANNEL_MAX, 6);
    if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) erls_set_data(CRSF_DIGITAL_CHANNEL_MAX, 7);
    if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) erls_set_data(CRSF_DIGITAL_CHANNEL_MAX, 8);

    // Send ELRS data
    erls_send_data();
}

/**
 * Send USB data to PC
 */
void send_usb_data()
{
    // Set USB analog channels
    int joystick_x1_10bit = map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
    int joystick_y1_10bit = map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
    int joystick_x2_10bit = map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
    int joystick_y2_10bit = map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);

    Joystick.Zrotate(joystick_x1_10bit);
    Joystick.Z(joystick_y1_10bit);

    if (fn_mode == 2)
    {
        Joystick.X(AXIS_10BIT_CENTER);
        Joystick.Y(AXIS_10BIT_CENTER);
        Joystick.sliderRight(joystick_x2_10bit);
        Joystick.sliderLeft(joystick_y2_10bit);
    }
    else if (fn_mode == 1)
    {
        Joystick.X(AXIS_10BIT_CENTER);
        Joystick.Y(joystick_y2_10bit);
        Joystick.sliderRight(joystick_x2_10bit);
        Joystick.sliderLeft(AXIS_10BIT_CENTER);
    }
    else
    {
        Joystick.X(joystick_x2_10bit);
        Joystick.Y(joystick_y2_10bit);
        Joystick.sliderRight(AXIS_10BIT_CENTER);
        Joystick.sliderLeft(AXIS_10BIT_CENTER);
    }

    // Set USB digital channels
    for (int i = 1; i < 32; i++)
    {
        Joystick.button(i, 0);
    }

    Joystick.hat(JOYSTICK_HAT_CENTER);

    if (fn_mode == 2)
    {
        if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(12, 1);
        if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(13, 1);
        if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(14, 1);
        if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(15, 1);
        if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(16, 1);
        if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(17, 1);
    }
    else if (fn_mode == 1)
    {
        if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(8, 1);
        if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(9, 1);
        if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(10, 1);
        if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(11, 1);
        if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1);
        if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1);
    }
    else
    {
        if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(3, 1);
        if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(4, 1);
        if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(5, 1);
        if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(6, 1);
        if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(7, 1);
        if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1);
        if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1);
    }

    Joystick.send_now();
}

/**
 * @brief Apply calibration to 12-bit gimbal value
 *
 * @param gimbal_value
 * @param min_value
 * @param max_value
 * @param center_value
 * @param deadband_value
 * @param expo_value
 */
int apply_calibration_12bit(int gimbal_value, int min_value, int max_value, int center_value, int deadband_value, int expo_value)
{

    int calibrated_value = AXIS_12BIT_CENTER;

    if (gimbal_value > (center_value + deadband_value))
    {
        calibrated_value = constrain(map(gimbal_value, (center_value + deadband_value), max_value, AXIS_12BIT_CENTER, AXIS_12BIT_MAX), AXIS_12BIT_CENTER, AXIS_12BIT_MAX);
    }
    else if (gimbal_value < (center_value - deadband_value))
    {
        calibrated_value = constrain(map(gimbal_value, min_value, (center_value - deadband_value), AXIS_12BIT_MIN, AXIS_12BIT_CENTER), AXIS_12BIT_MIN, AXIS_12BIT_CENTER);
    }

    if (expo_value != 0)
    {
        float joystick_x_float = calibrated_value / float(AXIS_12BIT_MAX);
        /* Calculate expo using 9th order polynomial function with 0.5 as center point */
        float joystick_x_exp = expo_value * (0.5 + 256 * pow((joystick_x_float - 0.5), 9)) + (1 - expo_value) * joystick_x_float;
        calibrated_value = constrain(int(joystick_x_exp * float(AXIS_12BIT_MAX)), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
    }

    return calibrated_value;
}

/**
 * @brief Process input data from gimbal and buttons
 */
void process_input_data()
{

    int analog_x1_gimbal_value = 0;
    int analog_y1_gimbal_value = 0;
    int analog_x2_gimbal_value = 0;
    int analog_y2_gimbal_value = 0;

    if (gimbal_mode == GIMBAL_MODE_FRSKY_M10)
    {
        analog_x1_gimbal_value = constrain(AXIS_12BIT_MAX - analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
        analog_y1_gimbal_value = constrain(analog_y1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
        analog_x2_gimbal_value = constrain(analog_x2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
        analog_y2_gimbal_value = constrain(AXIS_12BIT_MAX - analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
    }
    else if (gimbal_mode == GIMBAL_MODE_FRSKY_M7)
    {
        analog_x1_gimbal_value = constrain(analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
        analog_y1_gimbal_value = constrain(AXIS_12BIT_MAX - analog_y1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
        analog_x2_gimbal_value = constrain(AXIS_12BIT_MAX - analog_x2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
        analog_y2_gimbal_value = constrain(analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
    }

    if (joystick_calibration_mode != CALIBRATION_OFF)
    {
        calibrate_axis(analog_x1_gimbal_value, analog_y1_gimbal_value, analog_x2_gimbal_value, analog_y2_gimbal_value);
        return;
    }

    joystick_x1_12bit = apply_calibration_12bit(analog_x1_gimbal_value, joystick_x1_12bit_min, joystick_x1_12bit_max, joystick_x1_12bit_center, DEADZONE_X, exp_constant);
    joystick_y1_12bit = apply_calibration_12bit(analog_y1_gimbal_value, joystick_y1_12bit_min, joystick_y1_12bit_max, joystick_y1_12bit_center, DEADZONE_Y, 0);
    joystick_x2_12bit = apply_calibration_12bit(analog_x2_gimbal_value, joystick_x2_12bit_min, joystick_x2_12bit_max, joystick_x2_12bit_center, DEADZONE_X, exp_constant);
    joystick_y2_12bit = apply_calibration_12bit(analog_y2_gimbal_value, joystick_y2_12bit_min, joystick_y2_12bit_max, joystick_y2_12bit_center, DEADZONE_Y, exp_constant);

    // Check fn mode
    fn_mode = 0;
    if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED)
    {
        fn_mode = 1;
        if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED)
        {
            fn_mode = 2;
        }
    }

    // Check toggle mode buttons
    if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED)
    {
        toggle_button_mode = true;
    }
    else if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED)
    {
        toggle_button_mode = false;
    }

    // Check toggle arm button
    if ((fn_mode == 1) && (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) != toggle_button_arm_previous_value))
    {
        if ((digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) && (toggle_button_arm == false))
        {
            toggle_button_arm = true;
        }
        else if ((digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) && (toggle_button_arm == true))
        {
            toggle_button_arm = false;
        }
        toggle_button_arm_previous_value = digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN);
    }
}

/**
 * @brief Update indicator LED
 */
void update_indicator(int led_mode, bool &current_led_state, int led_pin)
{

    if (led_mode == LED_MODE_BLINK && current_led_state == false)
    {
        digitalWrite(led_pin, LED_ON);
        current_led_state = true;
    }
    else if (led_mode == LED_MODE_BLINK && current_led_state == true)
    {
        digitalWrite(led_pin, LED_OFF);
        current_led_state = false;
    }
    else if (led_mode == LED_MODE_ON)
    {
        digitalWrite(led_pin, LED_ON);
        current_led_state = true;
    }
    else
    {
        digitalWrite(led_pin, LED_OFF);
        current_led_state = false;
    }
}

void setup()
{
    /* Init HW */
    pinMode(STATUS_LED_PIN, OUTPUT);
    digitalWrite(STATUS_LED_PIN, LED_OFF);

    pinMode(BUTTON_LED_1_PIN, OUTPUT);
    digitalWrite(BUTTON_LED_1_PIN, LED_OFF);

    pinMode(BUTTON_LED_2_PIN, OUTPUT);
    digitalWrite(BUTTON_LED_2_PIN, LED_OFF);

    pinMode(BUTTON_FRONT_LEFT_LOWER_PIN, INPUT_PULLUP);
    pinMode(BUTTON_FRONT_LEFT_UPPER_PIN, INPUT_PULLUP);
    pinMode(BUTTON_FRONT_RIGHT_LOWER_PIN, INPUT_PULLUP);
    pinMode(BUTTON_FRONT_RIGHT_UPPER_PIN, INPUT_PULLUP);
    pinMode(BUTTON_TOP_LEFT_LOWER_PIN, INPUT_PULLUP);
    pinMode(BUTTON_TOP_RIGHT_LOWER_PIN, INPUT_PULLUP);
    pinMode(BUTTON_TOP_LEFT_UPPER_PIN, INPUT_PULLUP);
    pinMode(BUTTON_TOP_RIGHT_UPPER_PIN, INPUT_PULLUP);

    pinMode(GIMBAL_MODE_PIN, INPUT_PULLUP);

    // Set ADC resolution to 12bit
    analogReadResolution(12);
    analogReadAveraging(32);
    delay(500);

    // Set analog (lib) resolution to 12bit
    analog_x1.setAnalogResolution(4096);
    analog_y1.setAnalogResolution(4096);
    analog_x2.setAnalogResolution(4096);
    analog_y2.setAnalogResolution(4096);

    // Init EEPROM
    load_from_eeprom();

    // Init Joystick
    Joystick.useManualSend(true);

    // Check if calibration mode is enabled
    if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED)
    {
        joystick_calibration_mode = CALIBRATION_INIT;
    }

    // Check if bootloader mode is enabled
    if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED)
    {
        digitalWrite(BUTTON_LED_2_PIN, HIGH);
        delay(200);
        _reboot_Teensyduino_();
    }

    // Check what gimbal mode is selected
    if (digitalRead(GIMBAL_MODE_PIN) == LOW)
    {
        gimbal_mode = GIMBAL_MODE_FRSKY_M10;
    }

    // Init ELRS
    ELRS_SERIAL_PORT.begin(ELRS_SERIAL_BAUDRATE);
}

void loop()
{
    current_timestamp_micros = micros();

    analog_x1.update();
    analog_y1.update();
    analog_x2.update();
    analog_y2.update();

    /* Process data with 1ms interval*/
    if (current_timestamp_micros >= process_data_timestamp_micros)
    {
        process_input_data();
        process_data_timestamp_micros = current_timestamp_micros + TIME_US_1ms;
    }

    /* Update/Send USB data with 5ms interval*/
    if (current_timestamp_micros >= send_usb_timestamp_micros)
    {
        send_usb_data();
        send_usb_timestamp_micros = current_timestamp_micros + TIME_US_5ms;
    }

    /* Update/Send ERLS data with about 1,6ms interval */
    if (current_timestamp_micros >= send_elrs_timestamp_micros)
    {
        send_erls_data();
        send_elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US;
    }

    /* Update indicator with 200ms interval */
    if (current_timestamp_micros >= indicator_timestamp_micros)
    {
        button_led_1_mode = LED_MODE_OFF;
        button_led_2_mode = LED_MODE_OFF;

        if (joystick_calibration_mode == CALIBRATION_INIT)
        {
            button_led_1_mode = LED_MODE_BLINK;
            button_led_2_mode = LED_MODE_BLINK;
        }
        else if (joystick_calibration_mode == CALIBRATION_CENTER)
        {
            button_led_1_mode = LED_MODE_BLINK;
            button_led_2_mode = LED_MODE_OFF;
        }
        else if (joystick_calibration_mode == CALIBRATION_MINMAX)
        {
            button_led_1_mode = LED_MODE_OFF;
            button_led_2_mode = LED_MODE_BLINK;
        }
        else if ((joystick_x1_12bit != AXIS_12BIT_CENTER) || (joystick_y1_12bit != AXIS_12BIT_MIN) ||
                 (joystick_x2_12bit != AXIS_12BIT_CENTER) || (joystick_y2_12bit != AXIS_12BIT_CENTER))
        {
            button_led_1_mode = LED_MODE_ON;
            button_led_2_mode = LED_MODE_ON;
        }

        update_indicator(button_led_1_mode, button_led_1_on, BUTTON_LED_1_PIN);
        update_indicator(button_led_2_mode, button_led_2_on, BUTTON_LED_2_PIN);
        update_indicator(status_led_mode, status_led_on, STATUS_LED_PIN);

        indicator_timestamp_micros = current_timestamp_micros + TIME_US_200ms;
    }
}