/* main.c -- Project 3 bring-up on a simulated Pico (wokwi.com)
 *
 * The condition monitor of motormon/main.c without the Linux parts (web
 * dashboard, CSV log, files, threads).  The vibration sampling runs in the
 * main loop at 100 samples/s instead of in a thread.  Dashboard commands
 * are typed in the serial monitor:
 *     1..9  speed 10..90 %      0  speed 100 %
 *     l     learn the baseline (10 s)        k  acknowledge maintenance
 *     s     start (= START)     x  stop (= RESET)
 * In Wokwi the "Motor test rig" chip is the machine: it turns the PWM into
 * rpm and current, sends the encoder pulses, and answers as the INA219 and
 * the MPU-6050.  Slide "load" to overload it, "vibration" to unbalance it.
 * The NTC + MCP3008 are separate parts, as on the real bench.
 * Drivers ina219.c, mpu6050.c, mcp3008.c, panel.c, oled.c, util.c are the
 * project's own files, unchanged.
 */
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "lgpio.h"
#include "panel.h"
#include "oled.h"
#include "util.h"
#include "ina219.h"
#include "mpu6050.h"
#include "mcp3008.h"
#include "keys.h"

#define AIN1    18
#define AIN2    23
#define ENCODER 17
#define NTC_CH  0

#define PWM_HZ          500.0
#define PULSES_PER_REV  20.0
#define R_FIXED     10000.0
#define NTC_R0      10000.0
#define NTC_BETA     3950.0
#define VIB_WARN_X      2.0
#define VIB_ALARM_X     4.0
#define CUR_WARN_X      1.5
#define CUR_ALARM_X     2.5
#define TEMP_WARN_C    45.0
#define TEMP_ALARM_C   60.0
#define RPM_DROP_WARN   0.30
#define MAINT_HOURS     0.25

enum state { STOPPED, RUNNING, ESTOP, TRIPPED };
static const char *STATE_NAME[] = {"STOPPED", "RUNNING", "E-STOP", "TRIPPED"};
static enum state state = STOPPED;
static int    duty = 60;
static double rpm = 0, current_a = 0, bus_v = 0, vib_mg = 0, temp_c = 0;
static int    health = 100;
static char   warnings[96] = "", alarm_text[48] = "";
static double base_vib = 0, base_cur = 0, base_rpm = 0; static int base_duty = 0;
static int    learning = 0, learn_n = 0;
static double learn_t0 = 0, learn_vib = 0, learn_cur = 0, learn_rpm = 0;
static double run_hours = 0, hours_at_maint = 0;
static volatile unsigned long enc_pulses = 0;
static int h, ina, mpu, adc;

static void enc_cb(int n, lgGpioAlert_p a, void *ud)
{
    int i;
    (void)ud;
    for (i = 0; i < n; i++)
        if (a[i].report.level == 1)
            enc_pulses++;
}

/* 100 samples/s; every 100 samples: std per axis, combined -> milli-g */
static void vib_sample(void)
{
    static double buf[3][100];
    static int n = 0;
    int i, k;
    if (mpu_read_accel(mpu, &buf[0][n], &buf[1][n], &buf[2][n]) == 0)
        n++;
    if (n == 100) {
        double total = 0;
        for (k = 0; k < 3; k++) {
            double mean = 0, var = 0;
            for (i = 0; i < n; i++) mean += buf[k][i];
            mean /= n;
            for (i = 0; i < n; i++) var += (buf[k][i] - mean) * (buf[k][i] - mean);
            total += var / n;
        }
        vib_mg = sqrt(total) * 1000.0;
        n = 0;
    }
}

static double ntc_temp(int code)
{
    double r, inv_t;
    if (code <= 0 || code >= 1023)
        return -999;
    /* Wokwi's NTC module has the thermistor on the GND side of its 10 k
       divider; the bench circuit has it on the 3.3 V side, where
       r = R_FIXED * (1023.0 / code - 1.0) as in motormon/main.c.       */
    r = R_FIXED * code / (1023.0 - code);
    inv_t = 1.0 / 298.15 + log(r / NTC_R0) / NTC_BETA;
    return 1.0 / inv_t - 273.15;
}

