/*
 * Dancer.c
 *
 *  Created on: Aug 5, 2018
 *      Author: avi
 */

#include "include.h"
#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>

#include "inc/hw_memmap.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/ssi.h"
#include "driverlib/sysctl.h"
#include "utils/uartstdio.h"

#include <driverlib/rom.h>
#include <driverlib/rom_map.h>
#include <DataDef.h>
#include <Drivers/SSI_Comm/SSI_Comm.h>
#include "driverlib/uart.h"

#include "drivers/FPGA/FPGA_SSI_Comm.h"
#include "PMR/Hardware/HardwareDancerType.pb-c.h"
#include <driverlib/timer.h>
#include "Modules/Control/MillisecTask.h"


extern SSI_DANCER_ENC DANCER_ENC[NUM_OF_ROTENC];

typedef struct  // 24bit
{
    unsigned short Position:14;
    unsigned char Gen_status:2;
    unsigned char Det_status:8;
}SSI_ENC;

SSI_ENC SSI_enc;

#ifndef HUNDRED_MICROSECONDS_DANCER_READ
#ifdef FOUR_WINDERS
uint32_t Control_Read_Dancer_Position(Dancers_4_Winders DancerId, uint32_t Parameter1)
#else
uint32_t Control_Read_Dancer_Position(HardwareDancerType DancerId, uint32_t Parameter1)
#endif
{
    return DANCER_ENC[DancerId].Position;
    //return SSI_enc.Position;
}
#endif

#ifdef FOUR_WINDERS
uint32_t Read_Dancer_Position (Dancers_4_Winders DancerId)
#else
uint32_t Read_Dancer_Position (HardwareDancerType DancerId)
#endif
{
    #ifdef EVALUATION_BOARD
        uint32_t ENC_SSI_Data = 0;

        SSIEnable(SSI1_BASE);

        ENC_SSI_Data = Write_Dummy_Byte();


        //SSI_enc.Det_status = ENC_SSI_Data & 0xFF;
        //ENC_SSI_Data = ENC_SSI_Data >> 8;
        SSI_enc.Det_status = 0;

        SSI_enc.Gen_status = ENC_SSI_Data & 0x3;
        ENC_SSI_Data = ENC_SSI_Data >> 2;
        SSI_enc.Position =   ENC_SSI_Data & 0x3FFF; // up 11130 down 8747

        SSIDisable(SSI1_BASE);
        return SSI_enc.Position;
    #else
        // must call FPGA_SSI_Transmit first!
        #ifdef FOUR_WINDERS
        Dancers_4_Winders Dancer_Id = (Dancers_4_Winders)DancerId;//to remove warning
        #else
        HardwareDancerType Dancer_Id = (HardwareDancerType)DancerId;//to remove warning
        #endif
        FPGA_SSI_Receive(Dancer_Id);
        FPGA_SSI_Transmit(Dancer_Id);
        return DANCER_ENC[Dancer_Id].Position;
    #endif
}

uint32_t Read_Dryer_ENC_Position ()
{
        // must call FPGA_SSI_Dryer_ENC_Transnit first!
    FPGA_SSI_Dryer_ENC_Receive();
    FPGA_SSI_Dryer_ENC_Transnit();
    //REPORT_MSG(Dryer_ENC.Position,"Read_Dryer_ENC_Position");
    return Dryer_ENC.Position;
}
#ifdef FOUR_WINDERS
uint32_t Read_Dryer_Status(Dancers_4_Winders DancerId)
#else
uint32_t Read_Dryer_Status(HardwareDancerType DancerId)
#endif
{
    return Dryer_ENC.Gen_status;
}
#ifdef FOUR_WINDERS
uint32_t Control_Read_Dryer_Position(Dancers_4_Winders DancerId, uint32_t Parameter1)
#else
uint32_t Control_Read_Dryer_Position(HardwareDancerType DancerId, uint32_t Parameter1)
#endif

{
    return Dryer_ENC.Position;
    //return SSI_enc.Position;
}

void Loop_SSI()
{
    while(1)
    {
        Read_Dancer_Position(HARDWARE_DANCER_TYPE__RightDancer);
        SysCtlDelay(1000); // Min 1000 (Pause time > 20uSec)
    }

}

//-----------------------------------------------------------------

#include "drivers/FPGA/FPGA_SPI_Comm.h"
bool test_dancer_direction = false, test_dancer_flag = false;

void test_dancer_responce_RTFU()
{
    static uint32_t i = 0, FT = 0;

    static uint32_t save_milisec = 0;

    static uint16_t Save_start_ENC = 0;


    if(test_dancer_flag == true)
    {
        i=1;
        test_dancer_flag = false;
        MotorDriverRequest[21].Direction = test_dancer_direction;
        //MotorDriverRequest[21].Speed = 60;
        //FPGA_SetMotSpeedDirect(21);//RDancer
        MotorSetSpeed(21, 60);
        save_milisec = msec_millisecondCounter;
    }

    if((i>0)&&(i<5000))
    {
        FPGA_SSI_Transmit(2);//RDancer
        delayUs(100);
        FPGA_SSI_Receive(2);//RDancer

        if(i == 1)
        {
            Save_start_ENC = DANCER_ENC[2].Position;
        }
        if((((DANCER_ENC[2].Position - Save_start_ENC) >= 2 ) || ((Save_start_ENC - DANCER_ENC[2].Position) >= 2 )) && (FT == 0))
        {
            FT = 1;
            Report("*-*-*-*-* time till movement mSec *-*-*-*-*", __FILE__, __LINE__, 0, RpMessage, (int)(/*abs*/(msec_millisecondCounter-save_milisec)), 0);

        }

        Report("*-*-*-*-* test_dancer_responce_RTFU *-*-*-*-*", __FILE__, __LINE__, 0, RpMessage, (int)(DANCER_ENC[2].Position), 0);
        i=i+1;
    }
    else
    {
        i=0;
        FT = 0;
        //MotorDriverRequest[21].Stop = Hard_Stop;
        //FPGA_SetMotStop(21);
    }
}