<?xml version="1.0" encoding="utf-8"?>
<configuration>
  <configSections>
    
    <section name="entityFramework" type="System.Data.Entity.Internal.ConfigFile.EntityFrameworkSection, EntityFramework, Version=6.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089" requirePermission="false" />
  <!-- For more information on Entity Framework configuration, visit http://go.microsoft.com/fwlink/?LinkID=237468 --></configSections>
  <entityFramework>
    <defaultConnectionFactory type="System.Data.Entity.Infrastructure.SqlConnectionFactory, EntityFramework" />
    <providers>
      <provider invariantName="System.Data.SqlClient" type="System.Data.Entity.SqlServer.SqlProviderServices, EntityFramework.SqlServer" />
    </providers>
  </entityFramework>
  <runtime>
/************************************************************************************************************************
 * Thread_print.c
 * Printing module is responsible for :
     * operating diffrent winding algorithms with predefined parameters from the UI
     * operating the dispensers according to predefined dispensing rate from the UI
 **************************************************************************************************************************/
#include "include.h"
#include "thread.h"
#include "../control/control.h"
#include "../control/pidalgo.h"
#include "PMR/Hardware/HardwareMotor.pb-c.h"
#include "PMR/Hardware/HardwareMotorType.pb-c.h"
#include "PMR/Printing/JobSegment.pb-c.h"
#include "PMR/Printing/JobTicket.pb-c.h"

#include "StateMachines/Printing/PrintingSTM.h"

#include "drivers/Motors/Motor.h"
#include "drivers/Danser_SSI/ssi_comm.h"
#include "drivers/Heater/TemperatureSensor.h"
#include "drivers/Heater/Heater.h"
////////////////////////////////State machine operation////////////////////////////////////
//the state machine operation is used to operate in runtime correct profile flow execution
//by recieved esign flow of the user from the UI
///////////////////////////////////////////////////////////////////////////////////////////


TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW};
DANCER_ENUM ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};

typedef struct
{
    bool                m_isEnabled;
    int32_t             m_SetParam;
    float               m_mesuredParam;
    float               m_preError;
    float               m_integral;
    float               m_calculatedError;
    bool                m_isReady;
    PID_Config_Params   m_params;
}MotorControlConfig_t;

MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];
////////////////////////Slow Motor State////////////////////////////////////
uint32_t ThreadPreSegmentState(void *JobDetails);

////////////////////////////////////////////////////////////////////////////

uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    //read value is the dancer angle
    int index=MAX_THREAD_MOTORS_NUM;
    if (IfIndex>>8 != IfTypeThread)
    {
        LOG_ERROR (IfIndex, "Wrong  Interface type");
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;
/*    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
        if (ControlIdtoMotorId[i] == deviceID)
        {
            index = i;
            break;
        }
    if (index==MAX_THREAD_MOTORS_NUM)
    {
        LOG_ERROR (deviceID, "No motor  for device");
        return 0xFFFFFFFF;
    }
    */
    if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0))
    {
        MotorControlConfig[index].m_mesuredParam = ReadValue;
        MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam,
                                                                              &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
        if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
        {
            MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MAX;
        }
        if (MotorControlConfig[index].m_calculatedError < MotorControlConfig[index].m_params.MIN)
        {
            MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
        }

        //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
    }
 return OK;
}
uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
//#define MAX_CONTROL_SAMPLES 6
//extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
//extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];

    //read value is the dancer angle
    int i,index=MAX_THREAD_MOTORS_NUM;
    int Pid_Id,DancerId;
    int32_t TranslatedReadValue, avreageSampleValue = 0;
    double NormalizedError;
    if (IfIndex>>8 != IfTypeThread)
    {
        LOG_ERROR (IfIndex, "Wrong  Interface type");
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;

    /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
        if (ControlIdtoMotorId[i] == MotorId)
        {
            index = i;
            break;
        }
    if (index==MAX_THREAD_MOTORS_NUM)
    {
        LOG_ERROR (MotorId, "No motor  for device");
        return 0xFFFFFFFF;
    }*/

    if(MotorControlConfig[index].m_isEnabled )
    {
        Pid_Id = ThreadMotorIdToControlId[index];
        DancerId = ThreadMotorIdToDancerId[index];
        TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
        MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
        MotorSamplePointer[index]++;
        if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0;
        for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
            avreageSampleValue += MotorSamples[index][i];
        avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
        NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
if (index != 0) return OK;
        MotorControlConfig[index].m_mesuredParam = NormalizedError;
        MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
                                                                              &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
        /*if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
        {
            MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MAX;
        }
        if (MotorControlConfig[index].m_calculatedError < MotorControlConfig[index].m_params.MIN)
        {
            MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
        }*/

        MotorSetSpeed(ThreadMotorIdToMotorId[index], (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index],  MotorsCfg[ThreadMotorIdToMotorId[index]].microstep);
        //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
    }

 return OK;
}

