/* Include Files */
#include "KTag.h"

#define FIRE_CONTROL_REGISTER__IR_OFF                       0b00000
#define FIRE_CONTROL_REGISTER__IR_ON_MODULATED_LOW_POWER    0b00011
#define FIRE_CONTROL_REGISTER__IR_ON_MODULATED_HIGH_POWER   0b00101
#define FIRE_CONTROL_REGISTER__IR_ON_MODULATED_MAX_POWER    0b11111
#define FIRE_CONTROL_REGISTER__IR_ON_UNMODULATED_LOW_POWER  0b00010
#define FIRE_CONTROL_REGISTER__IR_ON_UNMODULATED_HIGH_POWER 0b00100
#define FIRE_CONTROL_REGISTER__IR_ON_UNMODULATED_MAX_POWER  0b00110

#define TRIGGER_STATUS_REGISTER__NO_ACTION        0x00
#define TRIGGER_STATUS_REGISTER__TRIGGER_PULLED   0x01
#define TRIGGER_STATUS_REGISTER__TRIGGER_RELEASED 0x02


void Trigger_Interrupt_ISR();
void Bit_Stream_Timer_ISR();

TimedPulseTrain_T * Shot_Buffer;
TagPacket_T Shot_Packet;

TaskHandle_t Fire_Control_Task_Handle;

static TimedPulseTrain_T * Active_Pulse_Train = NULL;
static uint8_t Active_Bitstream_Index = 0;

static TickType_t TicksAtTriggerPress;

static inline void Initiate_Pulse_Train(TimedPulseTrain_T * pulsetrain)
{
    Bit_Stream_Timer_Disable();
    Active_Pulse_Train = pulsetrain;
    Active_Bitstream_Index = 0;
    
    if (Active_Pulse_Train->bitstream[Active_Bitstream_Index].symbol == MARK)
    {
        Fire_Control_Register_Write(FIRE_CONTROL_REGISTER__IR_ON_MODULATED_MAX_POWER);
    }
    else
    {
        Fire_Control_Register_Write(FIRE_CONTROL_REGISTER__IR_OFF);
    }
    Bit_Stream_Timer_SetPeriod(Active_Pulse_Train->bitstream[0].duration);
    Bit_Stream_Timer_SetCounter(0);
    Active_Bitstream_Index++;
    Bit_Stream_Timer_Enable();
    Bit_Stream_Timer_TriggerStart();
}

