#include "main.h"

static esp_err_t mqtt_event_handler(esp_mqtt_event_handle_t event)
{
    esp_mqtt_client_handle_t client = event->client;
    int msg_id;
    // your_context_t *context = event->context;
    switch (event->event_id) {
        case MQTT_EVENT_CONNECTED:
            ESP_LOGI(TAG, "MQTT_EVENT_CONNECTED");
            msg_id = esp_mqtt_client_subscribe(client, "todev/flags", 2);
            ESP_LOGI(TAG, "sent subscribe successful, msg_id=%d", msg_id);
            msg_id = esp_mqtt_client_subscribe(client, "todev/ref", 2);
            ESP_LOGI(TAG, "sent subscribe successful, msg_id=%d", msg_id);
            msg_id = esp_mqtt_client_subscribe(client, "toPC", 2);
            ESP_LOGI(TAG, "sent subscribe successful, msg_id=%d", msg_id);


            break;
        case MQTT_EVENT_DISCONNECTED:
            ESP_LOGI(TAG, "MQTT_EVENT_DISCONNECTED");
            break;

        case MQTT_EVENT_SUBSCRIBED:
            ESP_LOGI(TAG, "MQTT_EVENT_SUBSCRIBED, msg_id=%d", event->msg_id);
            //msg_id = esp_mqtt_client_publish(client, "sensor", "data", 0, 0, 0);
            msg_id = esp_mqtt_client_publish(client, "sensor/toPC", "5000", 5, 2, 0);
            ESP_LOGI(TAG, "sent publish successful, msg_id=%d", msg_id);
            break;
        case MQTT_EVENT_UNSUBSCRIBED:
            ESP_LOGI(TAG, "MQTT_EVENT_UNSUBSCRIBED, msg_id=%d", event->msg_id);
            break;
        case MQTT_EVENT_PUBLISHED:
            ESP_LOGI(TAG, "MQTT_EVENT_PUBLISHED, msg_id=%d", event->msg_id);
            break;
        case MQTT_EVENT_DATA:
            ESP_LOGI(TAG, "MQTT_EVENT_DATA");
            printf("TOPIC=%.*s\r\n", event->topic_len, event->topic);
            printf("DATA=%.*s\r\n", event->data_len, event->data);
            strcpy(str_topic, event->topic);
            strcpy(str_data, event->data);

           /* if(!strncmp(str_topic, kp_topic, sizeof(kp_topic))){
                kp = (float)((str_data[0] - '0')*10.0 + (str_data[0] - '0'))/10.0;
            }*/
                
            /*if(!strncmp(str_topic, ki_topic, sizeof(ki_topic)))
                ESP_LOGI(TAG, "KI received %s", event->data);
            if(!strncmp(str_topic, kd_topic, sizeof(kd_topic)))
                ESP_LOGI(TAG, "KD received %s", event->data);
            if(!strncmp(str_topic, bc_topic, sizeof(bc_topic)))
                ESP_LOGI(TAG, "BC received %s", event->data);*/

            if(!strncmp(str_topic, flag_topic, sizeof(flag_topic))){
                ESP_LOGI(TAG, "Flag received");
                if(str_data[0]- 48 == 0)
                    if(xSemaphoreTake(binary_flag_semaphore, 200)){
                        device_power_on = false;
                        SetOffLED(LED_1);
                        xSemaphoreGive(binary_flag_semaphore);
                    } 
                if(str_data[0]- 48 == 1)
                    if(xSemaphoreTake(binary_flag_semaphore, 200)){
                        device_power_on = true;
                        SetOnLED(LED_1);
                        xSemaphoreGive(binary_flag_semaphore);
                    } 
                if(str_data[0]-48 == 3)
                    SetOffLED(LED_2);
                if(str_data[0]-48 == 2)
                    SetOnLED(LED_2);
                if(str_data[0]-48 == 5)
                    if(xSemaphoreTake(binary_flag_semaphore, 200)){
                        control_algorithm_on = false;
                        SetOffLED(LED_4);
                        xSemaphoreGive(binary_flag_semaphore);
                    } 
                if(str_data[0]-48 == 4)
                    if(xSemaphoreTake(binary_flag_semaphore, 200)){
                        control_algorithm_on = true;
                        SetOnLED(LED_4);
                        xSemaphoreGive(binary_flag_semaphore);
                    }               
                if(str_data[0] - 48 == 6)
                    if(xSemaphoreTake(control_semaphore, 200)){
                        kp = (float)((str_data[1] - 48)*10.0 + (str_data[2] - 48))/10.0;
                        xSemaphoreGive(control_semaphore);
                    }
                        
                if(str_data[0] - 48 == 7)
                    if(xSemaphoreTake(control_semaphore, 200)){
                        ki = (float)((str_data[1] - 48)*10.0 + (str_data[2] - 48))/10.0;
                        xSemaphoreGive(control_semaphore);
                    }
                if(str_data[0] - 48 == 8)
                    if(xSemaphoreTake(control_semaphore, 200)){
                        kd = (float)((str_data[1] - 48)*10.0 + (str_data[2] - 48))/100000.0;
                        xSemaphoreGive(control_semaphore);
                    }
                if(str_data[0] - 48 == 9)
                    if(xSemaphoreTake(control_semaphore, 200)){
                        bc_const = (float)((str_data[1] - 48)*10.0 + (str_data[2] - 48))/1000.0;
                        xSemaphoreGive(control_semaphore);
                    }
                
            }
            if(!strncmp(str_topic, ref_topic, sizeof(ref_topic))){
                ESP_LOGI(TAG, "Reference received");
                string2ref(str_data,event->data_len, &reference);
    
            }
                
            break;
        case MQTT_EVENT_ERROR:
            ESP_LOGI(TAG, "MQTT_EVENT_ERROR");
            break;
        default:
            ESP_LOGI(TAG, "Other event id:%d", event->event_id);
            break;
    }
    return ESP_OK;
}

