#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <time.h>
#include <sys/mman.h>
#include <pthread.h>
#include <sched.h>
#include <stdint.h>

// پایه‌های رجیستر GPADC و GPIO
#define GPADC_BASE      0x02009000
#define GP_SR_CON       0x0000
#define GP_CTRL         0x0004
#define GP_CS_EN        0x0008
#define GP_DATA_INTS    0x0038
#define GP_CH0_DATA     0x0080
#define ADC_VREF 1.8
#define VCC 3.3
#define ADC_MAX 4095.0
#define R1 10000.0
#define R0 10000.0
#define T0 298.15
#define B 3950.0
#define GPIO_BASE       0x02000000
#define PB_CFG0         0x0030
#define PB_DAT          0x0040
#define PE_CFG0         0x00C0
#define PE_DAT          0x00D0
#define PB5_SHIFT       20
#define PB5_MASK        (1u << 5)
#define PE0_SHIFT       0
#define PE0_MASK        (1u << 0)
#define FUNC_OUTPUT     0x1
#define PWM_PIN         PB5_MASK
#define MODE_PIN        PE0_MASK

static volatile uint32_t *gpio;
static volatile uint32_t *gpadc;
static volatile int pwm_duty_cycle = 0;
static volatile int mode = 0;
static volatile double shared_temperature = 0.0;
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

// توابع تایمر
static inline uint32_t read_cntfrq(void) {
    uint32_t v;
    asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r"(v));
    return v;
}

static inline uint64_t read_cntpct(void) {
    uint32_t lo, hi;
    asm volatile("mrrc p15, 0, %0, %1, c14" : "=r"(lo), "=r"(hi));
    return ((uint64_t)hi << 32) | lo;
}

// ساختار فیلتر کالمن
typedef struct {
    double estimate;
    double errorEstimate;
    double processNoise;
    double measurementNoise;
} KalmanFilter;

void initKalman(KalmanFilter *kf, double processNoise, double measurementNoise) {
    kf->processNoise = processNoise;
    kf->measurementNoise = measurementNoise;
}

double kalmanUpdate(KalmanFilter *kf, double measurement) {
    double predictedEstimate = kf->estimate;
    double predictedError = kf->errorEstimate + kf->processNoise;
    double kalmanGain = predictedError / (predictedError + kf->measurementNoise);
    kf->estimate = predictedEstimate + kalmanGain * (measurement - predictedEstimate);
    kf->errorEstimate = (1.0 - kalmanGain) * predictedError;
    return kf->estimate;
}

// ساختار PID
typedef struct {
    double Kp, Ki, Kd;
    double prevError;
    double integral;
    double derivative;
    double maxOutput;
} PID;

void initPID(PID *pid, double kp, double ki, double kd, double max_output) {
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->prevError = 0;
    pid->integral = 0;
    pid->derivative = 0;
    pid->maxOutput = max_output;
}

// ساختار جدید برای محدوده ضرایب PID
typedef struct {
    double Kp_min, Kp_max;
    double Ki_min, Ki_max;
    double Kd_min, Kd_max;
} PIDBounds;

// ساختار برای ذخیره معیارهای عملکرد
typedef struct {
    double rise_time; // زمان صعود (ثانیه)
    double overshoot; // overshoot (درجه)
    double steady_state_error; // خطای حالت ماندگار (درجه)
} PerformanceMetrics;

// توابع اولویت و اتصال به هسته
void set_realtime_priority(void) {
    struct sched_param sp = { .sched_priority = 90 };
    if (sched_setscheduler(0, SCHED_FIFO, &sp))
        perror("خطا در تنظیم اولویت بلادرنگ");
}

void set_affinity(int cpu) {
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(cpu, &cpuset);
    if (pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset) != 0)
        perror("خطا در اتصال به هسته");
}

// توابع دسترسی به رجیستر
static inline void reg_write(volatile uint32_t *base, uint32_t off, uint32_t clr_mask, uint32_t set_mask) {
    volatile uint32_t *addr = base + (off >> 2);
    uint32_t val = *addr;
    val = (val & ~clr_mask) | set_mask;
    *addr = val;
}

