/*
 * Stub_Dancer.c
 *
 *  Created on: May 7, 2018
 *      Author: avi
 */


#include <Container.h>
#include <DataDef.h>
#include <Drivers/FPGA/Motors_Driver/L6470.h>
#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>

#include <PMR/Stubs/StubDancerPositionResponse.pb-c.h>
#include <PMR/Stubs/StubDancerPositionRequest.pb-c.h>

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_uart.h"

#include "Drivers/USB_Communication/USBCDCD.h"


#include "MessageContainer.pb-c.h"

#include "Stub_Status.h"
#include "drivers/FPGA/FPGA.h"
#include "drivers/SPI/SPI_Comm.h"

#include "driverlib/ssi.h"
#include "drivers/SPI/SPI_Comm.h"
#include "drivers/FPGA/FPGA_SSI_Comm.h"
#include "Modules/Thread/Thread_ex.h"


#include "Modules/thread/thread.h"


void Stub_DancerPositionRequest(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    StubDancerPositionRequest* request = stub_dancer_position_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    StubDancerPositionResponse response =   STUB_DANCER_POSITION_RESPONSE__INIT;

    response.dancer_id = request->dancer_id;
    response.has_dancer_id = true;

    #ifdef FOUR_WINDERS
    Dancers_4_Winders Dancer_Id = (Dancers_4_Winders)request->dancer_id;//to remove warning
    #else
    HardwareDancerType Dancer_Id = (HardwareDancerType)request->dancer_id;//to remove warning
    #endif

    if(Dancer_Id < NUM_OF_ROTENC)
    {
        FPGA_SSI_Transmit(Dancer_Id);
        SysCtlDelay(1000);
        FPGA_SSI_Receive(Dancer_Id);

        response.position = DANCER_ENC[request->dancer_id].Position;
        response.has_position = true;
        response.general_status = DANCER_ENC[request->dancer_id].Gen_status;
        response.has_general_status = true;
        response.detailed_status = DANCER_ENC[request->dancer_id].Det_status;
        response.has_general_status = true;
        StoreDancerConfigMessage(Dancer_Id);
    }
    else if(Dancer_Id < NUM_OF_ROTENC*2)
    {
        int Dancer = Dancer_Id - NUM_OF_ROTENC;
        response.position = DancersCfg[Dancer].zeropoint;
        response.has_position = true;
        response.general_status = DANCER_ENC[Dancer].Gen_status;
        response.has_general_status = true;
        response.detailed_status = DANCER_ENC[Dancer].Det_status;
        response.has_general_status = true;
    }
    else
    {
        response.detailed_status = ERROR; // use the Reserved bits to send our errors
        response.has_general_status = true;
    }
    responseContainer = createContainer(MESSAGE_TYPE__StubDancerPositionResponse, requestContainer->token, true, &response, &stub_dancer_position_response__pack, &stub_dancer_position_response__get_packed_size);

    //free(request);
    //-------------------------------------------------------------------------------------------
    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    free(responseContainer.data.data);
    SendChars((char*)container_buffer, container_size);
    //free(container_buffer);
    //free(requestContainer);
    stub_dancer_position_request__free_unpacked(request,NULL);
}