static esp_err_t wifi_event_handler(void *ctx, system_event_t *event)
{
    switch (event->event_id) {
        case SYSTEM_EVENT_STA_START:
            esp_wifi_connect();
            break;
        case SYSTEM_EVENT_STA_GOT_IP:
            xEventGroupSetBits(wifi_event_group, CONNECTED_BIT);

            break;
        case SYSTEM_EVENT_STA_DISCONNECTED:
            esp_wifi_connect();
            xEventGroupClearBits(wifi_event_group, CONNECTED_BIT);
            break;
        default:
            break;
    }
    return ESP_OK;
}

static void wifi_init(void)
{
    tcpip_adapter_init();
    wifi_event_group = xEventGroupCreate();
    ESP_ERROR_CHECK(esp_event_loop_init(wifi_event_handler, NULL));
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
    ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM));
    wifi_config_t wifi_config = {
        .sta = {
            .ssid = CONFIG_WIFI_SSID,
            .password = CONFIG_WIFI_PASSWORD,
        },
    };
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
    ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_STA, &wifi_config));
    ESP_LOGI(TAG, "start the WIFI SSID:[%s]", CONFIG_WIFI_SSID);
    ESP_ERROR_CHECK(esp_wifi_start());
    ESP_LOGI(TAG, "Waiting for wifi");
    xEventGroupWaitBits(wifi_event_group, CONNECTED_BIT, false, true, portMAX_DELAY);
}

static void mqtt_app_start(void)
{
    esp_mqtt_client_config_t mqtt_cfg = {
        .uri = CONFIG_BROKER_URL,
        .event_handle = mqtt_event_handler,
        // .user_context = (void *)your_context
    };

    client = esp_mqtt_client_init(&mqtt_cfg);
    esp_mqtt_client_start(client);
}

static void string2ref(char *string, int len, float *duty){
    float temp_val_float = 0;
    for(int i = 0; i < len; ++i)
        temp_val_float = temp_val_float*10 + (string[i] - '0'); 
    temp_val_float = 2047.0*(temp_val_float - 5000.0)/5000.0;
    *duty = temp_val_float;
}

float ay, az, gx;
float roll;
float froll;
int count = 0;
uint32_t lasttime = 0;