static inline uint32_t gpadc_read(uint32_t off) {
    return *(volatile uint32_t*)(gpadc + (off>>2));
}

static inline void gpadc_write(uint32_t off, uint32_t val) {
    *(volatile uint32_t*)(gpadc + (off>>2)) = val;
}

int readGPADCRaw() {
    while (!(gpadc_read(GP_DATA_INTS) & 0x1));
    gpadc_write(GP_DATA_INTS, 1);
    return gpadc_read(GP_CH0_DATA) & 0x0FFF;
}

void measure_gpadc_time(void) {
    const uint32_t cnt_hz = read_cntfrq();
    const int num_trials = 100;
    uint64_t total_ticks = 0;
    for (int i = 0; i < num_trials; i++) {
        uint64_t start = read_cntpct();
        int raw = readGPADCRaw();
        uint64_t end = read_cntpct();
        total_ticks += (end - start);
    }
    double avg_ticks = (double)total_ticks / num_trials;
    double avg_time_us = (avg_ticks / cnt_hz) * 1e6;
    printf("میانگین زمان اجرای readGPADCRaw: %.2f میکروثانیه\n", avg_time_us);
}

void setupPins() {
    int fd = open("/dev/mem", O_RDWR | O_SYNC);
    if (fd < 0) { perror("open /dev/mem"); exit(1); }
    size_t pagesz = (size_t)sysconf(_SC_PAGESIZE);
    gpio = mmap(NULL, pagesz, PROT_READ | PROT_WRITE, MAP_SHARED, fd, GPIO_BASE);
    close(fd);
    if (gpio == MAP_FAILED) { perror("mmap"); exit(1); }
    reg_write(gpio, PB_CFG0, 0xFu << PB5_SHIFT, FUNC_OUTPUT << PB5_SHIFT);
    reg_write(gpio, PE_CFG0, 0xFu << PE0_SHIFT, FUNC_OUTPUT << PE0_SHIFT);
}

void initGPADC() {
    int fd = open("/dev/mem", O_RDWR | O_SYNC);
    if (fd < 0) { perror("open /dev/mem for GPADC"); exit(1); }
    size_t pagesz = sysconf(_SC_PAGESIZE);
    gpadc = mmap(NULL, pagesz, PROT_READ | PROT_WRITE, MAP_SHARED, fd, GPADC_BASE);
    close(fd);
    if (gpadc == MAP_FAILED) { perror("mmap GPADC"); exit(1); }
    gpadc_write(GP_SR_CON, (0x17 << 16) | 0x9);
    gpadc_write(GP_CS_EN, 0x1);
    uint32_t ctrl = gpadc_read(GP_CTRL);
    ctrl &= ~(0x3 << 18);
    ctrl |= (0x2 << 18) | (1 << 16);
    gpadc_write(GP_CTRL, ctrl);
}

double read_temperature() {
    double raw = readGPADCRaw();
    double adc_voltage = (raw * ADC_VREF) / ADC_MAX;
    double R_therm = R1 * (adc_voltage / (VCC - adc_voltage));
    double lnR = log(R_therm / R0);
    double T_kelvin = 1.0 / ((1.0 / T0) + (1.0 / B) * lnR);
    return T_kelvin - 273.15;
}

double read_filtered_temperature(KalmanFilter *kf) {
    pthread_mutex_lock(&mutex);
    double rawTemp = shared_temperature;
    pthread_mutex_unlock(&mutex);
    return kalmanUpdate(kf, rawTemp);
}

