#include <main.h>
#include<math.h>
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7, bits=8)
#use i2c(Master, Slow, sda=PIN_D1, scl=PIN_D0, force_hw)

#define AVERAGE_COUNT 5
float temp_buffer[AVERAGE_COUNT];
int temp_index = 0;
int temp_filled = 0;

// --- Per-channel zero bias (measure when no current flows) ---
float ct_zero_bias[4] = {0.0, 0.0, 0.0, 0.0};  // 1=L1, 2=L2, 3=L3

// Scaling factor to match real current
float ct_scale[4] = {1, 1, 1, 1}; 

// --- ZMPT101B zero scale ---
float zmpt_scale[8] = {1,1,1,1,1, 714.0,714.0,714.0}; 
// index matches channel number (5=L1, 6=L2, 7=L3)

#define RMS_AVG_COUNT 5

// Rolling buffers for each CT channel
float rms_buffer_ch1[RMS_AVG_COUNT] = {0};
float rms_buffer_ch2[RMS_AVG_COUNT] = {0};
float rms_buffer_ch3[RMS_AVG_COUNT] = {0};
int rms_index = 0;
int rms_filled = 0;

#define MCP23017_ADDRESS  0x40
#define IODIRA            0x00
#define IODIRB            0x01
#define GPIOB             0x13
#define OLATA             0x14


#define SCK_PIN PIN_B4
#define MISO_PIN PIN_B5
#define CS_PIN PIN_A5
#define LM35_PIN PIN_A0


// Functions 
void mcp23017_init();
void mcp23017_writeA(BYTE data);
BYTE mcp23017_readB();
void rs232_listener();
float read_max31855_temperature();
float read_lm35_temperature();
void  max31855_init();


// Global variable to store GPIOA state
BYTE gpioa_state = 0x00;

//Functions
float read_current_rms_debug(int channel, int settle_timeout_ms, float settle_threshold);
float read_voltage_rms_debug(int channel, int settle_timeout_ms, float settle_threshold);

// --- Thermocouple Calibration ---
float tc_offset = -25.0;
float tc_slope  = 1.0;  // keep slope 1 for now

//Heartbeat
unsigned int16 comm_timeout = 0; // increments every loop when no comm
#define HEARTBEAT_TIMEOUT_MS 1000  // 5 seconds

// Global state variables for RD4 and RB1
int rd4_state = 0;  // 0 = LOW, 1 = HIGH
int rb1_state = 0;

//Global flag to indicate emergency stop 
int emergency_active = 0;
void check_emergency_stop();

