#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"

#include "pile_id.h"
#include "config.h"
#include "modbusS.h"
#include "flow.h"
#include "depth.h"
#include "../communication_pad/inc/communication_pad.h"
#include "ads1220.h"

#define TAG "pile_id"

uint16_t *cur_pile_id = &gWordVar[LAST_PILE_ID_ADDR];
gps_message_t *gps_message = (gps_message_t *)&gWordVar[REG_GPS_MESSAGE];
double gps_xy[2];
int none_flag = 1;

extern move_t *pMoveCtx;
extern int depth_over_inc_pile_flag;

static double double_abs(double a, double b)
{
    if (a > b) return a - b;
    else return b - a;
} 

int gps_distance_check(void)
{
    if (gps_message->gps_status != 4)   // gps信号不是精准定位则不启用   
    {
        none_flag = 1;
        return 0;
    }
    if (none_flag) return 0;
    if (double_abs(gps_message->x, gps_xy[0]) > GPS_DISTANCE_TH) return 1; 
    if (double_abs(gps_message->y, gps_xy[1]) > GPS_DISTANCE_TH) return 1; 
    return 0;
}

void inc_pile_fun(void)
{
    ESP_LOGI(TAG, "inc pile");
    // 增加桩号
    (*cur_pile_id)++;
    save_pile_id();
    // 更新判断的标志数据
    depth_over_inc_pile_flag = 0;
    pMoveCtx->work_time = 0;
    pMoveCtx->work_stop_state_flag = 0;
    gps_xy[0] = gps_message->x;
    gps_xy[1] = gps_message->y;
    none_flag = 0;
    // 清空数据
    zero_flow1();
    zero_flow2();
    zero_depth();
}

// 轮询检查是否换桩
void pile_id_check_task(void)
{
    static uint16_t last_pile_work_state = PILE_STATE_STOP;
    while (1)
    {
        // 自动更新工作状态
        if (last_pile_work_state == PILE_STATE_STOP)    // 判断是否开始
        {
            if (gWordVar[AC_CURRENT_REG_ADDR + 0] > CURRENT0_TH && depth_over_inc_pile_flag == 1)   // 钻头电流开 && 深度超过阈值
            {
                depth_data->pile_work_state_and_direction = (depth_data->pile_work_state_and_direction & 0XFF) | PILE_STATE_WORK;
            }
        }

        if (last_pile_work_state == PILE_STATE_WORK)    // 判断是否结束
        {
            if (pMoveCtx->work_stop_state_flag == 1 || gps_distance_check()) 
            {
                depth_data->pile_work_state_and_direction = (depth_data->pile_work_state_and_direction & 0XFF) | PILE_STATE_STOP;
            }
        }

        
        // 开始则换桩
        if ((depth_data->pile_work_state_and_direction & 0xff00) == PILE_STATE_WORK && last_pile_work_state == PILE_STATE_STOP)
        {
            inc_pile_fun();
        }
        last_pile_work_state = depth_data->pile_work_state_and_direction & 0xff00;

        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

void pile_id_init(void)
{
    xTaskCreate(pile_id_check_task, "ads1220_task", 4096, NULL, 10, NULL);
}