// تابع PWM
void* pwm_thread(void* arg) {
    set_affinity(0);
    set_realtime_priority();
    const uint32_t cnt_hz = read_cntfrq();
    const uint64_t period = cnt_hz / 25000;
    volatile uint32_t *pb_dat = gpio + (PB_DAT >> 2);
    volatile uint32_t *pe_dat = gpio + (PE_DAT >> 2);
    int duty = 0;
    int current_mode = 0;
    uint64_t t = read_cntpct();
    while (1) {
        pthread_mutex_lock(&mutex);
        duty = pwm_duty_cycle;
        current_mode = mode;
        pthread_mutex_unlock(&mutex);
        uint64_t high_ticks = (period * duty) / 100;
        uint64_t low_ticks = period - high_ticks;
        if (duty > 0) {
            *pb_dat |= PWM_PIN;
        }
        uint64_t target = t + high_ticks;
        while (read_cntpct() < target);
        *pb_dat &= ~PWM_PIN;
        target = t + period;
        pthread_mutex_lock(&mutex);
        shared_temperature = read_temperature();
        pthread_mutex_unlock(&mutex);
        if (current_mode) {
            *pe_dat &= ~MODE_PIN;
        } else {
            *pe_dat |= MODE_PIN;
        }
        while (read_cntpct() < target);
        t += period;
    }
    return NULL;
}

// تابع محاسبه PID
double computePID(PID *pid, double setpoint, double measured, double dt, double minOutput, double maxOutput, int invertError, double feedforward) {
    double error = invertError ? (measured - setpoint) : (setpoint - measured);
    pid->integral += error * dt;
    double raw_derivative = (error - pid->prevError) / dt;
    double alpha = 0.8;
    pid->derivative = alpha * raw_derivative + (1 - alpha) * pid->derivative;
    double output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * pid->derivative + feedforward;
    if (output > maxOutput) {
        output = maxOutput;
        if (error > 0) pid->integral -= error * dt;
    } else if (output < minOutput) {
        output = minOutput;
        if (error < 0) pid->integral -= error * dt;
    }
    pid->prevError = error;
    return output;
}

// فیدفوروارد
double calculate_feedforward(double targetTemp, double currentTemp) {

    
    double k = 0.3;  // ضریب تجربی - قابل تنظیم با آزمایش
    double bias = 0.15; // بایاس تجربی - قابل تنظیم با آزمایش
    double feedforward ;

    if (targetTemp < 30) {
        feedforward =  k * (targetTemp - currentTemp) + bias;
    } else if (targetTemp < 50) {
        feedforward =  2k * (targetTemp - currentTemp) + bias;
    } else {
        feedforward =  3k * (targetTemp - currentTemp) + bias;
    }
    return feedforward;
}

// تابع timespec_add (انتقال به اینجا برای جلوگیری از خطای implicit declaration)
static inline void timespec_add(struct timespec *res, const struct timespec *a, const struct timespec *b) {
    res->tv_sec = a->tv_sec + b->tv_sec;
    res->tv_nsec = a->tv_nsec + b->tv_nsec;
    if (res->tv_nsec >= 1000000000L) {
        res->tv_sec++;
        res->tv_nsec -= 1000000000L;
    }
}



// تابع جدید برای تنظیم خودکار ضرایب PID
void autoTunePID(PID *pid, PerformanceMetrics *metrics, PIDBounds *bounds, double targetTemp, double currentTemp, double dt) {
    const double rise_time_threshold = 35.0; // ثانیه
    const double overshoot_threshold = 0.25; // درجه
    const double steady_state_error_threshold = 0.5; // درجه

    double error = fabs(targetTemp - currentTemp);

    if (metrics->rise_time > rise_time_threshold) {
        pid->Kp = fmin(pid->Kp * 1.05, bounds->Kp_max);
        pid->Ki = fmin(pid->Ki * 1.05, bounds->Ki_max);

    }
    if (metrics->overshoot > overshoot_threshold) {
        pid->Kd = fmin(pid->Kd * 1.1, bounds->Kd_max);
        pid->Kp = fmax(pid->Kp * 0.95, bounds->Kp_min);
        pid->Ki = fmax(pid->Ki * 0.95, bounds->Ki_min);
       
    }
    if (metrics->steady_state_error > steady_state_error_threshold && metrics->steady_state_error > 0) {
        pid->Ki = fmin(pid->Ki * 1.05, bounds->Ki_max);

    }
}

