/*******************************************************************************
 TEST medi50  Dec 23
 //=========power_control.c====================
 ******************************************************************************/
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "main.h"
#include "power_control.h"
#include "nrf_delay.h"
#include "measurements.h"

#include "meas_pd_voltage_simple.h"

//#include "inv_imu_driver.h"
//#include "app_raw_main.h"
#include "mcp4725_i2c.h"
#include "ada2200_spi.h"
#include "drivers/w25q32/w25q32.h"
//#include "ad5272_i2c.h"
#include "nrf_log.h"
#include "app_timer.h"
#include "debug_print.h"
#include "cat_interface.h"
#include "i2c_manager.h"
//#define IR_POWER_HOLD	NRF_GPIO_PIN_MAP(0, 11)  /* Removed: P0.11 now W25Q32 power */
//#define RCV_POWER_HOLD	NRF_GPIO_PIN_MAP(1, 9)
//#define EEP_WP	NRF_GPIO_PIN_MAP(0, 24)  /* Disabled: P0.24 now used as Flash CS */
APP_TIMER_DEF(m_power_timer_id);
APP_TIMER_DEF(m_w25q_test_timer_id);

#define POWER_LOOP_INTERVAL 30
static uint8_t p_order;
static volatile bool power_up_done = false;
extern uint8_t simple_samples_in_buffer;
extern volatile bool processing;
bool lock_check = false;
void power_gpio_init(void)
{
	//nrf_gpio_cfg_output(IR_POWER_HOLD);  /* Removed: P0.11 now W25Q32 power */
	//nrf_gpio_cfg_output(RCV_POWER_HOLD);
	//nrf_gpio_cfg_output(EEP_WP);  /* Disabled: Flash CS */
}


void ir_power_control(on_off_cont_t ir_power_st)
{
	/* P0.11 = PWR_I/R_EN = U6 LDO EN (+3.3V_R for ADA2200)
	 * Shared with W25Q32 power (W25Q_PWR_PIN2) - both need HIGH */
	nrf_gpio_cfg_output(NRF_GPIO_PIN_MAP(0, 11));
	if (ir_power_st == OFF) {
		nrf_gpio_pin_clear(NRF_GPIO_PIN_MAP(0, 11));
	} else if (ir_power_st == ON) {
		nrf_gpio_pin_set(NRF_GPIO_PIN_MAP(0, 11));
	}
}


void eeprom_control(on_off_cont_t eeprom_st)
{
	/* Disabled: P0.24 now used as Flash CS, not EEPROM WP */
	(void)eeprom_st;
}

//void rcv_power_control(on_off_cont_t rcv_power_st)
//{
//	if(rcv_power_st == OFF) {
//	    nrf_gpio_pin_clear(RCV_POWER_HOLD);
//	}else if(rcv_power_st == ON){
//	    nrf_gpio_pin_set(RCV_POWER_HOLD);
//	}
//}


int device_sleep_mode(void){
	int rc = 0;
		eeprom_control(OFF);
	 mcp4725_PowerDownMode();

	nrf_delay_ms(2);
	ada2200_stop();
	nrf_delay_ms(2);
//	ada2200_uninit();

	nrf_delay_ms(2);

	nrf_delay_ms(2);
//	ad5272_i2c_uninit();

	nrf_delay_ms(2);
//	rcv_power_control(OFF);
	nrf_delay_ms(2);
	ir_power_control(OFF);
	nrf_delay_ms(2);
	DBG_PRINTF("Device_Sleep_Mode OK!\r\n");
	if(rc == 0) {
//		DBG_PRINTF("Device_Sleep_Mode OK!\r\n");
	} else {
		DBG_PRINTF("ERR!!! Device_Sleep_Mode Failed!\r\n");	
	}
	
	nrf_delay_ms(10);
	processing = false;
	return rc;
}


bool is_power_up_done(void) { return power_up_done; }

int device_activated(void){
	int rc = 0;
	p_order = 0;
	power_up_done = false;
	lock_check =true;
	power_timer_start();
	eeprom_control(OFF);
//	nrf_delay_ms(1);
//	ir_power_control(ON);
//	nrf_delay_ms(1);
//	rcv_power_control(ON);

//	//	nrf_delay_ms(5);
////	ada2200_start();
//	nrf_delay_ms(100);
//	ada2200_init();
//	nrf_delay_ms(50);
//	mcp4725_init();
//	nrf_delay_ms(50);
//	ad5272_i2c_init();
//	nrf_delay_ms(50);
//	mcp4725_powerOnWakeUp();
//	
//	nrf_delay_ms(10);
//	
//	
//	ad5272_normal_mode();
//	nrf_delay_ms(50);
//	ada2200_start();

 
	

//	if( rc == 0) {
//	DBG_PRINTF("Device_Activate_Mode OK!\r\n");
//	} else {
//		DBG_PRINTF("ERR!!! Device_Activated Failed!\r\n");
//	}
//	nrf_delay_ms(1000);

	return rc;
}




