/*
 * SPDX-FileCopyrightText: 2010-2022 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: CC0-1.0
 */

#include <stdio.h>
#include <inttypes.h>
#include "sdkconfig.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "driver/gpio.h"
#include "driver/ledc.h"
#include "time.h"

#include "mcp2521.hpp"
#include "mcp2521_hardware_handle.hpp"

#define MCP2521_PIN_INT  GPIO_NUM_5
#define MCP2521_PIN_SCLK  GPIO_NUM_18
#define MCP2521_PIN_MISO GPIO_NUM_26
#define MCP2521_PIN_MOSI GPIO_NUM_23
#define MCP2521_PIN_CS   GPIO_NUM_25

#define SERVO_0_PIN GPIO_NUM_2
#define SERVO_1_PIN GPIO_NUM_4
#define SERVO_2_PIN GPIO_NUM_19
#define SERVO_3_PIN GPIO_NUM_21

#define LED_PIN GPIO_NUM_22

#define DETECTOR_PIN GPIO_NUM_32
#define Z_AXIS_DETECT_PIN GPIO_NUM_35
#define BUTTON_PIN GPIO_NUM_33

#define MOTOR_PERIOD_US 20000
#define MOTOR_MIN_PULSE_US 500
#define MOTOR_MAX_PULSE_US 2500 

#define MOTOR_MG996R_US 20000
#define MOTOR_MG996R_MIN_PULSE_US 500
#define MOTOR_MG996R_MAX_PULSE_US 2500 

#define PWM_RESOLUTION 18

#define PWM_MAX_VALUE (1 << PWM_RESOLUTION)
#define MOTOR_MAX_VALUE (MOTOR_MAX_PULSE_US * PWM_MAX_VALUE / MOTOR_PERIOD_US)
#define MOTOR_MIN_VALUE (MOTOR_MIN_PULSE_US * PWM_MAX_VALUE / MOTOR_PERIOD_US)
#define MOTOR_CENTER_VALUE ((MOTOR_MAX_VALUE + MOTOR_MIN_VALUE) / 2)

#define MOTOR_MG996R_MAX_VALUE (MOTOR_MG996R_MAX_PULSE_US * PWM_MAX_VALUE / MOTOR_MG996R_US)
#define MOTOR_MG996R_MIN_VALUE (MOTOR_MG996R_MIN_PULSE_US * PWM_MAX_VALUE / MOTOR_MG996R_US)
#define MOTOR_MG996R_CENTER_VALUE ((MOTOR_MG996R_MAX_VALUE + MOTOR_MG996R_MIN_VALUE) / 2)

#define MOTOR_RANGE (MOTOR_MAX_VALUE - MOTOR_MIN_VALUE)
#define MOTOR_HALF_RANGE (MOTOR_RANGE / 2)

#define MOTOR_MG996R_RANGE (MOTOR_MG996R_MAX_VALUE - MOTOR_MG996R_MIN_VALUE)
#define MOTOR_MG996R_HALF_RANGE (MOTOR_MG996R_RANGE / 2)

#define CHOPPER_0_CLOSED (MOTOR_CENTER_VALUE - 1000)
#define CHOPPER_1_CLOSED (MOTOR_CENTER_VALUE + 900)
#define CHOPPER_0_OPEN (MOTOR_CENTER_VALUE - MOTOR_HALF_RANGE*0.75)
#define CHOPPER_1_OPEN (MOTOR_CENTER_VALUE + MOTOR_HALF_RANGE*0.75)

#define HEAD_MOVE_UP_TIME 250
#define HEAD_MOVE_UP   (MOTOR_MG996R_CENTER_VALUE + MOTOR_MG996R_HALF_RANGE*0.5)
#define HEAD_MOVE_STOP (MOTOR_MG996R_CENTER_VALUE)
#define HEAD_MOVE_DOWN_TIME 750
#define HEAD_MOVE_DOWN (MOTOR_MG996R_CENTER_VALUE - MOTOR_MG996R_HALF_RANGE*0.5)

// Z-Axis Motor
bool motor_stop_up_flag = false;
SemaphoreHandle_t motor_stop_down_semaphore;
SemaphoreHandle_t motor_stop_up_semaphore;