//********************************************************************************************************************

//********************************************************************************************************************
/*#ifdef DEBUG_TEST_FUNCTIONS
uint32_t Debug_Get_Dancer_Read(uint32_t DancerId, uint32_t Parameter1, uint32_t Parameter2)
{

    return (rand() % (103 + 1 + 103) - 103);
}
#endif*/
uint32_t ThreadInitialTestStub(HardwareMotor * request)
{


    //MotorsConfigMessage(request);
     ThreadPrepareState(request);
     ThreadPreSegmentState(request);
    return OK;
}
//********************************************************************************************************************
 uint32_t ThreadPrepareState(void *JobDetails)
{
    int Motor_i, HW_Motor_Id, Pid_Id;
    //start thread control for all motors
    for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
    {
        HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
        Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/
            MotorControlConfig[Motor_i].m_params.MAX = 1;
            MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1;
            MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime;
            MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
            MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
            MotorControlConfig[Motor_i].m_params.epsilon = 0.01;
            MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond;
            MotorControlConfig[Motor_i].m_calculatedError = 0;
            MotorControlConfig[Motor_i].m_integral = 0;
            MotorControlConfig[Motor_i].m_isEnabled = true;
            MotorControlConfig[Motor_i].m_isReady = true;
            MotorControlConfig[Motor_i].m_mesuredParam = 0;
            MotorControlConfig[Motor_i].m_preError = 0;
            MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage

            MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
    #ifdef DEBUG_TEST_FUNCTIONS

            if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
            /*if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
    //            continue;
                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i);
            else if ((HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);*/
    #else
            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
                continue;
                //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
            else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING))
                AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
    #endif
    }
    Winder_Prepare();
    //set 3 dancers to the profile positions

    return OK;
}

//********************************************************************************************************************
uint32_t ThreadPreSegmentState(void *JobDetails)
{
//set the speed only before the first segment, speed is constant accros job
    JobTicket* JobTicket = JobDetails;
    int Motor_i, HW_Motor_Id;

    int process_speed = JobTicket->processparameters->dyeingspeed;

    process_speed = 10; //debug

    for (Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++)
    {
        HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
        //(Speed*uStep*PPR)/((2*PI*motor_Radius)
        double motor_speed = (process_speed *  MotorsCfg[HW_Motor_Id].pulseperround *  MotorsCfg[HW_Motor_Id].microstep)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
        //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
        OriginalMotorSpd_2PPS[Motor_i] = (int)motor_speed;
    }
    //ControlStart();
    // set the new speed in the dryer motor to the speed of the new segment
    MotorSetSpeed(MOTOR_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR],  MotorsCfg[MOTOR_DRYER_DRIVING].microstep);
    MotorSetSpeed(MOTOR_RLOADING, 1, 1);
    MotorSetSpeed(MOTOR_LLOADING, 1,1);

    // activate control fr all motors
    //set speed for both rocker motors
    //wait for all motors to get to the required speed (set the target speed for the control to check)
    //call the job state machine when the thread system is ready
    PreSegmentReady(Module_Thread,OK);

    return OK;
}

//********************************************************************************************************************
 uint32_t ThreadSegmentState(void *JobDetails)
{
    return OK;
}

//********************************************************************************************************************
 uint32_t ThreadEndState(void *JobDetails)
{
     int Motor_i;
     for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
        StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
     }
     StopMotor(MOTOR_RLOADING,Hard_Hiz);
     StopMotor(MOTOR_LLOADING,Hard_Hiz);

    return OK;
}



//********************************************************************************************************************

void ThreadStartPrinting(void)
{
    //PrintingIterate();
}

//********************************************************************************************************************
//********************************************************************************************************************

void ThreadStopPrinting(void)
{
    //PrintingIterate();
}