pre { line-height: 125%; }
td.linenos .normal { color: inherit; background-color: transparent; padding-left: 5px; padding-right: 5px; }
span.linenos { color: inherit; background-color: transparent; padding-left: 5px; padding-right: 5px; }
td.linenos .special { color: #000000; background-color: #ffffc0; padding-left: 5px; padding-right: 5px; }
span.linenos.special { color: #000000; background-color: #ffffc0; padding-left: 5px; padding-right: 5px; }
.highlight .hll { background-color: #ffffcc }
.highlight .c { color: #888888 } /* Comment */
.highlight .err { color: #a61717; background-color: #e3d2d2 } /* Error */
.highlight .k { color: #008800; font-weight: bold } /* Keyword */
.highlight .ch { color: #888888 } /* Comment.Hashbang */
.highlight .cm { color: #888888 } /* Comment.Multiline */
.highlight .cp { color: #cc0000; font-weight: bold } /* Comment.Preproc */
.highlight .cpf { color: #888888 } /* Comment.PreprocFile */
.highlight .c1 { color: #888888 } /* Comment.Single */
.highlight .cs { color: #cc0000; font-weight: bold; background-color: #fff0f0 } /* Comment.Special */
.highlight .gd { color: #000000; background-color: #ffdddd } /* Generic.Deleted */
.highlight .ge { font-style: italic } /* Generic.Emph */
.highlight .ges { font-weight: bold; font-style: italic } /* Generic.EmphStrong */
.highlight .gr { color: #aa0000 } /* Generic.Error */
.highlight .gh { color: #333333 } /* Generic.Heading */
.highlight .gi { color: #000000; background-color: #ddffdd } /* Generic.Inserted */
.highlight .go { color: #888888 } /* Generic.Output */
.highlight .gp { color: #555555 } /* Generic.Prompt */
.highlight .gs { font-weight: bold } /* Generic.Strong */
.highlight .gu { color: #666666 } /* Generic.Subheading */
.highlight .gt { color: #aa0000 } /* Generic.Traceback */
.highlight .kc { color: #008800; font-weight: bold } /* Keyword.Constant */
.highlight .kd { color: #008800; font-weight: bold } /* Keyword.Declaration */
.highlight .kn { color: #008800; font-weight: bold } /* Keyword.Namespace */
.highlight .kp { color: #008800 } /* Keyword.Pseudo */
.highlight .kr { color: #008800; font-weight: bold } /* Keyword.Reserved */
.highlight .kt { color: #888888; font-weight: bold } /* Keyword.Type */
.highlight .m { color: #0000DD; font-weight: bold } /* Literal.Number */
.highlight .s { color: #dd2200; background-color: #fff0f0 } /* Literal.String */
.highlight .na { color: #336699 } /* Name.Attribute */
.highlight .nb { color: #003388 } /* Name.Builtin */
.highlight .nc { color: #bb0066; font-weight: bold } /* Name.Class */
.highlight .no { color: #003366; font-weight: bold } /* Name.Constant */
.highlight .nd { color: #555555 } /* Name.Decorator */
.highlight .ne { color: #bb0066; font-weight: bold } /* Name.Exception */
.highlight .nf { color: #0066bb; font-weight: bold } /* Name.Function */
.highlight .nl { color: #336699; font-style: italic } /* Name.Label */
.highlight .nn { color: #bb0066; font-weight: bold } /* Name.Namespace */
.highlight .py { color: #336699; font-weight: bold } /* Name.Property */
.highlight .nt { color: #bb0066; font-weight: bold } /* Name.Tag */
.highlight .nv { color: #336699 } /* Name.Variable */
.highlight .ow { color: #008800 } /* Operator.Word */
.highlight .w { color: #bbbbbb } /* Text.Whitespace */
.highlight .mb { color: #0000DD; font-weight: bold } /* Literal.Number.Bin */
.highlight .mf { color: #0000DD; font-weight: bold } /* Literal.Number.Float */
.highlight .mh { color: #0000DD; font-weight: bold } /* Literal.Number.Hex */
.highlight .mi { color: #0000DD; font-weight: bold } /* Literal.Number.Integer */
.highlight .mo { color: #0000DD; font-weight: bold } /* Literal.Number.Oct */
.highlight .sa { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Affix */
.highlight .sb { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Backtick */
.highlight .sc { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Char */
.highlight .dl { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Delimiter */
.highlight .sd { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Doc */
.highlight .s2 { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Double */
.highlight .se { color: #0044dd; background-color: #fff0f0 } /* Literal.String.Escape */
.highlight .sh { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Heredoc */
.highlight .si { color: #3333bb; background-color: #fff0f0 } /* Literal.String.Interpol */
.highlight .sx { color: #22bb22; backgr
/************************************************************************************************************************
 **************************************************************************************************************************/

#include <DataDef.h>
#include <Drivers/FPGA/Motors_Driver/PowerSTEP01.h>
#include "include.h"

#include "PMR/Hardware/UploadHardWareConfigurationRequest.pb-c.h"
#include "PMR/Hardware/HardWareConfiguration.pb-c.h"
#include "PMR/Hardware/HardwareMotor.pb-c.h"
#include "PMR/Hardware/HardwareDancer.pb-c.h"
#include "PMR/Hardware/HardwareWinder.pb-c.h"
#include "PMR/Hardware/HardwareBreakSensor.pb-c.h"
#include "PMR/Printing/JobSpool.pb-c.h"
#include "PMR/common/MessageContainer.pb-c.h"

#include "Common/SWUpdate/FileSystem.h"
#include "drivers/Flash_Memory/fatfs/ff.h"
#include "drivers/SSI_Comm/Dancer/Dancer.h"

#include "thread.h"
MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};

double MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};

int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0};
int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0};
HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0};

#define BREAK_SENSOR_LIMIT 10

bool BreakSensorenabled;
int32_t BreakSensordebouncetimemilli = BREAK_SENSOR_LIMIT;

HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_PID_CONTROL_TYPE__MotorFeeder,HARDWARE_PID_CONTROL_TYPE__MotorDryer,HARDWARE_PID_CONTROL_TYPE__MotorPooler,HARDWARE_PID_CONTROL_TYPE__MotorWinder,0};

//********************************************************************************************************************
uint32_t MotorsConfigMessage(HardwareMotor * request)
{
    uint32_t status = PASSED;
    TimerMotors_t Motor_i;
    Motor_i = request->hardwaremotortype;
        MotorsCfg[Motor_i].configword = request->configword;
        MotorsCfg[Motor_i].hardwaremotortype = request->hardwaremotortype;
        MotorsCfg[Motor_i].minfrequency = request->minfrequency;
        MotorsCfg[Motor_i].maxfrequency = request->maxfrequency;
        MotorsCfg[Motor_i].setmicrostep = request->setmicrostep;
        MotorsCfg[Motor_i].microstep = request->microstep;
        MotorsCfg[Motor_i].maxchangeslope = request->maxchangeslope;
        MotorsCfg[Motor_i].highlengthmicrosecond = request->highlengthmicrosecond;
        MotorsCfg[Motor_i].speedmaster = request->speedmaster;
        MotorsCfg[Motor_i].pulseperround = request->pulseperround;
        MotorsCfg[Motor_i].pulleyradius = request->pulleyradius;
        MotorsCfg[Motor_i].configword = request->configword;
        if(MotorDriverResponse[Motor_i].DriverType == VoltageCombinedMotDriver)
        {
            MotorsCfg[Motor_i].directionthreadwize = !(request->directionthreadwize);//PowerSTEP01
        }
        else
        {
            MotorsCfg[Motor_i].directionthreadwize = request->directionthreadwize;//L6470 + L6472
        }
        MotorsCfg[Motor_i].directionthreadwize = request->directionthreadwize;
        MotorsCfg[Motor_i].kvalhold = request->kvalhold;
        MotorsCfg[Motor_i].kvalrun = request->kvalrun;
        MotorsCfg[Motor_i].kvalacc = request->kvalacc;
        MotorsCfg[Motor_i].kvaldec = request->kvaldec;
        MotorsCfg[Motor_i].overcurrentthreshold = request->overcurrentthreshold;
        MotorsCfg[Motor_i].stallthreshold = request->stallthreshold;
        MotorsCfg[Motor_i].thermalcompensationfactor = request->thermalcompensationfactor;
        MotorsCfg[Motor_i].lowspeedoptimization = request->lowspeedoptimization;
        MotorsCfg[Motor_i].stslp = request->stslp;
        MotorsCfg[Motor_i].intspd = request->intspd;
        MotorsCfg[Motor_i].fnslpacc = request->fnslpacc;
        MotorsCfg[Motor_i].fnslpdec = request->fnslpdec;
        MotorsCfg[Motor_i].fsspd = request->fsspd;

        status = MotorConfig( Motor_i,  &MotorsCfg[Motor_i]);
//        if (Motor_i == MOTOR_RDRIVING)
//            ThreadInitialTestStub(request);

        return status;
//    }
//    else return Motor_i;

}
uint32_t MotorPidRequestMessage(HardwarePidControl* request)
{
    int Motor_i,i;
    int temp;
    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
    {
        if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
        {
            Motor_i = i;
            break;
        }
    }
    memcpy (&MotorsControl[Motor_i],request,sizeof(HardwarePidControl));
    if (MotorsControl[Motor_i].pvinputfilterfactormode > MAX_CONTROL_SAMPLES)
        MotorsControl[Motor_i].pvinputfilterfactormode = MAX_CONTROL_SAMPLES;
#ifdef TEST_LONGER_PID_THREAD
    MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control
#endif
    for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
    {
        MotorSamples[Motor_i][i] = 0;  //reset the samples value for control beginning
        MotorSpeedSamples[Motor_i][i] = 0;
    }
    NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
    temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
    temp=(NORMAL_COEF_DIVIDER*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm);
    NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp;
//    uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
    temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
    temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*3/2);
    DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
    return OK;
}
char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg";

uint32_t StoreDancerConfigMessage()
{
    FRESULT Fresult = FR_OK;
    HardwareConfiguration DancerConfig;
    HardwareDancer Dancers[MAX_SYSTEM_DANCERS];
    uint8_t* response_buffer;
    size_t response_size = 0;
    int Dancer_i;

    hardware_configuration__init(&DancerConfig);
    DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS);
    for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++)
    {
        hardware_dancer__init(&Dancers[Dancer_i]);
        DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i];
        Dancers[Dancer_i].has_zeropoint = true;
        Dancers[Dancer_i].hardwaredancertype = Dancer_i;
        Dancers[Dancer_i].has_hardwaredancertype = true;
        DancerConfig.dancers[Dancer_i]->zeropoint=Control_Read_Dancer_Position(Dancer_i,0,0);
        DancerConfig.n_dancers++;
        DancersCfg[Dancer_i].zeropoint = DancerConfig.dancers[Dancer_i]->zeropoint;
    }
    response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig));
    if (response_buffer)
    {
        response_size = hardware_configuration__pack(&DancerConfig, response_buffer);
    }
    Fresult = FileWrite(response_buffer,response_size,DancerConfigPath);
    my_free(response_buffer);

    return Fresult;
}
uint32_t LoadDancerConfigMessage()
{
    void* buffer = NULL;
    uint32_t Bytes = 0;
    FRESULT Fresult = FR_OK;

    HardwareConfiguration *DancerConfig;
    int Dancer_i;

    Fresult = FileRead(DancerConfigPath, &Bytes, &buffer);
    if (Fresult == FR_OK)
    {
        DancerConfig = hardware_configuration__unpack(NULL, Bytes, buffer);
        for (Dancer_i = 0; Dancer_i < DancerConfig->n_dancers ; Dancer_i++)
        {
            DancersCfg[Dancer_i].zeropoint = DancerConfig->dancers[Dancer_i]->zeropoint;
        }
        hardware_configuration__free_unpacked(DancerConfig,NULL);
        free (buffer);
    }
    return Fresult;

}

uint32_t DancerConfigMessage(HardwareDancer * request)
{
    uint32_t status = PASSED;
    int Dancer_i;

    Dancer_i = request->hardwaredancertype;
    if (Dancer_i<MAX_SYSTEM_DANCERS )
    {
        memcpy (&DancersCfg[Dancer_i],request,sizeof(HardwareDancer));
        return status;
    }
    else
        return Dancer_i;

}
uint32_t ThreadConfigBreakSensor(void *request)
{
    HardwareBreakSensor  *SensorCfg = (HardwareBreakSensor*)request;
    if (SensorCfg)
    {
        BreakSensorenabled = SensorCfg->enabled;
        if (SensorCfg->debouncetimemilli)
        {
            BreakSensordebouncetimemilli = SensorCfg->debouncetimemilli;
        }
        return OK;
    }
    return ERROR;
}
uint32_t thread_init(void)
{
    //memset (MotorsCfg,0,sizeof(MotorsCfg));
    //memset (&InternalWinderCfg,0,sizeof(InternalWinderConfigStruc));

    return OK;
}
void ThreadSetBreakSensorLimit(int limit)
{
    if (limit)
        BreakSensordebouncetimemilli = limit;
}