static inline void Next_Bit(void)
{
    static BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    
    Bit_Stream_Timer_Disable();
    
    if (Active_Pulse_Train->bitstream[Active_Bitstream_Index].duration != LAST_PULSE)
    {
        if (Active_Pulse_Train->bitstream[Active_Bitstream_Index].symbol == MARK)
        {
            Fire_Control_Register_Write(FIRE_CONTROL_REGISTER__IR_ON_MODULATED_MAX_POWER);
        }
        else
        {
            Fire_Control_Register_Write(FIRE_CONTROL_REGISTER__IR_OFF);
        }
        
        if (Active_Bitstream_Index < ((2*MAX_PULSES) - 2))
        {
            Bit_Stream_Timer_SetPeriod(Active_Pulse_Train->bitstream[Active_Bitstream_Index].duration);
            Bit_Stream_Timer_SetCounter(0);
            Active_Bitstream_Index++;
            Bit_Stream_Timer_Enable();
            Bit_Stream_Timer_TriggerStart();
        }
        else
        {
            // The bitstream is too long!
            
            // Turn the IR Emitter off, and wait a long time.
            Fire_Control_Register_Write(FIRE_CONTROL_REGISTER__IR_OFF);
            xSemaphoreGiveFromISR(NeoPixels_Semaphore, &xHigherPriorityTaskWoken);
            Bit_Stream_Timer_SetPeriod(UINT16_MAX);
            Bit_Stream_Timer_SetCounter(0);
            Active_Pulse_Train = NULL;
            Bit_Stream_Timer_Enable();
            Bit_Stream_Timer_TriggerStart();
        }
    }
    else
    {
        // Turn the IR Emitter off, and wait a long time.
        Fire_Control_Register_Write(FIRE_CONTROL_REGISTER__IR_OFF);
        xSemaphoreGiveFromISR(NeoPixels_Semaphore, &xHigherPriorityTaskWoken);
        Bit_Stream_Timer_SetPeriod(UINT16_MAX);
        Bit_Stream_Timer_SetCounter(0);
        Active_Pulse_Train = NULL;
        Bit_Stream_Timer_Enable();
        Bit_Stream_Timer_TriggerStart();
    }
    
    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

void Init_Fire_Control(void)
{    
    // Register and enable the ISRs.
    Cy_SysInt_Init(&Trigger_Interrupt_cfg, Trigger_Interrupt_ISR);
    Cy_SysInt_Init(&Bit_Stream_Timer_Interrupt_cfg, Bit_Stream_Timer_ISR);
    NVIC_EnableIRQ(Trigger_Interrupt_cfg.intrSrc);
    NVIC_EnableIRQ(Bit_Stream_Timer_Interrupt_cfg.intrSrc);
    
    // Initialize the hardware.
    Bit_Stream_Timer_Clock_Enable();
    Bit_Stream_Timer_Init(&Bit_Stream_Timer_config);
    Bit_Stream_Timer_SetPeriod(2);
    Bit_Stream_Timer_Start();
    SW_CLK_Enable();
    PWM_IR_Modulation_Start();
    
    
    Fire_Control_Register_Write(FIRE_CONTROL_REGISTER__IR_OFF);
}

void Fire_Control_Task(void * pvParameters)
{
    while (true)
    {   
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
}

SystemKResult_T Prepare_Tag()
{
    Shot_Packet.player_ID = NVM_PLAYER_ID;
    Shot_Packet.team_ID = NVM_TEAM_ID;
    Weapon_t weapon = GetWeaponFromID(NVM_WEAPON_ID);
    Shot_Packet.color = (uint32_t)PROTOCOLS_GetColor(weapon.Protocol, Shot_Packet.team_ID, Shot_Packet.player_ID);
    Shot_Packet.protocol = weapon.Protocol;
    Shot_Packet.damage = weapon.Damage_Per_Shot;
    Shot_Buffer = PROTOCOLS_EncodePacket(&Shot_Packet);
    Fire_Control_Set_Modulation_Frequency(PROTOCOLS_GetModulationFrequency(weapon.Protocol));
    return SYSTEMK_RESULT_SUCCESS;
}

SystemKResult_T Send_Tag()
{
    xSemaphoreTake(NeoPixels_Semaphore, portMAX_DELAY);
    Initiate_Pulse_Train(Shot_Buffer);
    
    KEvent_T tag_sent_event = { .ID = KEVENT_TAG_SENT, .Data = (void *)0x00 };
    Post_KEvent(&tag_sent_event);

    return SYSTEMK_RESULT_SUCCESS;
}

void Fire_Control_Set_Modulation_Frequency(ModulationFrequency_T freq)
{
    PWM_IR_Modulation_TriggerKill();
    if (freq == FREQUENCY_38kHz)
    {
        PWM_IR_Modulation_SetPeriod0(314);
        //PWM_IR_Modulation_SetCompare0(314/2);      // 50% Duty Cycle
        PWM_IR_Modulation_SetCompare0((314 * 3)/10); // 30% Duty Cycle
    }
    else // (freq == FREQUENCY_56kHz)
    {
        PWM_IR_Modulation_SetPeriod0(213);
        //PWM_IR_Modulation_SetCompare0(213/2);      // 50% Duty Cycle
        PWM_IR_Modulation_SetCompare0((213 * 3)/10); // 30% Duty Cycle
    }
    PWM_IR_Modulation_TriggerStart();
}

//! ISR for the trigger input.
void Trigger_Interrupt_ISR()
{
    portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
    
    // Clear the interrupt.
    NVIC_ClearPendingIRQ(Trigger_Interrupt_cfg.intrSrc);
    
    // Read the trigger register to know if this was a pull or a release.
    uint8_t trigger_status = Trigger_Status_Reg_Read();
    
    if ((trigger_status & TRIGGER_STATUS_REGISTER__TRIGGER_PULLED) == TRIGGER_STATUS_REGISTER__TRIGGER_PULLED)
    {
        TicksAtTriggerPress = xTaskGetTickCountFromISR();
        KEvent_T switch_event = {.ID = KEVENT_CENTER_SWITCH_PRESSED, .Data = NULL};
        Post_KEvent_From_ISR(&switch_event, &xHigherPriorityTaskWoken);
    }
    else if ((trigger_status & TRIGGER_STATUS_REGISTER__TRIGGER_RELEASED) == TRIGGER_STATUS_REGISTER__TRIGGER_RELEASED)
    {
        uint32_t triggerPressDurationInms = pdTICKS_TO_MS(xTaskGetTickCountFromISR() - TicksAtTriggerPress);
        KEvent_T switch_event = {.ID = KEVENT_CENTER_SWITCH_RELEASED, .Data = (void *) triggerPressDurationInms};
        Post_KEvent_From_ISR(&switch_event, &xHigherPriorityTaskWoken);
    }
    else
    {
        // What happened!!?
    }
    
    // If an event was enqueued above, a context switch might be required.
	// xHigherPriorityTaskWoken was initialized to pdFALSE on interrupt entry.  If calling 
	// xSemaphoreGiveFromISR() caused a task to unblock, and the unblocked task has a
    // priority equal to or higher than the currently running task (the task that was
    // interrupted by this ISR), then xHigherPriorityTaskWoken will have been set to pdTRUE
    // and portEND_SWITCHING_ISR() will request a context switch to the newly unblocked task.
	portEND_SWITCHING_ISR( xHigherPriorityTaskWoken );
}

void Bit_Stream_Timer_ISR()
{
    // Get all the enabled pending interrupts...
    uint32_t source = Bit_Stream_Timer_GetInterruptStatusMasked();
    // ...and clear them.
    Bit_Stream_Timer_ClearInterrupt(source);
    
    if (Active_Pulse_Train != NULL)
    {
        Next_Bit();
    }
}