// تابع reachTemperature
void reachTemperature(int stageNumber, double targetTemp, int holdTimeSeconds, double pwm_min, double pwm_max, 
                     PID *pid_heat, PID *pid_cool, PID *pid_first, double firstMeas, KalmanFilter *kf, PIDBounds *bounds) {
    const double dt = 0.125;
    const double stability_threshold = 0.55;
    const double reset_threshold = 4;
    const double coolingThreshold = 3;

    struct timespec start_time, next_time;
    clock_gettime(CLOCK_MONOTONIC, &start_time);
    clock_gettime(CLOCK_MONOTONIC, &next_time);

    long stable_ms = 0;
    long hold_ms = holdTimeSeconds * 1000;
    double max_temp = firstMeas;
    int rise_time_reached = 0;
    double rise_time = 0.0;
    double steady_state_error_sum = 0.0;
    int steady_state_count = 0;

    PerformanceMetrics metrics = {0.0, 0.0, 0.0};

    printf("\n--- شروع مرحله %d: هدف = %.1f°C ---\n", stageNumber, targetTemp);

    while (stable_ms < hold_ms) {
        timespec_add(&next_time, &next_time, &(struct timespec){0, (long)(dt * 1e9)});
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL);
        double currentTemp = read_filtered_temperature(kf);

        struct timespec now;
        clock_gettime(CLOCK_MONOTONIC, &now);
        double elapsed_time = (now.tv_sec - start_time.tv_sec) + (now.tv_nsec - start_time.tv_nsec) / 1e9;

        if (!rise_time_reached && currentTemp >= targetTemp * 0.8) {
            rise_time = elapsed_time;
            rise_time_reached = 1;
        }

        if (currentTemp > max_temp) {
            max_temp = currentTemp;
        }
        metrics.overshoot = fmax(0.0, max_temp - targetTemp);

        double error = targetTemp - currentTemp;
        if (fabs(error) <= stability_threshold) {
            steady_state_error_sum += fabs(error);
            steady_state_count++;
            metrics.steady_state_error = steady_state_count > 0 ? steady_state_error_sum / steady_state_count : 0.0;
        }

        metrics.rise_time = rise_time_reached ? rise_time : elapsed_time;

        autoTunePID(pid_heat, &metrics, bounds, targetTemp, currentTemp, dt);

        int isCooling = (currentTemp > targetTemp + coolingThreshold);
        int dutyCycle = 0;
        int currentMode = 0;
        double feedforward = calculate_feedforward(targetTemp, currentTemp);

        if (!isCooling && targetTemp > firstMeas) {
            currentMode = 1;
            double pidOutput = computePID(pid_heat, targetTemp, currentTemp, dt, pwm_min, pwm_max, 0, feedforward);
            dutyCycle = (int)pidOutput;
        } else if (isCooling && targetTemp > firstMeas) {
            currentMode = 0;
            double pidOutput = computePID(pid_cool, targetTemp, currentTemp, dt, pwm_min, pwm_max, 1, feedforward);
            dutyCycle = 0.4 * (int)pidOutput;

        } else if (targetTemp < firstMeas) {
            currentMode = 0;
            double pidOutput = computePID(pid_cool, targetTemp, currentTemp, dt, pwm_min, pwm_max, 1, feedforward);
            dutyCycle = (int)pidOutput;
        }

        if (dutyCycle > pwm_max) dutyCycle = pwm_max;
        if (dutyCycle < pwm_min) dutyCycle = pwm_min;

        pthread_mutex_lock(&mutex);
        pwm_duty_cycle = dutyCycle;
        mode = currentMode;
        pthread_mutex_unlock(&mutex);

        if (fabs(error) <= stability_threshold) {
            stable_ms += (long)(dt * 1000);
        } else if (fabs(error) > reset_threshold) {
            stable_ms = 0;
        }

        static double acc_print = 0;
        acc_print += dt;
        if (acc_print >= 1) {
            printf("مرحله %d - %s: دما=%.2f°C, خطا=%.2f°C, PWM=%d%%, حالت=%s, پایدار=%dms, Kp=%.2f, Ki=%.3f, Kd=%.3f\n",
                   stageNumber, isCooling ? "سرمایش" : "گرمایش",
                   currentTemp, error, dutyCycle, currentMode ? "گرمایش" : "سرمایش", 
                   stable_ms, pid_heat->Kp, pid_heat->Ki, pid_heat->Kd);
            acc_print = 0;
        }
    }

    pthread_mutex_lock(&mutex);
    pwm_duty_cycle = 0;
    mode = 0;
    pthread_mutex_unlock(&mutex);
    printf("--- پایان مرحله %d: دما نهایی = %.2f°C ---\n", stageNumber, shared_temperature);
}