static void IRAM_ATTR isr_motor_stop_up(void *args) {
    if(motor_stop_up_flag) {
        motor_stop_up_flag = false;
        xSemaphoreGiveFromISR(motor_stop_up_semaphore, NULL);
    }
}

static void task_fn_motor_stop_up(void *args) {
    while(true) {
        xSemaphoreTake(motor_stop_up_semaphore, portMAX_DELAY);
        vTaskDelay(HEAD_MOVE_UP_TIME / portTICK_PERIOD_MS);
        ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_STOP);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
    }
}

static void task_fn_motor_stop_down(void *args) {
    while(true) {
        xSemaphoreTake(motor_stop_down_semaphore, portMAX_DELAY);
        vTaskDelay(HEAD_MOVE_DOWN_TIME / portTICK_PERIOD_MS);
        ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_STOP);
        ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
    }
}

void lower_head() {
    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_DOWN);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
    xSemaphoreGive(motor_stop_down_semaphore);
    vTaskDelay(500 / portTICK_PERIOD_MS);
}

void raise_head() {
    if(!gpio_get_level(Z_AXIS_DETECT_PIN)) {
        return;
    }

    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2, HEAD_MOVE_UP);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_2);
    motor_stop_up_flag = true;
}

// Dispencing mechanism
void set_copper(bool bottom, bool top) {
    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1, bottom ? CHOPPER_1_OPEN : CHOPPER_1_CLOSED);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, top ? CHOPPER_0_OPEN : CHOPPER_0_CLOSED);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
}

enum DISPENSER_RESULT {
    SUCESS = 0,
    EMPTY_WARINING = 1,
    EMPTY = 2,
};

bool load_stone() {
    if(!gpio_get_level(DETECTOR_PIN)) {
        // No stone loaded
        return true;
    }
    
    set_copper(true, false);
    vTaskDelay(250 / portTICK_PERIOD_MS);
    set_copper(false, false);
    
    if(!gpio_get_level(DETECTOR_PIN)) {
        return true;
    } else {
        return false;
    }
}

enum DISPENSER_RESULT dispens_stone() {
    
    if(!gpio_get_level(DETECTOR_PIN)) {
        // No stone loaded

        set_copper(false, true);
        vTaskDelay(250 / portTICK_PERIOD_MS);
        set_copper(false, false);
        vTaskDelay(100 / portTICK_PERIOD_MS);
    
        if(!gpio_get_level(DETECTOR_PIN)) {
             // No stone loaded again
            return EMPTY; 
        }

    }
    lower_head();
    set_copper(true, false);
    vTaskDelay(250 / portTICK_PERIOD_MS);
    set_copper(false, false);
    vTaskDelay(100 / portTICK_PERIOD_MS);
    set_copper(false, true);
    vTaskDelay(250 / portTICK_PERIOD_MS);
    set_copper(false, false);
    raise_head();
    
    if(!gpio_get_level(DETECTOR_PIN)) {
        return SUCESS;
    } else {
        return EMPTY_WARINING;
    }
}