void main() {
    setup_adc_ports(sAN0 | sAN1 | sAN2 | sAN3 | sAN5 | sAN6 | sAN7);
    setup_adc(ADC_CLOCK_DIV_32);

    printf("Initializing...\n");
    mcp23017_init();
    max31855_init();
    // --- Add 3-second delay for CTs to settle ---
    printf("Waiting 3 seconds for CTs to settle...\n");
    delay_ms(3000);
    mcp23017_writeA(gpioa_state);
    printf("Ready...\n");   
    
    // --- Set RD4 as output (existing) ---
    set_tris_d(get_tris_d() & 0xEF);  // RD4 as output (bit 4 = 0)
    output_low(PIN_D4);               // Initially OFF

    // --- Set RB1 as output ---
    set_tris_b(get_tris_b() & 0xFD);  // RB1 as output (bit 1 = 0)
    output_low(PIN_B1);               // Initially OFF

    while(TRUE) {
        check_emergency_stop();   //emergency check
        rs232_listener();  // reset comm_timeout if 'H' received

        // --- increment comm_timeout by time spent in loop ---
        comm_timeout += 200 + 200 + 200; // sum of all delay_ms() in loop

        if(comm_timeout >= HEARTBEAT_TIMEOUT_MS) {
            gpioa_state = 0x00;
            output_low(PIN_D4);
            output_low(PIN_B1);
            mcp23017_writeA(gpioa_state);
            printf("Comm lost -> All outputs OFF\n");
            comm_timeout = 0;
        }
        
        rs232_listener();
        
        //Tiny delay to avoid overclocking ADC      
        delay_us(50);
        // Read LM35 thermocouple
        float lm_temp = read_lm35_temperature();
        // Read MAX31855 thermocouple
        float tc_temp = read_max31855_temperature();
        
        // --- Store valid thermocouple readings and compute moving average ---
        if(tc_temp != -1.0) {  // Only store valid readings
            temp_buffer[temp_index] = tc_temp;

            // Move index forward in circular buffer
            temp_index = (temp_index + 1) % AVERAGE_COUNT;

            // Track how many valid samples we have (max = AVERAGE_COUNT)
            if(temp_filled < AVERAGE_COUNT) 
                temp_filled++;
        }

        // Default value if no valid readings yet
        float avg_tc_temp = -1.0;

        // Compute average if we have at least 1 valid reading
        if(temp_filled > 0) {
            float sum = 0.0;

            for(int i = 0; i < temp_filled; i++) {
                sum += temp_buffer[i];
            }

            avg_tc_temp = sum / temp_filled;
        }

        float ct1 = read_current_rms_debug(1, 3000, 0.002); // 3000ms max, 2mV threshold
        float ct2 = read_current_rms_debug(2, 3000, 0.002);
        float ct3 = read_current_rms_debug(3, 3000, 0.002);

        // --- Store in rolling buffers ---
        rms_buffer_ch1[rms_index] = ct1;
        rms_buffer_ch2[rms_index] = ct2;
        rms_buffer_ch3[rms_index] = ct3;

        rms_index = (rms_index + 1) % RMS_AVG_COUNT;
        if(rms_filled < RMS_AVG_COUNT) rms_filled++;

        // --- Compute averaged values ---
        float avg_ct1 = 0, avg_ct2 = 0, avg_ct3 = 0;
        for(int i = 0; i < rms_filled; i++){
            avg_ct1 += rms_buffer_ch1[i];
            avg_ct2 += rms_buffer_ch2[i];
            avg_ct3 += rms_buffer_ch3[i];
        }
        avg_ct1 /= rms_filled;
        avg_ct2 /= rms_filled;
        avg_ct3 /= rms_filled;

        // --- Ignore currents below 1A ---
        if(avg_ct1 < 1.0f) avg_ct1 = 0.0f;
        if(avg_ct2 < 1.0f) avg_ct2 = 0.0f;
        if(avg_ct3 < 1.0f) avg_ct3 = 0.0f;
     
        // --- Display readings ---
        printf("CT1:%.2fA CT2:%.2fA CT3:%.2fA\r\n", avg_ct1, avg_ct2, avg_ct3);

        float v1 = read_voltage_rms_debug(5, 3000, 0.002); // AN5
        float v2 = read_voltage_rms_debug(6, 3000, 0.002); // AN6
        float v3 = read_voltage_rms_debug(7, 3000, 0.002); // AN7

        printf("V1: %.2fV  V2: %.2fV  V3: %.2fV\r\n", v1, v2, v3);

        delay_ms(200);

        printf("LM35:%.2f,TC:%.2f\r\n", lm_temp, avg_tc_temp);

         // ---Check which GPB inputs are ON ---
        BYTE gpb_state = mcp23017_readB();
        BYTE input_state = gpb_state & 0x3F;   // keep GPB0..GPB5 only

        // Send compact format for app
        printf("IN:%02X\r\n", input_state);

        //debug prints like old code
        for (int i = 0; i <= 5; i++) {
            if (gpb_state & (1 << i)) {
                printf("GPB%d - Input %d ON\r\n", i, i + 1);
            }
        }
        delay_ms(200);  
    }
}