// تابع main
int main() {
    int numStages;
    double pwm_min = 0.0;
    double pwm_max = 50.0;
    double Q = 0.07, R = 0.68;
    double Kp_heat, Ki_heat, Kd_heat;
    PIDBounds bounds = {
        .Kp_min = 0.04, .Kp_max = 0.3,
        .Ki_min = 0.15, .Ki_max = 0.3,
        .Kd_min = 5, .Kd_max = 6
    };

    setupPins();
    initGPADC();
    measure_gpadc_time();

    fprintf(stderr, "تعداد مراحل را وارد کنید: ");
    scanf("%d", &numStages);
    double targetTemps[numStages];
    int holdTimes[numStages];

    for (int i = 0; i < numStages; i++) {
        fprintf(stderr, "دمای هدف برای مرحله %d (°C): ", i + 1);
        scanf("%lf", &targetTemps[i]);
        fprintf(stderr, "زمان نگه‌داری برای مرحله %d (ثانیه): ", i + 1);
        scanf("%d", &holdTimes[i]);
    }

    fprintf(stderr, "ضریب Kp برای گرمایش: ");
    scanf("%lf", &Kp_heat);
    fprintf(stderr, "ضریب Ki برای گرمایش: ");
    scanf("%lf", &Ki_heat);
    fprintf(stderr, "ضریب Kd برای گرمایش: ");
    scanf("%lf", &Kd_heat);
    fprintf(stderr, "حداقل و حداکثر Kp (مثل 1.0 5.0): ");
    scanf("%lf %lf", &bounds.Kp_min, &bounds.Kp_max);
    fprintf(stderr, "حداقل و حداکثر Ki (مثل 0.01 0.1): ");
    scanf("%lf %lf", &bounds.Ki_min, &bounds.Ki_max);
    fprintf(stderr, "حداقل و حداکثر Kd (مثل 0.05 0.3): ");
    scanf("%lf %lf", &bounds.Kd_min, &bounds.Kd_max);

    double firstMeas = read_temperature();
    KalmanFilter kf;
    initKalman(&kf, Q, R);
    kf.estimate = firstMeas;
    kf.errorEstimate = 1.0;

    PID pid_heat, pid_cool, pid_first;
    initPID(&pid_heat, Kp_heat, Ki_heat, Kd_heat, pwm_max);
    initPID(&pid_cool, 0.1, 0.25, 4.1, pwm_max);
    initPID(&pid_first, 1.5, 0.1, 8, pwm_max);

    pthread_t pwm_tid;
    if (pthread_create(&pwm_tid, NULL, pwm_thread, NULL) != 0) {
        perror("خطا در ایجاد رشته PWM");
        return 1;
    }

    set_affinity(1);

    for (int i = 0; i < numStages; i++) {
        reachTemperature(i + 1, targetTemps[i], holdTimes[i], pwm_min, pwm_max, 
                        &pid_heat, &pid_cool, &pid_first, firstMeas, &kf, &bounds);
    }

    pthread_cancel(pwm_tid);
    pthread_join(pwm_tid, NULL);
    printf("پایان فرآیند ترمال سایکلر\n");
    return 0;
}