#include <arm.h>
#include <eeprom.h>
#include <spool.h>

// ─────────────────────────────────────────────
//  Machine d'état traverse
// ─────────────────────────────────────────────

typedef enum {
    ARM_IDLE,
    ARM_GO_TO_ZERO,     // retour vers LEFT
    ARM_WAIT_LEFT,      // attente 3/4 tour à LEFT
    ARM_TRAVERSE,       // déplacement vers RIGHT
    ARM_WAIT_RIGHT,     // attente 3/4 tour à RIGHT
    ARM_RETURN          // déplacement vers LEFT
} arm_state_t;

// ─────────────────────────────────────────────
//  Constantes
// ─────────────────────────────────────────────

#define ARM_WAIT_STEPS          600U        // 3/4 tour SPOOL (200pas × 4µstep × 3/4)
#define ARM_DIR_RIGHT           1
#define ARM_DIR_LEFT            0

// ─────────────────────────────────────────────
//  État interne
// ─────────────────────────────────────────────

static volatile uint16_t  gear_um;         // déplacement µm par tour SPOOL
static volatile uint32_t  accumulator;     // accumulateur µm
static          uint16_t  arm_offset;      // distance capteur home → origine (µm)
static volatile uint8_t   free_arm;        // 1 = vgear désactivé, moteur libre
static          uint16_t  pos_left;        // limite gauche (1/10 mm)
static          uint16_t  pos_right;       // limite droite (1/10 mm)
static volatile arm_state_t state;         // état courant
static volatile uint16_t  wait_ctr;        // compteur steps SPOOL pour les pauses
static          uint16_t  travel_steps;    // micropas pour aller de LEFT à RIGHT

// ─────────────────────────────────────────────
//  Calcul du trajet LEFT→RIGHT en micropas
// ─────────────────────────────────────────────

static uint16_t ARM_CalcTravelSteps(void)
{
    uint32_t um = (uint32_t)(pos_right - pos_left) * 100UL;  // 1/10mm → µm
    return (uint16_t)(um / ARM_UM_PER_USTEP);
}

// ─────────────────────────────────────────────
//  Init — lecture offset EEPROM au démarrage
// ─────────────────────────────────────────────

void ARM_Init(void)
{
    gear_um      = 0;
    accumulator  = 0;
    free_arm     = 0;
    pos_left     = ARM_LEFT_DEFAULT;
    pos_right    = ARM_RIGHT_DEFAULT;
    state        = ARM_IDLE;
    wait_ctr     = 0;
    travel_steps = ARM_CalcTravelSteps();

    ARM_EN(0);
    ARM_REV(0);

    arm_offset = EE_ReadWord(EE_ARM_OFFSET_H, EE_ARM_OFFSET_L);
}

// ─────────────────────────────────────────────
//  Enable / Dir
// ─────────────────────────────────────────────

void ARM_Enable(uint8_t state_en)
{
    ARM_EN(state_en);
}

void ARM_SetDir(uint8_t dir)
{
    ARM_REV(dir);
}

// ─────────────────────────────────────────────
//  UpdateTarget — appelée depuis l'ISR SPOOL
//  Machine d'état légère : accumule µm et met
//  à jour arm_target_position (pas de steps ici)
// ─────────────────────────────────────────────

void ARM_UpdateTarget(void)
{
    if (free_arm || gear_um == 0)
        return;

    switch (state) {

        case ARM_IDLE:
            break;

        case ARM_GO_TO_ZERO:
            accumulator += gear_um;
            while (accumulator >= ARM_UM_PER_REV) {
                accumulator -= ARM_UM_PER_REV;
                if (arm_target_position > 0)
                    arm_target_position--;
                else {
                    accumulator = 0;
                    wait_ctr    = 0;
                    state       = ARM_WAIT_LEFT;
                }
            }
            break;

        case ARM_WAIT_LEFT:
            wait_ctr++;
            if (wait_ctr >= ARM_WAIT_STEPS) {
                wait_ctr = 0;
                ARM_REV(ARM_DIR_RIGHT);
                state = ARM_TRAVERSE;
            }
            break;

        case ARM_TRAVERSE:
            accumulator += gear_um;
            while (accumulator >= ARM_UM_PER_REV) {
                accumulator -= ARM_UM_PER_REV;
                if (arm_target_position < travel_steps)
                    arm_target_position++;
                else {
                    accumulator = 0;
                    wait_ctr    = 0;
                    ARM_REV(ARM_DIR_LEFT);
                    state = ARM_WAIT_RIGHT;
                }
            }
            break;

        case ARM_WAIT_RIGHT:
            wait_ctr++;
            if (wait_ctr >= ARM_WAIT_STEPS) {
                wait_ctr = 0;
                state    = ARM_RETURN;
            }
            break;

        case ARM_RETURN:
            accumulator += gear_um;
            while (accumulator >= ARM_UM_PER_REV) {
                accumulator -= ARM_UM_PER_REV;
                if (arm_target_position > 0)
                    arm_target_position--;
                else {
                    accumulator = 0;
                    wait_ctr    = 0;
                    ARM_REV(ARM_DIR_RIGHT);
                    state = ARM_WAIT_LEFT;
                }
            }
            break;
    }
}

// ─────────────────────────────────────────────
//  GO_TO_ZERO — retour à LEFT, nouvelle bobine
// ─────────────────────────────────────────────

void ARM_GoToZero(void)
{
    accumulator = 0;
    wait_ctr    = 0;
    ARM_REV(ARM_DIR_LEFT);
    state = ARM_GO_TO_ZERO;
}

// ─────────────────────────────────────────────
//  Écriture registre GEAR (0x0802)
// ─────────────────────────────────────────────

void ARM_SetGear(uint16_t um)
{
    gear_um     = um;
    accumulator = 0;
}

// ─────────────────────────────────────────────
//  Écriture registre OFFSET (0x0803) + EEPROM
// ─────────────────────────────────────────────

void ARM_SetOffset(uint16_t um)
{
    arm_offset = um;
    EE_WriteWord(EE_ARM_OFFSET_H, EE_ARM_OFFSET_L, um);
}

uint16_t ARM_GetOffset(void)
{
    return arm_offset;
}

// ─────────────────────────────────────────────
//  Free ARM — appelé depuis REG_CTRL bit1
// ─────────────────────────────────────────────

void ARM_SetFree(uint8_t s)
{
    free_arm = s;
    ARM_EN(s ? 0 : 1);
}

// ─────────────────────────────────────────────
//  Positions limites gauche/droite (1/10 mm)
// ─────────────────────────────────────────────

void ARM_SetLeft(uint16_t dmm)
{
    pos_left     = dmm;
    travel_steps = ARM_CalcTravelSteps();
}

void ARM_SetRight(uint16_t dmm)
{
    pos_right    = dmm;
    travel_steps = ARM_CalcTravelSteps();
}