// Function to initialize MCP23017
void mcp23017_init() {
    // GPIOA = outputs
    i2c_start();
    i2c_write(MCP23017_ADDRESS);
    i2c_write(IODIRA);
    i2c_write(0x00); // Set all GPIOA as outputs
    i2c_stop();

    // GPIOB = inputs
    i2c_start();
    i2c_write(MCP23017_ADDRESS);
    i2c_write(IODIRB);
    i2c_write(0xFF); // Set all GPIOB as inputs
    i2c_stop();

    // Initialize GPIOA output latch to 0
    i2c_start();
    i2c_write(MCP23017_ADDRESS);
    i2c_write(OLATA);
    i2c_write(0x00);
    i2c_stop();
}
// Function to write data to GPIOA
void mcp23017_writeA(BYTE data) {
    i2c_start();
    i2c_write(MCP23017_ADDRESS);
    i2c_write(OLATA);
    i2c_write(data);
    i2c_stop();
}
//Function to readB from mcp23017
BYTE mcp23017_readB() {
    BYTE data;
    i2c_start();
    i2c_write(MCP23017_ADDRESS);
    i2c_write(GPIOB);
    i2c_start();
    i2c_write(MCP23017_ADDRESS | 1);
    data = i2c_read(0);
    i2c_stop();
    return data;
}
//Function to read rs232
void rs232_listener() {
    if (kbhit()) {
        char cmd = getc();
        comm_timeout = 0;  // Reset watchdog on any command

        // --- MCP23017 GPIOA toggle (0-7) ---
        if(cmd >= '0' && cmd <= '7') {
            BYTE pin = cmd - '0';
            gpioa_state ^= (1 << pin);
            mcp23017_writeA(gpioa_state);
            printf("GPIOA Pin %u toggled. Current State: 0x%02X\n", pin, gpioa_state);
        }
        // --- Toggle RD4 ---
        else if(cmd == '8') {
            rd4_state = !rd4_state;  // Toggle state
            if(rd4_state)
                output_high(PIN_D4);
            else
                output_low(PIN_D4);
            printf("RD4 toggled. Current state: %u\n", rd4_state);
        }
        // --- Toggle RB1 ---
        else if(cmd == '9') {
            rb1_state = !rb1_state;  // Toggle state
            if(rb1_state)
                output_high(PIN_B1);
            else
                output_low(PIN_B1);
            printf("RB1 toggled. Current state: %u\n", rb1_state);
        }
        // --- Turn off everything immediately ---
        else if(cmd == 'C') {
            rd4_state = 0;
            rb1_state = 0;
            output_low(PIN_D4);
            output_low(PIN_B1);
            gpioa_state = 0x00;
            mcp23017_writeA(gpioa_state);
            printf("All outputs OFF. GPIOA State: 0x%02X\n", gpioa_state);
        }
        // --- Heartbeat command ---
        else if(cmd == 'H') {
            printf("Heartbeat received\n");
        }
        else {
            printf("Invalid command.\n");
        }
    }
}
//Function to read lm35 temp
float read_lm35_temperature() {
    int16 adc_value;
    float temperature;

    set_adc_channel(LM35_PIN);
    delay_us(10);
    adc_value = read_adc();

    // Original uncalibrated formula
    temperature = (adc_value * 5.0 / 1023.0) * 100.0;

    // --- Calibration Section ---
    //Values based on real vs measured
    float scale = 0.2559;   // 17.5 / 68.4
    float offset = 10.3;     // Add offset if needed

    float calibrated_temp = (temperature * scale) + offset;
    return calibrated_temp;
}
// Initialize MAX31855 interface
void max31855_init() {
    output_high(CS_PIN);  // CS idle high
    output_low(SCK_PIN);  // Clock idle low (SPI mode 0)
    delay_ms(10);         // Allow chip to settle
}
// Function to read from MAX6675
float read_max31855_temperature() {
    unsigned int32 raw_data = 0;

    // SPI read 32 bits
    output_low(CS_PIN);
    delay_us(10);
    raw_data = 0;
    for (int i = 0; i < 32; i++) {
        raw_data <<= 1;
        if (input(MISO_PIN)) raw_data |= 1;
        output_high(SCK_PIN);
        delay_us(10);
        output_low(SCK_PIN);
        delay_us(10);
    }
    output_high(CS_PIN);

    // Fault detection
    if (raw_data & 0x00010000) { printf("Open circuit!\n"); return -1; }
    if (raw_data & 0x00000002) { printf("Short to GND!\n"); return -1; }
    if (raw_data & 0x00000001) { printf("Short to VCC!\n"); return -1; }

    // Thermocouple temperature (14-bit signed)
    int32 tc_raw = (raw_data >> 18) & 0x3FFF;
    if (tc_raw & 0x2000) tc_raw |= ~0x3FFF;
    float tc_temp = tc_raw * 0.25;

    // Cold-junction temperature (12-bit signed)
    int16 cj_raw = (raw_data >> 4) & 0x0FFF;
    if (cj_raw & 0x800) cj_raw |= 0xF000;
    float cj_temp = cj_raw * 0.0625;

    // Add CJ compensation
    tc_temp += cj_temp;

    // Apply user calibration
    tc_temp = (tc_temp * tc_slope) + tc_offset;

    return tc_temp;
}
//Function to read Current
float read_current_rms_debug(int channel, int settle_timeout_ms, float settle_threshold) {
    const int NUM_SAMPLES = 1000;         
    const float ADC_REF_VOLTAGE = 3.2;    
    const float ADC_MAX_VALUE   = 1023.0; 

    int16 adc_val;
    float voltage, sum = 0.0f, sum_sq = 0.0f;
    int adc_min = 1023, adc_max = 0;

    // --- Select ADC channel ---
    set_adc_channel(channel);
    delay_us(10);

    // --- Settling loop: wait for voltage to stabilize ---
    float last_voltage = 0.0f;
    int elapsed_ms = 0;
    while(elapsed_ms < settle_timeout_ms) {
        adc_val = read_adc();
        voltage = (adc_val * ADC_REF_VOLTAGE) / ADC_MAX_VALUE;
        if(fabs(voltage - last_voltage) < settle_threshold) break;
        last_voltage = voltage;
        delay_ms(10);
        elapsed_ms += 10;
    }

    // --- Collect RMS samples ---
    sum = 0.0f; sum_sq = 0.0f;
    adc_min = 1023; adc_max = 0;

    for(int i = 0; i < NUM_SAMPLES; i++) {
        adc_val = read_adc();
        if(adc_val < adc_min) adc_min = adc_val;
        if(adc_val > adc_max) adc_max = adc_val;

        voltage = (adc_val * ADC_REF_VOLTAGE) / ADC_MAX_VALUE;
        sum += voltage;
        sum_sq += voltage * voltage;

        delay_us(10);
    }

    // --- Compute DC bias ---
    float bias_voltage = sum / NUM_SAMPLES;

    // --- Compute AC RMS ---
    float mean_sq = sum_sq / NUM_SAMPLES;
    float variance = mean_sq - (bias_voltage * bias_voltage);
    if(variance < 0.0f) variance = 0.0f;
    float ac_rms_voltage = sqrt(variance);

    // --- Self-calibration: set zero bias if not yet initialized ---
    static int ct_initialized[4] = {0,0,0,0};
    if(!ct_initialized[channel]) {
        ct_zero_bias[channel] = ac_rms_voltage;
        ct_initialized[channel] = 1;
        printf("CH%d zero bias auto-calibrated: %.5f V\n", channel, ct_zero_bias[channel]);
    }

    // --- Subtract zero-current bias ---
    float ac_corrected = ac_rms_voltage - ct_zero_bias[channel];
    if(ac_corrected < 0.0f) ac_corrected = 0.0f;

    // --- Convert voltage to current ---
    float CT_RATING = 100.0f;   // CT max current
    float CT_VOLTAGE = 1.0f;    // Voltage at full current
    float gain = CT_RATING / CT_VOLTAGE;

    float current = ac_corrected * gain * ct_scale[channel];

    // --- Debug print ---
    printf("CH%d: Bias=%.5f V, RMSraw=%.5f V, Corrected=%.5f V -> Current=%.2f A, ADCmin=%u ADCmax=%u\r\n",
           channel, bias_voltage, ac_rms_voltage, ac_corrected, current, adc_min, adc_max);

    return current;
}
//Function to read Voltage
float read_voltage_rms_debug(int channel, int settle_timeout_ms, float settle_threshold) {
    const int NUM_SAMPLES = 1000;         
    const float ADC_REF_VOLTAGE = 3.3;    
    const float ADC_MAX_VALUE   = 1023.0; 

    int16 adc_val;
    float voltage, sum = 0.0f, sum_sq = 0.0f;
    int adc_min = 1023, adc_max = 0;
    rs232_listener();

    //Tiny delay to avoid overclocking ADC
    delay_us(10);
    set_adc_channel(channel);
    delay_us(10);
    
    // --- Settling loop ---
    float last_voltage = 0.0f;
    int elapsed_ms = 0;
    while (elapsed_ms < settle_timeout_ms) {
        adc_val = read_adc();
        voltage = (adc_val * ADC_REF_VOLTAGE) / ADC_MAX_VALUE;
        if (fabs(voltage - last_voltage) < settle_threshold) break;
        last_voltage = voltage;
        delay_ms(10);
        elapsed_ms += 10;
    }

    // --- Collect RMS samples ---
    for (int i = 0; i < NUM_SAMPLES; i++) {
        adc_val = read_adc();
        if (adc_val < adc_min) adc_min = adc_val;
        if (adc_val > adc_max) adc_max = adc_val;

        voltage = (adc_val * ADC_REF_VOLTAGE) / ADC_MAX_VALUE;
        sum += voltage;
        sum_sq += voltage * voltage;

        delay_us(10);
    }

    float bias_voltage = sum / (float)NUM_SAMPLES;
    float mean_sq = sum_sq / (float)NUM_SAMPLES;
    float var = mean_sq - (bias_voltage * bias_voltage);
    if (var < 0.0f) var = 0.0f;

    float ac_rms_voltage = sqrt(var);

    // --- Apply per-channel calibration scale ---
    float voltage_real = ac_rms_voltage * zmpt_scale[channel];

    // --- Noise filter: ignore tiny values ---
    if (voltage_real < 2.0f) voltage_real = 0.0f;

    printf("ZMPT%d Bias: %.4f V, RMSraw: %.4f V -> AC: %.2f Vrms, ADCmin:%u ADCmax:%u\r\n",
           channel, bias_voltage, ac_rms_voltage, voltage_real, adc_min, adc_max);

    return voltage_real;
}
//Function for emergency stop
void check_emergency_stop() {
    int button_pressed = !input(PIN_B0); // Active-low

    if(button_pressed && !emergency_active) {
        emergency_active = 1;
        gpioa_state = 0x00;
        mcp23017_writeA(gpioa_state);
        output_low(PIN_D4);
        output_low(PIN_B1);
        printf("!!! EMERGENCY STOP ACTIVATED !!! All outputs OFF\n");
    } 
    else if(!button_pressed && emergency_active) {
        emergency_active = 0;
        printf("Emergency released. Resuming normal operation.\n");
    }

    // If emergency is active, skip outputs but keep loop running
    if(emergency_active) {
        // Small delay to reduce RS232 spam
        delay_ms(10);
        return;  // Skip the rest of main loop actions
    }
}





