/*
 * Stub_TempSensor.c
 *
 *  Created on: May 27, 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/StubTempSensorResponse.pb-c.h>
#include <PMR/Stubs/StubTempSensorRequest.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 "drivers/Heater/TemperatureSensor.h"

#include "Modules/thread/thread.h"
#include "Drivers/I2C_Communication/Head_Card/PT100/Head_PT100_ADC.h"
#include "Drivers/I2C_Communication/Head_Card/PT100/ADS122X04.h"
#include "Modules/Control/MillisecTask.h"

void Stub_TempSensorRequest(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    StubTempSensorRequest* request = stub_temp_sensor_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    StubTempSensorResponse response =   STUB_TEMP_SENSOR_RESPONSE__INIT;

    static bool first_time_flag = TRUE;

    if(request->stemp_sensor_id & 0x100) //ADS120  - Internal Temperature Sensor
    {
        request->stemp_sensor_id = request->stemp_sensor_id &~0x100;

        response.temp_sensor_id = request->stemp_sensor_id;
        response.has_temp_sensor_id = true;

        if(first_time_flag == TRUE)
        {
            FPGA_SensorInitConfig();
            HeadADCPT100_InitConfigReg();
            first_time_flag = FALSE;
        }

        if(request->stemp_sensor_id < MAX_MAIN_CARD_TEMP_SENS_ID)
        {
            response.temperature_c_mult_by_100 = Test_ADS1220_Internal_Temperature_Sensor(request->stemp_sensor_id); //main
        }
        else if(request->stemp_sensor_id < MAX_HEAD_CARD_TEMP_SENS_ID)
        {
            response.temperature_c_mult_by_100 = Test_ADS122X_Internal_Temperature_Sensor(request->stemp_sensor_id) *100;//head / WHS
        }
        response.has_temperature_c_mult_by_100 = true;

/*        TemperatureSensorsReset();
        SysCtlDelay(5000);
        FPGA_SensorInitConfig();
        SysCtlDelay(5000);
*/
        first_time_flag = TRUE;

    }
    else if(request->stemp_sensor_id & 0x8000) //Init + Read the temperature from ADC
    {
        request->stemp_sensor_id = request->stemp_sensor_id &~0x8000;

        response.temp_sensor_id = request->stemp_sensor_id;
        response.has_temp_sensor_id = true;

        if(first_time_flag == TRUE)
        {
            FPGA_SensorInitConfig();
            HeadADCPT100_SensorInitConfig();
            first_time_flag = FALSE;
        }

        if(request->stemp_sensor_id < MAX_MAIN_CARD_TEMP_SENS_ID)
        {
        }
        else
        if(request->stemp_sensor_id < MAX_HEAD_CARD_TEMP_SENS_ID)
        {
            control_HeadCard_PT100(HEAD_CONFIG_ODD_PT100_MUX);
            delayms(50);//must use deley
            control_HeadCard_PT100(HEAD_CONFIG_EVEN_PT100_MUX);
            //delayms(5000);//must use deley
            //control_HeadCard_PT100(HEAD_CONFIG_ODD_PT100_MUX);

            //response.temperature_c_mult_by_100 = TempSensorResponse[request->stemp_sensor_id].PT100_ADC_Reading_Bits;
            //response.temperature_c_mult_by_100 = TempSensorResponse[12].Temperature_C_mult_by_100;
        }
    }
    else //
    {

        response.temp_sensor_id = request->stemp_sensor_id;
        response.has_temp_sensor_id = true;


        if(first_time_flag == TRUE)
        {
            FPGA_SensorInitConfig();
            HeadADCPT100_SensorInitConfig();
            first_time_flag = FALSE;
        }
        //Debug_Start_PT00(request->stemp_sensor_id);

        if(request->stemp_sensor_id < MAX_MAIN_CARD_TEMP_SENS_ID)
        {
            response.temperature_c_mult_by_100 = TempSensorResponse[request->stemp_sensor_id].Temperature_C_mult_by_100;
        }
        else if(request->stemp_sensor_id < MAX_HEAD_CARD_TEMP_SENS_ID)
        {
            /*
            //--------------------------------------------------------------------------------------
            if(request->stemp_sensor_id < HEAD_PT100_ZONE_2_0X80_1)
                control_HeadCard_PT100(HEAD_CONFIG_ODD_PT100_MUX);
            else
                control_HeadCard_PT100(HEAD_CONFIG_EVEN_PT100_MUX);
            //---------------------------------------------------------------------------------------

             */
            //response.temperature_c_mult_by_100 = TempSensorResponse[request->stemp_sensor_id].PT100_ADC_Reading_Bits;
            //delayms(1000);
            response.temperature_c_mult_by_100 = TempSensorResponse[request->stemp_sensor_id].Temperature_C_mult_by_100;
        }

        response.temperature_c_mult_by_100 = MillisecGetTemperatures(request->stemp_sensor_id);


    /*
        if(first_flag == 0)
        {
                FPGA_SensorInitConfigReg(); // Initial the configuration registers


                FPGA_SensorConfig_callback((TEMPERATURE_SENSOR_ID_ENUM) request->stemp_sensor_id, SENSOR_CONFIG_REG0);//0x08
                SysCtlDelay(5000);

                FPGA_SensorConfig_callback((TEMPERATURE_SENSOR_ID_ENUM) request->stemp_sensor_id, SENSOR_CONFIG_REG1);//0x04
                SysCtlDelay(5000);

                FPGA_SensorConfig_callback( (TEMPERATURE_SENSOR_ID_ENUM)request->stemp_sensor_id, SENSOR_CONFIG_REG2);//0x53
                SysCtlDelay(5000);

                FPGA_SensorConfig_callback( (TEMPERATURE_SENSOR_ID_ENUM)request->stemp_sensor_id, SENSOR_CONFIG_REG3);//0x70
                SysCtlDelay(5000);

                first_flag =1;
        }*/
            //SysCtlDelay(1000);

            /* ----------------- use the last measurement ---------------------
            TemperatureSensorSync((TEMPERATURE_SENSOR_ID_ENUM)request->stemp_sensor_id);
            SysCtlDelay(300);
            TemperatureSendSensorDummyClk((TEMPERATURE_SENSOR_ID_ENUM)request->stemp_sensor_id);
            SysCtlDelay(400);
            TemperatureSensorReadFromFPGA_Res((TEMPERATURE_SENSOR_ID_ENUM)request->stemp_sensor_id);
            */

            response.has_temperature_c_mult_by_100 = true;



    }

    responseContainer = createContainer(MESSAGE_TYPE__StubTempSensorResponse, requestContainer->token, true, &response, &stub_temp_sensor_response__pack, &stub_temp_sensor_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);

    stub_temp_sensor_request__free_unpacked(request,NULL);
}