extern "C" void app_main(void)
{
    printf("Hello world!\n");

    gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);
    gpio_set_direction(DETECTOR_PIN, GPIO_MODE_INPUT);
    gpio_set_direction(Z_AXIS_DETECT_PIN, GPIO_MODE_INPUT);
    gpio_set_direction(BUTTON_PIN, GPIO_MODE_INPUT);

    motor_stop_up_semaphore = xSemaphoreCreateBinary();
    motor_stop_down_semaphore = xSemaphoreCreateBinary();
    xTaskCreatePinnedToCore(task_fn_motor_stop_up, "motor_stop_up", 2048, NULL, 4, NULL, 0);
    xTaskCreatePinnedToCore(task_fn_motor_stop_down, "motor_stop_down", 2048, NULL, 4, NULL, 0);

    gpio_set_intr_type(Z_AXIS_DETECT_PIN, GPIO_INTR_ANYEDGE);
    gpio_install_isr_service(0);
    gpio_isr_handler_add(Z_AXIS_DETECT_PIN, isr_motor_stop_up, NULL);

    spi_bus_config_t bus_config;
    MCP2521_HardwareHandle_ESP MCP2521_HardwareHandle_ESP(
        VSPI_HOST,
        &bus_config,
        MCP2521_PIN_MOSI,
        MCP2521_PIN_MISO,
        MCP2521_PIN_SCLK,
        MCP2521_PIN_CS,
        MCP2521_PIN_INT
    );

    if(0) {
        gpio_set_direction(MCP2521_PIN_SCLK, GPIO_MODE_OUTPUT);
        gpio_set_direction(MCP2521_PIN_MISO, GPIO_MODE_OUTPUT);
        gpio_set_direction(MCP2521_PIN_MOSI, GPIO_MODE_OUTPUT);
        gpio_set_direction(MCP2521_PIN_CS, GPIO_MODE_OUTPUT);
        gpio_set_level(MCP2521_PIN_SCLK, 1);
        gpio_set_level(MCP2521_PIN_MISO, 1);
        gpio_set_level(MCP2521_PIN_MOSI, 1);
        gpio_set_level(MCP2521_PIN_CS, 1);
        vTaskDelay(50 / portTICK_PERIOD_MS);
        gpio_set_level(MCP2521_PIN_SCLK, 0);
        vTaskDelay(50 / portTICK_PERIOD_MS);
        gpio_set_level(MCP2521_PIN_MOSI, 0);
        vTaskDelay(50 / portTICK_PERIOD_MS);
        gpio_set_level(MCP2521_PIN_MISO, 0);
        vTaskDelay(50 / portTICK_PERIOD_MS);
        gpio_set_level(MCP2521_PIN_CS, 0); // Works
    }

    MCP2521 mcp2521(&MCP2521_HardwareHandle_ESP);
    mcp2521.reset();
    mcp2521.set_mode_of_operation(MCP2521_OPERATION_MODE::NORMAL, true);
    printf("CANSTAT: %x\n", mcp2521.read_reg(MCP2521_CANCTRL));
    printf("MCP2521 initialized\n");

    ledc_timer_config_t ledc_timer = {
        .speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
        .duty_resolution = LEDC_TIMER_18_BIT,
        .timer_num = LEDC_TIMER_0,
        .freq_hz = 50,
        .clk_cfg = LEDC_AUTO_CLK
    };

    ledc_channel_config_t servo0_channel_config = {
        .gpio_num = SERVO_0_PIN,
        .speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
        .channel = LEDC_CHANNEL_0,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,
        .hpoint = 0,
        .flags = {
            .output_invert = 1
        }
    };

    ledc_channel_config_t servo1_channel_config = {
        .gpio_num = SERVO_1_PIN,
        .speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
        .channel = LEDC_CHANNEL_1,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,
        .hpoint = 0,
        .flags = {
            .output_invert = 1
        }
    };

    ledc_channel_config_t servo2_channel_config = {
        .gpio_num = SERVO_2_PIN,
        .speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
        .channel = LEDC_CHANNEL_2,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,
        .hpoint = 0,
        .flags = {
            .output_invert = 1
        }
    };
    
    ledc_channel_config_t servo3_channel_config = {
        .gpio_num = SERVO_3_PIN,
        .speed_mode = ledc_mode_t::LEDC_LOW_SPEED_MODE,
        .channel = LEDC_CHANNEL_3,
        .intr_type = LEDC_INTR_DISABLE,
        .timer_sel = LEDC_TIMER_0,
        .duty = 0,
        .hpoint = 0,
        .flags = {
            .output_invert = 1
        }
    };

    ledc_timer_config(&ledc_timer);
    ledc_channel_config(&servo0_channel_config);
    ledc_channel_config(&servo1_channel_config);
    ledc_channel_config(&servo2_channel_config);
    ledc_channel_config(&servo3_channel_config);

    raise_head();
    set_copper(false, false);

    while (true) {
        while (!gpio_get_level(BUTTON_PIN)) {
            vTaskDelay(50 / portTICK_PERIOD_MS);
        }    
        printf("Sucess: %d\n", dispens_stone());
    }
    

}