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

#include <DataDef.h>
#include "include.h"

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

#include "thread.h"
HardwareMotor  MotorsCfg[NUM_OF_MOTORS]={0};
HardwarePidControl MotorsControl[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};
InternalWinderConfigStruc InternalWinderCfg = {0};
HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0};

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 InternalWinderConfigMessage(HardwareWinder* request)
{
    uint32_t status = PASSED;

    InternalWinderCfg.milimetersperrotation = request->millimeterperrotation;

    return status;
}
uint32_t InternalWindingConfigMessage(JobSpool* request)
{
    uint32_t status = PASSED;

    InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses;
    InternalWinderCfg.spoolbackingrate = request->backingrate;
    InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
    InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
    InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
    InternalWinderCfg.diameter = request->diameter;

    return status;
}

//********************************************************************************************************************
uint32_t MotorsConfigMessage(HardwareMotor * request)
{
    uint32_t status = PASSED;
    TimerMotors_t Motor_i;
    Motor_i = request->hardwaremotortype;
    /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
    {
        if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
        {
            Motor_i = i;
            break;
        }
    }
    if (Motor_i == 0)
    {
        LOG_ERROR(request->hardwarepidcontroltype,"ERROR Control Id");
        return ERROR;
    }*/
    //if (Motor_i< MAX_THREAD_MOTORS_NUM)
    //{
        memcpy (&MotorsCfg[Motor_i],request,sizeof(HardwareMotor));

        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;
    for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++)
        MotorSamples[Motor_i][i] = 0;  //reset the samples value for control beginning
    NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
    temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits);
    temp=(10*(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*2);
    DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength);
    return OK;
}
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 thread_init(void)
{
    //memset (MotorsCfg,0,sizeof(MotorsCfg));
    //memset (&InternalWinderCfg,0,sizeof(InternalWinderConfigStruc));

    return OK;
}