void timer_example_evt_task(void *arg)
{

    bool local_power_flag = false;
    bool local_control_flag = false;
    float local_kp, local_ki, local_kd, local_bc;

    while (1) {

        timer_event_t evt;
        xQueueReceive(timer_queue, &evt, portMAX_DELAY);
        // Timer ISR routine here
        // evt.timer_counter_value -- sampling time
        if(xSemaphoreTake(binary_flag_semaphore, 10)){
            local_power_flag = device_power_on;
            xSemaphoreGive(binary_flag_semaphore);
        }
        if(xSemaphoreTake(control_semaphore, 100)){
            local_control_flag = binary_flag_semaphore;
            local_kp = kp;
            local_ki = ki;
            local_kd = kd;
            local_bc = bc_const;
            xSemaphoreGive(control_semaphore);
        }
        if(local_power_flag){
            float Ts = (float)evt.timer_counter_value/TIMER_SCALE;
            ay = getAccY();
            az = getAccZ();
            gx = getGyroX();
            roll = atan2f(ay,az)*57.2958;
            froll = filter(roll, -gx);
            count++;

            if(local_control_flag){

            //PID control

                error[1] = error[0];
                error[0] = reference - froll;
                integral[0] = integral[1] + error[0]*Ts;
                control_signal = (local_kp+local_kd/Ts)*error[0] - local_kd/Ts*error[1] + local_ki*integral[0];

                //Saturation
                float control_out = control_signal;
                if(control_signal > 2047.0)
                    control_out = 2047.0;
                if(control_signal < -2047.0)
                    control_out = -2047.0;

                //Back calculation

                float sat_error = control_out - control_signal;

                integral[1] = integral[0] + sat_error * local_bc;
                //integral[1] = integral[0];

                //Control output on pwm

                if(control_out >= 0)
                    SetDuty(0, (int)control_out);
                else
                    SetDuty(-(int)control_out, 0);
            
            }


            if(count == 100) {
                count = 0;    
                int temp_int = (int)(((froll + 90.0)/180.0)*10000.0);
                if(temp_int < 0) temp_int = 0;
                if(temp_int > 10000) temp_int = 10000;
                char temp_s[5];
                
                temp_s[0] = temp_int/10000 + '0';
                temp_s[1] = (temp_int/1000)%10 + '0';
                temp_s[2] = (temp_int/100)%10 + '0';
                temp_s[3] = (temp_int/10)%10 + '0';
                temp_s[4] = (temp_int)%10 + '0';
                
                esp_mqtt_client_publish(client, "sensor/toPC", temp_s, 5, 2, 0);

                //ESP_LOGI("mpu6050", "String token: %s", temp_s);
            }
         }
       
    }
}

void app_main()
{
    ESP_LOGI(TAG, "[APP] Startup..");
    ESP_LOGI(TAG, "[APP] Free memory: %d bytes", esp_get_free_heap_size());
    ESP_LOGI(TAG, "[APP] IDF version: %s", esp_get_idf_version());

    esp_log_level_set("*", ESP_LOG_INFO);
    esp_log_level_set("MQTT_CLIENT", ESP_LOG_VERBOSE);
    esp_log_level_set("TRANSPORT_TCP", ESP_LOG_VERBOSE);
    esp_log_level_set("TRANSPORT_SSL", ESP_LOG_VERBOSE);
    esp_log_level_set("TRANSPORT", ESP_LOG_VERBOSE);
    esp_log_level_set("OUTBOX", ESP_LOG_VERBOSE);

    nvs_flash_init();

    control_semaphore = xSemaphoreCreateMutex();
    binary_flag_semaphore = xSemaphoreCreateMutex();

    setUpGPIO();
    SetupPWM(PWM_FREQ, PWM_RES, PWM_PIN_1, PWM_PIN_2);
    wifi_init();
    timer_queue = xQueueCreate(10, sizeof(timer_event_t));
    example_tg0_timer_init(TIMER_1, TEST_WITH_RELOAD, TIMER_INTERVAL_SEC);
    if(!setupMPU6050(GPIO_NUM_19, GPIO_NUM_22, I2C_NUM_0)){
        ESP_LOGE("mpu6050", "init failed!");
        return;
    }
    initKF(0.005);
    mqtt_app_start();
    xTaskCreate(timer_example_evt_task, "timer_evt_task", 2048, NULL, 5, NULL);
}