/**
 * @brief Power-up sequence state machine
 * 
 * Executes hardware initialization in 9 steps (case 0-8)
 * Called by app_timer at POWER_LOOP_INTERVAL (20ms)
 * 
 * Sequence:
 *   0: LED off, IR power on
 *   1: (reserved)
 *   2: I2C init
 *   3: (reserved)
 *   4: DAC wake-up
 *   5: LED-PD modulation set
 *   6: ADA2200 lock-in amp start
 *   7: PD channel select
 *   8: Complete
 */
void power_loop(void *p_context)
{
    UNUSED_PARAMETER(p_context);
    
    power_timer_stop();
    
    switch (p_order) {
        /* Step 0: Power On */
        case 0:
            LED_ALLOFF();
            ir_power_control(ON);
            // rcv_power_control(ON);
            nrf_delay_ms(20);
            DBG_PRINTF("[PWR] Step %d: IR Power ON\r\n", p_order);
            break;
        
        /* Step 1: Reserved */
        case 1:
            // ir_power_control(OFF);
            // rcv_power_control(OFF);
            // DBG_PRINTF("POWER_OFF\r\n");
            // ada2200_init();
            DBG_PRINTF("[PWR] Step %d: (reserved)\r\n", p_order);
            break;
        
        /* Step 2: I2C Initialize */
        case 2:
            sw_i2c_init_once();
            nrf_delay_ms(10);
            // ad5272_i2c_init();
            // ir_power_control(ON);
            // rcv_power_control(ON);
            // DBG_PRINTF("POWER_ON\r\n");
            DBG_PRINTF("[PWR] Step %d: I2C Init\r\n", p_order);
            break;
        
        /* Step 3: Reserved */
        case 3:
            DBG_PRINTF("[PWR] Step %d: (reserved)\r\n", p_order);
            break;
        
        /* Step 4: DAC Wake-up */
        case 4:
            nrf_delay_ms(10);
            mcp4725_powerOnWakeUp();
            nrf_delay_ms(20);
            DBG_PRINTF("[PWR] Step %d: MCP4725 Wake-up\r\n", p_order);
            break;
        
        /* Step 5: LED-PD Modulation Set */
        case 5:
            led_pd_mod_set(1000);
            nrf_delay_ms(10);
            led_pd_mod_set(1000);  // Apply twice for stability
            nrf_delay_ms(10);
            DBG_PRINTF("[PWR] Step %d: LED-PD Mod Set (1000)\r\n", p_order);
            break;
        
        /* Step 6: ADA2200 Lock-in Amplifier Start */
        case 6:
            ada2200_start();  // SPI
            nrf_delay_ms(100);
            DBG_PRINTF("[PWR] Step %d: ADA2200 Start\r\n", p_order);
            break;
        
        /* Step 7: PD Channel Select */
        case 7:
            if (NRF_SUCCESS == pd_on(2)) {
                DBG_PRINTF("[PWR] Step %d: PD Channel ON\r\n", p_order);
                // simple_mesurement_start();
            }
            break;
        
        /* Step 8: Complete */
        case 8:
            // ada2200_start();
            DBG_PRINTF("[PWR] Step %d: Sequence Complete\r\n", p_order);
            break;
        
        default:
            break;
    }
    
    /* Advance to next step or finish */
    if (p_order < 8) {
        p_order++;
        power_timer_start();
    } else {
        power_up_done = true;
        DBG_PRINTF("[PWR] Device Activated OK!\r\n");
    }
}







int device_reactivated(void){
	
	int rc = 0;
  sw_i2c_init_once();
  mcp4725_PowerDownMode();
  DBG_PRINTF("LOCKIN!!\r\n");
	nrf_delay_ms(10);
	ada2200_stop();

	nrf_delay_ms(10);

	ir_power_control(OFF);
	nrf_delay_ms(10);
	lock_check = true;
	p_order = 0;
	power_timer_start();
	return rc;
}

void power_timer_start(void)
{
	APP_ERROR_CHECK(app_timer_start(m_power_timer_id, APP_TIMER_TICKS(POWER_LOOP_INTERVAL), NULL));
}


void power_timer_stop(void)
{
	APP_ERROR_CHECK(app_timer_stop(m_power_timer_id));

}


void power_timer_init(void)  //active start
{
    APP_ERROR_CHECK(app_timer_create(&m_power_timer_id, APP_TIMER_MODE_SINGLE_SHOT, power_loop));
}

/**
 * @brief W25Q32 test timer callback
 */
static void w25q_test_callback(void *p_context)
{
    UNUSED_PARAMETER(p_context);

    DBG_PRINTF("\r\n========== W25Q32 TEST ==========\r\n");

    if (w25q32_init()) {
        DBG_PRINTF("[W25Q] Init OK\r\n");
        if (w25q32_check_id()) {
            DBG_PRINTF("[W25Q] ID OK\r\n");
        } else {
            DBG_PRINTF("[W25Q] ID FAIL\r\n");
        }
    } else {
        DBG_PRINTF("[W25Q] Init FAIL\r\n");
    }

    DBG_PRINTF("=================================\r\n");
}

void w25q_test_timer_init(void)
{
    APP_ERROR_CHECK(app_timer_create(&m_w25q_test_timer_id, APP_TIMER_MODE_SINGLE_SHOT, w25q_test_callback));
}

void w25q_test_timer_start(void)
{
    app_timer_start(m_w25q_test_timer_id, APP_TIMER_TICKS(3000), NULL);
}