#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);
        bool _loop = false;
        uint8_t bits = 16;
        uint16_t time_on_start, time_off_start, duration_high, duration_low;
        // uint32_t time_profiling_start, time_profiling_end;


        // Serial.print(F("Transmitting bits:"));
        // time_profiling_start = ESP.getCycleCount();
        while(bits != 0) {
            bits--;

            // Serial.print(F("."));

            // Preparing the pulse timing
            if((packet >> bits) & 0x0001) {
                // HIGH PULSE timing
                duration_high = DSHOT_CLOCK_CYCLE_T1H;
                duration_low  = DSHOT_CLOCK_CYCLE_T1L;
            } else {
                // LOW PULSE timing
                duration_high = DSHOT_CLOCK_CYCLE_T0H;
                duration_low  = DSHOT_CLOCK_CYCLE_T0L;
            }

            //                                           __
            // If high then generate high signal pulse _|  |_|
            // Setting pin high for T1H
            GPIO.out_w1ts = ((uint32_t)1 << pin);
            time_on_start = ESP.getCycleCount();

            /***
            for(uint16_t i = 0; i < duration_high; i++) {
                asm volatile("nop");
            }
             ***/

            _loop = true;
            // Wait till the T1H/T1L time reached
            while(_loop) {
                if((uint32_t)(ESP.getCycleCount() - time_on_start) > duration_high)
                    _loop = false;
            }

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

            /***
            for(uint16_t i = 0; i < duration_low; i++) {
                asm volatile("nop");
            }
             ***/
            _loop = true;
            // Wait till the T1L/T0L time reached
            while(_loop) {
                if((uint32_t)(ESP.getCycleCount() - time_off_start) > duration_low) 
                    _loop = false;
            }
        }   // End of while
        // Serial.println(F("Done"));
        // time_profiling_end = ESP.getCycleCount();
        // uint32_t time_profiling_result = time_profiling_end - time_profiling_start;
        
        // Serial.print(F("1 Byte cpu cycles: "));
        // Serial.println(time_profiling_result);
    } else {
        // TODO: FUTURE for TURTLE MODE
    }
}