#include "Arduino.h"
#include "dshot.h"

dshot::dshot(void) {
}

/***
(>>4)  = 000010000010 # right shift value by 4
(^)    = 100010101110 # XOR with value
(>>8)  = 000000001000 # right shift value by 8
(^)    = 100010100110 # XOR with previous XOR 
(0x0F) = 000000001111 # Mask 0x0F
(&)    = 000000000110 # CRC 
 ***/
uint8_t dshot::crc(uint16_t val) {
    uint16_t a = val ^ (val >> 4);
    return (a ^ (a >> 8)) & 0x0F;
}

void dshot::send_cmd(uint8_t pin, uint16_t speed, bool telemetry) {
    // Over limit protection
    if(speed > 2047)
        speed = 2047;

    // Speed left shift by 1 and add the telemetry bit
    uint16_t val = (speed << 1) | telemetry;

    // Preparing packets
    if(!telemetry) {
        uint16_t packet = (val << 4) | crc(val);
        // Serial.print(F("PACKET: "));
        // Serial.println(packet, BIN);
        uint8_t bits = 16, duration_on, duration_off;
        uint32_t time_on_start, time_off_start, time_on_end, time_off_end;
        while(bits != 0) {
            bits--;
            /***
            if((packet >> bits) & 0x0001) { // HIGH PULSE
                duration_on  = DSHOT_CLOCK_CYCLE_T1H;
                duration_off = DSHOT_CLOCK_CYCLE_T1L;
            } else {                        // LOW PULSE
                duration_on  = DSHOT_CLOCK_CYCLE_T0H;
                duration_off = DSHOT_CLOCK_CYCLE_T0L;
            }

            GPIO.out_w1ts = ((uint32_t)1 << pin);
            time_on_start = ESP.getCycleCount();

            // Wait till the T1H time reached
            while(time_on_end < time_on_start + duration_on) {
                time_on_end = ESP.getCycleCount();
            }
                
            // Setting pin low for T1L
            GPIO.out_w1tc = ((uint32_t)1 << pin);
            time_off_start = ESP.getCycleCount();

            // Wait till the T1L time reached
            while(time_off_end < time_off_start + duration_off) {
                time_off_end = ESP.getCycleCount();
            }
             ***/
            if((packet >> bits) & 0x0001) { // HIGH PULSE
                //                                           __
                // If high then generate high signal pulse _|  |_|
                // Setting pin high for T1H
                GPIO.out_w1ts = ((uint32_t)1 << pin);
                time_on_start = ESP.getCycleCount();

                // Wait till the T1H time reached
                while(ESP.getCycleCount() < time_on_start + DSHOT_CLOCK_CYCLE_T1H) {
                }
                
                // Setting pin low for T1L
                GPIO.out_w1tc = ((uint32_t)1 << pin);
                time_off_start = ESP.getCycleCount();

                // Wait till the T1L time reached
                while(ESP.getCycleCount() < time_off_start + DSHOT_CLOCK_CYCLE_T1L) {
                }
                // delayMicroseconds(1);
            } else {                        // LOW PULSE
                //                                          _
                // If low then generate high signal pulse _| |__|
                GPIO.out_w1ts = ((uint32_t)1 << pin);
                time_on_start = ESP.getCycleCount();

                // Wait till the T1H time reached
                while(ESP.getCycleCount() < time_on_start + DSHOT_CLOCK_CYCLE_T0H) {
                }

                // Setting pin low for T1L
                GPIO.out_w1tc = ((uint32_t)1 << pin);
                time_off_start = ESP.getCycleCount();

                // Wait till the T1L time reached
                while(ESP.getCycleCount() < time_off_start + DSHOT_CLOCK_CYCLE_T0L) {
                }
            }
        }
    } else {
        // TODO: FUTURE for TURTLE MODE
    }
}