static void motor(int d)
{
    d = clampi(d, 0, 100);
    lgTxPwm(h, AIN1, PWM_HZ, d, 0, 0);
}

static void assess(void)
{
    int score = 100;
    warnings[0] = '\0';
    if (state != RUNNING) {
        if (state != TRIPPED) alarm_text[0] = '\0';
        health = score;
        return;
    }
    alarm_text[0] = '\0';
    if (base_vib > 0) {
        if (vib_mg > VIB_ALARM_X * base_vib) { strcat(warnings, "VIB-HIGH "); snprintf(alarm_text, sizeof alarm_text, "VIBRATION TRIP"); score -= 60; }
        else if (vib_mg > VIB_WARN_X * base_vib) { strcat(warnings, "VIB "); score -= 25; }
        if (current_a > CUR_ALARM_X * base_cur) { strcat(warnings, "OVERLOAD "); snprintf(alarm_text, sizeof alarm_text, "OVERCURRENT TRIP"); score -= 60; }
        else if (current_a > CUR_WARN_X * base_cur) { strcat(warnings, "CURRENT "); score -= 20; }
        if (duty == base_duty && base_rpm > 0 && rpm < (1.0 - RPM_DROP_WARN) * base_rpm) { strcat(warnings, "SPEED-LOW "); score -= 20; }
    } else
        strcat(warnings, "NO-BASELINE ");
    if (temp_c > TEMP_ALARM_C) { strcat(warnings, "OVERTEMP "); snprintf(alarm_text, sizeof alarm_text, "OVERTEMPERATURE TRIP"); score -= 60; }
    else if (temp_c > TEMP_WARN_C) { strcat(warnings, "TEMP "); score -= 15; }
    if (run_hours - hours_at_maint >= MAINT_HOURS) { strcat(warnings, "MAINT-DUE "); score -= 10; }
    health = clampi(score, 0, 100);
}

static void show_oled(void)
{
    oled_clear();
    oled_printf(0, 0, "MOTOR %s %3d%%", state == RUNNING ? "RUN" : state == STOPPED ? "STOP" : state == ESTOP ? "ESTOP" : "TRIP", duty);
    oled_printf(1, 0, "RPM   %6.0f", rpm);
    oled_printf(2, 0, "CUR   %6.3f A", current_a);
    oled_printf(3, 0, "VIB   %6.1f mg", vib_mg);
    oled_printf(4, 0, "TEMP  %6.1f C", temp_c);
    oled_printf(5, 0, "HEALTH %3d  %4.2fh", health, run_hours);
    oled_bar(6, health);
    oled_printf(7, 0, "%s", alarm_text[0] ? alarm_text : warnings[0] ? warnings : "OK");
    oled_show();
}

static void enter(enum state s)
{
    state = s;
    if (s == STOPPED) { motor(0); panel_light(0, 1, 0); }
    if (s == RUNNING) { motor(duty); panel_light(0, 0, 1); }
    if (s == ESTOP)   { motor(0); panel_light(1, 0, 0); panel_beep(1000, 800); }
    if (s == TRIPPED) { motor(0); panel_light(1, 0, 0); panel_beep(600, 1000); }
    printf("state %s %s\n", STATE_NAME[s], alarm_text);
}

