#include <Container.h>
#include <PMR/Common/MessageContainer.pb-c.h>
#include <PMR/Stubs/StubDispenserRequest.pb-c.h>
#include <PMR/Stubs/StubDispenserResponse.pb-c.h>
#include "include.h"
#include "driverlib/gpio.h"

#include "Drivers/USB_Communication/USBCDCD.h"
#include "drivers/adc_sampling/adc.h"
#include "modules/ids/ids_ex.h"

#include <Drivers/I2C_Communication/Dispenser_Card/IO_Ports/Dispenser_IO.h>

#include "Stub_Status.h"
#include <math.h>

void Stub_DispenserRequest(MessageContainer* requestContainer)
{
    uint32_t status = PASSED;
    MessageContainer responseContainer;

    StubDispenserRequest* request = stub_dispenser_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
/*
    ADCAcquireInit();
    SysCtlDelay(10000);
    ADCAcquireStart(0,1);
    SysCtlDelay(10000);
    ADC_TriggerCollection();
    SysCtlDelay(10000);

    ADC0SS0Handler();
    SysCtlDelay(100000);
*/

    //TCA9534Regs Regs;

    //static bool first_time = true;

    //if(first_time == true)
    //if ((Regs.Config[request->dispenserid].bit.DISP_DOWN != TCA9534_INPUT) || (Regs.Config[request->dispenserid].bit.DISP_UP!= TCA9534_INPUT))
    //{
        status |= TCA9534Init(request->dispenserid/*, &Regs*/,true);
        //first_time = false;
    //}

    if(request->setdirection == true)
    {
        TCA9534TestRelay(request->dispenserid/*, &Regs*/,ENABLE);
    }
    else
        TCA9534TestRelay(request->dispenserid/*, &Regs*/,DISABLE);

    delayms(5);


     if(request->start == true)
     {
        TCA9534ByPass(request->dispenserid/*, &Regs*/, ENABLE);

     }
     else
        TCA9534ByPass(request->dispenserid/*, &Regs*/, DISABLE);

     delayms(5);


    /*
    if(request->setmicrostepdivision == 1)
        TCA9534TestByPassTestRelay(request->dispenserid, &Regs);
    delayms(5);
    if(request->setspeed == 1)
        test_IO();
    delayms(5);
    */

    //request->dispenserid
	//request->start
	//request->setdirection
	//request->setmicrostepdivision
	//request->setspeed


    StubDispenserResponse response =   STUB_DISPENSER_RESPONSE__INIT;

    //response.dispenserid = (uint32_t)((round)(CalculateDispenserPressure(0x00000004)));//CHAN_DISPENSE_PRESSURE_6);


    status |= TCA9534SetReadInputReg(request->dispenserid);
    status |= TCA9534ReadInputReg(request->dispenserid/*, &Regs*/);

	
	response.dispenserid = request->dispenserid;
	response.has_dispenserid = true;
	
	response.dispenserposition = (Get_Notification_Disp_UP(request->dispenserid/*, &Regs*/) << 8) | (Get_Notification_Disp_Down(request->dispenserid/*, &Regs*/));
	response.has_dispenserposition = true;

	response.inkworninglevel = (Get_Notification_Realy_On(request->dispenserid/*, &Regs*/) << 8) | (Get_Notification_Over_Press(request->dispenserid/*, &Regs*/));
	response.has_inkworninglevel = true;
	
	status_response(status,&response.status, &response.statusword ,&response.has_statusword);


    Write_status_response(status);

    responseContainer = createContainer(MESSAGE_TYPE__StubDispenserResponse, requestContainer->token, true, &response, &stub_dispenser_response__pack, &stub_dispenser_response__get_packed_size);

    //-------------------------------------------------------------------------------------------
    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_dispenser_request__free_unpacked(request,NULL);
}