int main(void)
{
    double t_prev, last_fast = 0, last_print = 0, last_vib = 0;
    unsigned long pulses_prev = 0;
    int last_duty = -1;
    h = lgGpiochipOpen(0);
    if (h < 0) { printf("cannot open GPIO chip\n"); return 1; }
    if (panel_init(h, 0) < 0) { printf("panel pins busy\n"); return 1; }
    lgGpioClaimOutput(h, 0, AIN1, 0);
    lgGpioClaimOutput(h, 0, AIN2, 0);
    lgGpioSetAlertsFunc(h, ENCODER, enc_cb, NULL);
    lgGpioSetDebounce(h, ENCODER, 200);
    if (lgGpioClaimAlert(h, LG_SET_PULL_UP, LG_RISING_EDGE, ENCODER, -1) < 0) { printf("encoder pin busy\n"); return 1; }
    ina = ina_init(1);
    if (ina < 0) { printf("no INA219 at 0x40 -- check SDA/SCL wiring\n"); return 1; }
    mpu = mpu_init(1);
    if (mpu < 0) { printf("no MPU6050 at 0x68 -- check wiring\n"); return 1; }
    adc = mcp_init();
    if (adc < 0) { printf("SPI failed\n"); return 1; }
    if (oled_init(1, 0x3C) < 0) printf("no OLED at 0x3C (continuing)\n");
    enter(STOPPED);
    printf("motormon ready.  Keys: s start, x stop, 1-9/0 speed, l learn baseline, k maintenance done\n");
    t_prev = now_s();
    while (1) {
        double t = now_s();
        int k;
        if (t - last_vib >= 0.01) { vib_sample(); last_vib = t; }
        if (panel_estop()) {
            if (state != ESTOP) enter(ESTOP);
        } else if (state == ESTOP) {
            panel_light(1, 1, 0);
            if (panel_start_pressed()) enter(STOPPED);
        } else {
            if (panel_start_pressed()) { if (state == STOPPED) enter(RUNNING); }
            if (panel_reset_pressed()) { if (state == RUNNING || state == TRIPPED) enter(STOPPED); }
        }
        k = key_pressed();
        if (k >= '1' && k <= '9') duty = (k - '0') * 10;
        else if (k == '0') duty = 100;
        else if (k == 's' && state == STOPPED) enter(RUNNING);
        else if (k == 'x' && (state == RUNNING || state == TRIPPED)) enter(STOPPED);
        else if (k == 'l' && state == RUNNING && !learning) {
            learning = 1; learn_t0 = t; learn_vib = learn_cur = learn_rpm = 0; learn_n = 0;
            printf("learning the baseline for 10 s ...\n");
        } else if (k == 'k') { hours_at_maint = run_hours; printf("maintenance done\n"); }
        if (state == RUNNING && duty != last_duty) motor(duty);
        last_duty = duty;
        if (state == RUNNING) run_hours += (t - t_prev) / 3600.0;
        t_prev = t;
        if (t - last_fast >= 0.5) {
            unsigned long p = enc_pulses;
            rpm = (p - pulses_prev) / PULSES_PER_REV / (t - last_fast) * 60.0;
            pulses_prev = p;
            last_fast = t;
            current_a = ina_current_a(ina);
            bus_v = ina_bus_v(ina);
            temp_c = ntc_temp(mcp_read(adc, NTC_CH));
            if (learning) {
                learn_vib += vib_mg; learn_cur += current_a; learn_rpm += rpm; learn_n++;
                if (t - learn_t0 >= 10.0) {
                    base_vib = learn_vib / learn_n; base_cur = learn_cur / learn_n;
                    base_rpm = learn_rpm / learn_n; base_duty = duty;
                    learning = 0;
                    printf("baseline: vib %.1f mg, current %.3f A, rpm %.0f at %d %%\n", base_vib, base_cur, base_rpm, base_duty);
                    panel_beep(2000, 200);
                }
            }
            assess();
            if (alarm_text[0] && state == RUNNING) enter(TRIPPED);
            if (state == RUNNING) panel_light(0, warnings[0] != '\0', warnings[0] == '\0');
            show_oled();
        }
        if (t - last_print >= 1.0) {
            last_print = t;
            printf("%s duty %3d %%  rpm %5.0f  cur %.3f A  bus %.2f V  vib %5.1f mg  temp %5.1f C  health %3d  %s%s\n",
                   STATE_NAME[state], duty, rpm, current_a, bus_v, vib_mg, temp_c, health, warnings, alarm_text);
        }
        lguSleep(0.005);
    }
    return 0;
}