/*
 * Copyright (c) 2013 - 2016, Freescale Semiconductor, Inc.
 * Copyright 2016-2017 NXP
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * o Redistributions of source code must retain the above copyright notice, this list
 *   of conditions and the following disclaimer.
 *
 * o Redistributions in binary form must reproduce the above copyright notice, this
 *   list of conditions and the following disclaimer in the documentation and/or
 *   other materials provided with the distribution.
 *
 * o Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#include "../source/system/nt_system_prv.h"
#include "../source/system/nt_system_mem_prv.h"
#include "nt_controls.h"
#include "../source/controls/nt_controls_prv.h"
#include "nt_control_matrix.h"
#include "nt_control_matrix_prv.h"

/* Invoke callback based on the even has occurred */
static void _nt_control_matrix_invoke_callback(struct nt_control_data *control,
                                      enum nt_control_matrix_event event,
                                      nt_points16 *points,
                                      uint32_t index_or_gesture);
/* The Matrix control initialization function. */
static int32_t _nt_control_matrix_init(struct nt_control_data *control);
/* The Matrix control process function. */
static int32_t _nt_control_matrix_process(struct nt_control_data *control);
/* The Matrix control function for position recognition */
void _nt_control_matrix_get_position_enhanced (uint16_t *position1,
                                               uint16_t *position2,
                                               uint16_t state);
void _nt_control_matrix_get_position (uint16_t *position1,
                                      uint16_t *position2,
                                      uint16_t state,
                                      uint8_t *add_val);
/* Function to calculate the coincidence amount to determine how much 
   was actual unlock gesture like stored one. */
uint16_t _nt_control_matrix_gesture_recognition(nt_coordinates64_xy *buffer,
                                                nt_coordinates64_xy *gesture, uint16_t finger);

/* Function to calculate the coincidence amount to determine how much 
   was actual unlock gesture like stored one. */
uint16_t _nt_control_matrix_gesture_recognition_norm(nt_coordinates64_xy *buffer,
                                                     nt_coordinates64_xy *gesture, uint16_t finger);

/* Function to calculate the coincidence amount to determine how much 
   was actual unlock gesture like stored one. */
uint16_t _nt_control_matrix_gesture_recognition_square(nt_coordinates64_xy *buffer,
                                                       nt_coordinates64_xy *gesture, uint16_t finger);

/* Function to calculate the coincidence amount to determine how much 
   was actual unlock gesture like stored one. */
uint16_t _nt_control_matrix_gesture_recognition_angle(nt_coordinates64_xy *buffer,
                                                      nt_coordinates64_xy *gesture, uint16_t finger);
uint16_t _nt_control_matrix_gesture_recognition_angle2(nt_coordinates64_xy *buffer,
                                                       nt_coordinates64_xy *gesture, uint16_t finger);
uint16_t _nt_control_matrix_gesture_recognition_XY(nt_coordinates64_xy *buffer,
                                                   nt_coordinates64_xy *gesture, uint16_t finger);
uint16_t _nt_control_matrix_horiz_vertic_recognition(nt_coordinates64_xy *buffer, uint16_t finger);

/* The Matrix control interface. */
const struct nt_control_interface nt_control_matrix_interface = {
  .init    = _nt_control_matrix_init,
  .process = _nt_control_matrix_process,
  .name    = "nt_control_matrix_interface",
};

/* Clearing function */
void _nt_control_matrix_clean (nt_points64 *buffer);
/* Copy function */
void _nt_control_matrix_copy (nt_points16 *buffer_copy, nt_points16 *buffer_orig);


void nt_control_matrix_register_callback(const struct nt_control *control,
                                       nt_control_matrix_callback callback)
{
    NT_ASSERT(control != NULL);
    NT_ASSERT(control->interface == &nt_control_matrix_interface);
    
    struct nt_control_data *control_data = _nt_control_get_data(control);
    NT_ASSERT(control_data != NULL);
    
    struct nt_control_matrix_data *ram = control_data->data.matrix;
    NT_ASSERT(ram != NULL);
    
    ram->callback = callback;
}


static void _nt_control_matrix_invoke_callback(struct nt_control_data *control,
                                      enum nt_control_matrix_event event,
                                      nt_points16 *points,
                                      uint32_t index_or_gesture)
{
    struct nt_control_matrix_data *ram = control->data.matrix;
    NT_ASSERT(ram != NULL);
    
    if (ram->callback != NULL) 
    {
        ram->callback(control->rom, event, points, index_or_gesture);
    }
}  

/* Position recognition for specific axis and one or two finger positions.
   In case of two fingers on the touchpad it can swap their coordinates, 
   because it groups the low and high coordinates together.
   This function eliminates the nearest electrode activations and calculates middle
   position only - reduces the finger touch size.
   If increase_resolution mode selected the position recognition uses enhanced algorithm with different adding constant 
   defined in init unction. It enhances the position resolution by accepting the two-neighbor electrode as 
   a new virtual electrode in the middle.
*/ 
void _nt_control_matrix_get_position(uint16_t *position1,
                                     uint16_t *position2,
                                     uint16_t state,
                                     uint8_t *add_val) 
 
{   uint16_t actual_position[2] = {0,0};   
    uint16_t position = 1;
    uint16_t finger = 0;

/* Position recognition for specific axis and one or two finger positions.
   In case of two fingers on the touchpad it can swap their coordinates, 
   because it groups the low and high coordinates together.
   This function eliminates the nearest electrode activations and calculates middle
   position only - reduces the finger touch size.
   If increase_resolution mode selected the position recognition uses enhanced algorithm with different adding constant 
   defined in init unction. It enhances the position resolution by accepting the two-neighbor electrode as 
   a new virtual electrode in the middle.
*/ 
    while(state)           
    {                                                   
        if (finger > 1)                                 /* maximum two finger are detected */
            finger = 1;                                 /* the next finger would be rewritten */
        
        uint32_t xxx = state & 0x0007; 
        switch (xxx)                         /* three nearest electrodes are touched simultaneously */ 
        {                 
        case 0x0007:                                    
            {   actual_position[finger++] = position + add_val[0]; /* use position in the middle */ 
                state >>= 3;                            /* update electrode status for next step */
                position += add_val[1];                 /* update position counter for next step */ 
            }                                           
            break;  
        case 0x0003:                                    /* two nearest electrodes are touched simultaneously */
            {   actual_position[finger++] = position + add_val[2];   /* use lower position */    
                state >>= 2;                            /* update electrode status for next step */
                position += add_val[3];                 /* update position counter for next step */
            }                                           
            break;   
        case 0x0005:                                    
            {   actual_position[finger++] = position;   /* use position in the middle */ 
                state >>= 1;                            /* update electrode status for next step */
                position += add_val[4];                 /* update position counter for next step */ 
            }                                           
            break;             
        case 0x0001:                                    /* no nearest electrodes touched together */
            {   actual_position[finger++] = position;   /* use exact position */ 
                state >>= 1;                            /* update electrode status for next step */
                position += add_val[4];                   /* update position counter for next step */
            }                                           
            break;                                      
        default:                                        /* no electrode touched in current step */
            {   state >>= 1;                            /* update electrode status for next step */
                position += add_val[5];                   /* update position counter for next step */
            }    
            break;
        }            
    }          

    *position1 = actual_position[0];
    *position2 = actual_position[1];         
      
}

/* Function recognizes weather drawn swipe is more enough horizontal or vertical. The gesture must be longer than 3 
   recognized positions horizontally or vertically and the movement in dominant direction must cover twice the amount 
   recognized positions than in insignificant direction. */ 
uint16_t _nt_control_matrix_horiz_vertic_recognition(nt_coordinates64_xy *buffer, uint16_t finger)
{      
    uint64_t temp64;  
    uint16_t temp16; 
    int8_t sum_buff_x [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t sum_buff_y [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t sum_x = 0;
    int8_t sum_y = 0;
    
    if ((buffer->x | buffer->y) > 0xfff)      /* too short gesture - shorter than 3 point changes will not be recognized */       
    {          
        temp64  = buffer->x;
        while (temp64)                        /* Calculation of captured points lie in each column for captured gesture */
        {   temp16 = (temp64 & (uint64_t)0xf);/* Separate position from the first stored */
            sum_buff_x[temp16]++;             /* Find out the number of unchanged positions */
            temp64 >>= 4;                     /* Move to next position change in buffer */
        } 
        temp64 = 16;
        while (temp64--)                      /* Activated position calculation for x coordinate */
        {   if (sum_buff_x[temp64])
            sum_x++;              
        } 
        
        temp64  = buffer->y;
        while (temp64)                        /* Calculation of captured points lie in each row for captured gesture */
        {   temp16 = (temp64 & (uint64_t)0xf);/* Separate position from the first stored */
            sum_buff_y[temp16]++;             /* Find out the number of unchanged positions */
            temp64 >>= 4;                     /* Move to next position change in buffer */
        } 
        temp64 = 16;
        while (temp64--)                       /* Activated position calculation for y coordinate */
        {   if (sum_buff_y[temp64])
            sum_y++;              
        } 
        
        if (sum_x > sum_y)                     /* Dominant direction determination */
        {                                      /* Projection to x axis is higher */
            sum_y <<= 1; 
            if (sum_x >= sum_y)                /* Projection to x axis is double y axis - gesture is enough horiz/vertic */ 
            {
                return 0;
            }       
        }
        else 
        {
            sum_x <<= 1; 
            if (sum_y >= sum_x)                /* Projection to y axis is double x axis - gesture is enough horiz/vertic */ 
            {
                return 0;
            }  
        }        
    }                                          
    return 0x7fff;                             /* return max error if gesture is too short or is not enough horiz/vertic */
}

/* Function to calculate the coincidence amount to determine how much 
   was actual unlock gesture like stored one. Sum of angles between nearest points of gesture */
uint16_t _nt_control_matrix_gesture_recognition_angle(nt_coordinates64_xy *buffer,
                                                      nt_coordinates64_xy *gesture, uint16_t finger)
{      
    uint64_t temp64;  
    int16_t temp16;
    int16_t temp16_result=0;
    uint16_t temp_x;
    uint16_t temp_y;    
    uint16_t cntr = 0;    
    uint16_t buff_cntr = 0;   
    uint16_t gest_cntr = 0;    
    int8_t buff_diff_x [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t buff_diff_y [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t gest_diff_x [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t gest_diff_y [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t angles_buff [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
    int8_t angles_gest [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
    int8_t angles_buff1 [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
    int8_t angles_gest1 [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
    const uint8_t atan [5][5] = {{0,90,90,90,90},              /* table for atanXY calculation, */ 
                                 {0,45,63,72,76},              /* returning degree values,      */
                                 {0,27,45,56,63},              /* max input for x or y is 4     */
                                 {0,18,34,45,53},
                                 {0,14,27,37,45}};
          
    if ((buffer->x | buffer->y) > 0xfffff)  /* too short gesture - shorter than 5 point changes will not be recognized */       
    {          
            buff_cntr = 0;                 /* Init buffer counter */
            temp64  = buffer ->x;
            while (temp64)                 /* Calculate differences of position movement */
            {   temp16 = ((temp64 & (uint64_t)0xf0)>>4) - /* calculate  difference through two changes */
                          (temp64 & (uint64_t)0xf);
                buff_diff_x [buff_cntr++] = temp16; /* buffer size calculation and store differences in buff_cntr step*/
                temp64 >>= 4;                       /* update while condition */
            }  
            temp64  = buffer ->y;
            if (temp64 != 0)
                buff_cntr = 0;             /* Init buffer counter if buffer includes data */
            while (temp64)                 /* Calculate differences of position movement */
            {   temp16 = ((temp64 & (uint64_t)0xf0)>>4) - /* calculate  difference through two changes */
                          (temp64 & (uint64_t)0xf);
                buff_diff_y [buff_cntr++] = temp16;  /* buffer size calculation and store differences in buff_cntr step*/
                temp64 >>= 4;                       /* update while condition */
            }
            
            gest_cntr = 0;
            temp64  = gesture ->x;         /* Init buffer counter */
            while (temp64)                 /* Calculate differences of position movement */
            {   temp16 = ((temp64 & (uint64_t)0xf0)>>4) - /* calculate  difference through two changes */
                          (temp64 & (uint64_t)0xf);
                gest_diff_x [gest_cntr++] = temp16; /* buffer size calculation and store differences in buff_cntr step*/
                temp64 >>= 4;                       /* update while condition */
            }           
            temp64  = gesture ->y;
            if (temp64 != 0)
                gest_cntr = 0;            /* Init buffer counter if buffer includes data */
            while (temp64)                /* Calculate differences of position movement */
            {   temp16 = ((temp64 & (uint64_t)0xf0)>>4) - /* calculate  difference through two changes */
                          (temp64 & (uint64_t)0xf);
                gest_diff_y [gest_cntr++] = temp16; /* buffer size calculation and store differences in buff_cntr step*/
                temp64 >>= 4;                       /* update while condition */
            }            
            temp16 = 0;
            cntr = 0;
            temp64 = buffer ->x & gesture->x;  /* while cycles number are determine from buffer and unlock gesture long */
            temp64 >>= 4;                      /* avoid first and last step because of difference through two changes */
            while (temp64)                
            {   if (buff_diff_x[cntr] < 5 && buff_diff_y[cntr ] < 5 &&  /* avoid differences outside of the table */  
                    gest_diff_x[cntr] < 5 && gest_diff_y[cntr] < 5)
                {                
                    temp_x = (buff_diff_x[cntr] < 0) ? -buff_diff_x[cntr] : buff_diff_x[cntr] ; /* table inputs are  */
                    temp_y = (buff_diff_y[cntr] < 0) ? -buff_diff_y[cntr] : buff_diff_y[cntr]  ; /* always positive values*/
#define NEW 0 //with 1 it does not work correctly
#if NEW
                    if ((buff_diff_x[cntr]) >= 0 && (buff_diff_x[cntr]) >= 0)
                        temp16 = atan[temp_x][temp_y] ;
                    if ((buff_diff_x[cntr]) < 0 && (buff_diff_x[cntr]) >= 0)
                        temp16 = atan[temp_x][temp_y] + 90;                    
                    if ((buff_diff_x[cntr]) >= 0 && (buff_diff_x[cntr]) < 0)
                        temp16 = atan[temp_x][temp_y] + 180;
                    if ((buff_diff_x[cntr]) < 0 && (buff_diff_x[cntr]) < 0)
                        temp16 = atan[temp_x][temp_y] + 270;    
                    angles_buff[cntr] = temp16;
#else 
                    if ((buff_diff_x[cntr] * buff_diff_y[cntr]) < 0)    /* AtanXY result must be negative if one */
                        temp16 = -atan[temp_x][temp_y];          /* input if negative, two neg. inputs give pos. res.*/
                    else 
                        temp16 = atan[temp_x][temp_y]; 
                    angles_buff[cntr] = temp16;
                    if (cntr == 0) 
                        ;
                    else 
                        temp16 = temp16 - angles_buff[cntr-1];
                    temp16 += temp16;
                    angles_buff1[cntr] = temp16;
#endif     
                    temp_x = (gest_diff_x[cntr] < 0) ? -gest_diff_x[cntr] : gest_diff_x[cntr] ; /* table inputs are always */
                    temp_y = (gest_diff_y[cntr] < 0) ? -gest_diff_y[cntr] : gest_diff_y[cntr] ; /* positive values */                 
#if NEW 
                    if ((buff_diff_x[cntr]) >= 0 && (buff_diff_x[cntr]) >= 0)
                        temp16 -= atan[temp_x][temp_y] ;
                    if ((buff_diff_x[cntr]) < 0 && (buff_diff_x[cntr]) >= 0)
                        temp16 -= atan[temp_x][temp_y] + 90;                    
                    if ((buff_diff_x[cntr]) >= 0 && (buff_diff_x[cntr]) < 0)
                        temp16 -= atan[temp_x][temp_y] + 180;
                    if ((buff_diff_x[cntr]) < 0 && (buff_diff_x[cntr]) < 0)
                        temp16 -= atan[temp_x][temp_y] + 270; 
                    angles_gest[cntr] = temp16;
#else                     
                    if ((gest_diff_x[cntr] * gest_diff_y[cntr]) < 0)   /* AtanXY result must be negative if one */
                        temp16 = -atan[temp_x][temp_y];    //was  temp16 += atan     /* input if negative, two neg. inputs give pos. res.*/
                    else
                        temp16 = +atan[temp_x][temp_y];    //was  temp16 -= atan 
                    angles_gest[cntr] = temp16;
                    if (cntr == 0) 
                        ;
                    else 
                        temp16 = temp16 - angles_gest[cntr-1];
                    temp16 += temp16;
                    angles_gest1[cntr] = temp16;
#endif      
                    temp16_result += angles_buff1[cntr] - angles_gest1[cntr];
                   //if (temp16 < 0)                              /* Angle error accumulation with absolute value */
                   //    temp16_result -= temp16;      
                   //else                             
                   //    temp16_result += temp16;                ///sumation is not correct
                }
                cntr++;                                          /* Buffer and gest counter incrementation */ 
                if (cntr > buff_cntr || cntr > gest_cntr)        /* Counter cannot be higher than */ 
                    return 0x7fff;                               /* unlock gesture or buffer size */
                temp64 >>= 4;
            }
//            if (temp16 < 0)
//                temp16 = -temp16;
            return (temp16_result);                              /* return angle error */
    }
    else 
    {                                                            /* return max angle error */
        return 0x7fff;
    }
}


/* Function to calculate the coincidence amount to determine how much 
   was actual gesture like stored one. Sum of angles between near points of gesture but middle point is skipped. */
uint16_t _nt_control_matrix_gesture_recognition_angle2(nt_coordinates64_xy *buffer,
                                                       nt_coordinates64_xy *gesture, uint16_t finger)
{      
    uint64_t temp64;  
    int16_t temp16;
    int16_t temp16_result=0;
    uint16_t temp_x;
    uint16_t temp_y;    
    uint16_t cntr = 0;    
    uint16_t buff_cntr = 0;   
    uint16_t gest_cntr = 0;    
    int8_t buff_diff_x [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t buff_diff_y [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t gest_diff_x [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t gest_diff_y [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    const uint8_t atan [5][5] = {{0,90,90,90,90},              /* table for atanXY calculation, */ 
                                 {0,45,63,72,76},              /* returning degree values,      */
                                 {0,27,45,56,63},              /* max input for x or y is 4     */
                                 {0,18,34,45,53},
                                 {0,14,27,37,45}};
          
    if ((buffer->x | buffer->y) > 0xfffff)  /* too short gesture - shorter than 5 point changes will not be recognized */       
    {          
            buff_cntr = 0;                 /* Init buffer counter */
            temp64  = buffer ->x;
            while (temp64)                 /* Calculate differences of position movement */
            {   temp16 = ((temp64 & (uint64_t)0xf00)>>8) - /* calculate  difference through two changes */
                          (temp64 & (uint64_t)0xf);
                buff_diff_x [buff_cntr++] = temp16; /* buffer size calculation and store differences in buff_cntr step*/
                temp64 >>= 4;                       /* update while condition */
            }  
            temp64  = buffer ->y;
            if (temp64 != 0)
                buff_cntr = 0;             /* Init buffer counter if buffer includes data */
            while (temp64)                 /* Calculate differences of position movement */
            {   temp16 = ((temp64 & (uint64_t)0xf00)>>8) - /* calculate  difference through two changes */
                          (temp64 & (uint64_t)0xf);
                buff_diff_y [buff_cntr++] = temp16;  /* buffer size calculation and store differences in buff_cntr step*/
                temp64 >>= 4;                       /* update while condition */
            }
            
            gest_cntr = 0;
            temp64  = gesture ->x;         /* Init buffer counter */
            while (temp64)                 /* Calculate differences of position movement */
            {   temp16 = ((temp64 & (uint64_t)0xf00)>>8) - /* calculate  difference through two changes */
                          (temp64 & (uint64_t)0xf);
                gest_diff_x [gest_cntr++] = temp16; /* buffer size calculation and store differences in buff_cntr step*/
                temp64 >>= 4;                       /* update while condition */
            }           
            temp64  = gesture ->y;
            if (temp64 != 0)
                gest_cntr = 0;            /* Init buffer counter if buffer includes data */
            while (temp64)                /* Calculate differences of position movement */
            {   temp16 = ((temp64 & (uint64_t)0xf00)>>8) - /* calculate  difference through two changes */
                          (temp64 & (uint64_t)0xf);
                gest_diff_y [gest_cntr++] = temp16; /* buffer size calculation and store differences in buff_cntr step*/
                temp64 >>= 4;                       /* update while condition */
            }            
            temp16 = 0;
            cntr = 0;
            temp64 = buffer ->x & gesture->x;  /* while cycles number are determine from buffer and gesture long */
            temp64 >>= 8;                      /* avoid first and last step because of difference through two changes */
            while (temp64)                
            {   if (buff_diff_x[cntr] < 5 && buff_diff_y[cntr ] < 5 &&  /* avoid differences outside of the table */  
                    gest_diff_x[cntr] < 5 && gest_diff_y[cntr] < 5)
                {                
                    temp_x = (buff_diff_x[cntr] < 0) ? -buff_diff_x[cntr] : buff_diff_x[cntr] ; /* table inputs are  */
                    temp_y = (buff_diff_y[cntr] < 0) ? -buff_diff_y[cntr] : buff_diff_y[cntr]  ; /* always positive values*/
                    if ((buff_diff_x[cntr] * buff_diff_y[cntr]) < 0)    /* AtanXY result must be negative if one */
                        temp16 = -atan[temp_x][temp_y];          /* input if negative, two neg. inputs give pos. res.*/
                    else 
                        temp16 = atan[temp_x][temp_y]; 
                    
                    temp_x = (gest_diff_x[cntr] < 0) ? -gest_diff_x[cntr] : gest_diff_x[cntr] ; /* table inputs are always */
                    temp_y = (gest_diff_y[cntr] < 0) ? -gest_diff_y[cntr] : gest_diff_y[cntr] ; /* positive values */
                    if ((gest_diff_x[cntr] * gest_diff_y[cntr]) < 0)   /* AtanXY result must be negative if one */
                        temp16 += atan[temp_x][temp_y];          /* input if negative, two neg. inputs give pos. res.*/
                    else
                        temp16 -= atan[temp_x][temp_y];
                    if (temp16 < 0)                              /* Angle error accumulation with absolute value */
                        temp16_result -= temp16;      
                    else                             
                        temp16_result += temp16;     
                }
                cntr++;                                          /* Buffer and gest counter incrementation */ 
                if (cntr > buff_cntr || cntr > gest_cntr)        /* Counter can not be higher than */ 
                    return 0x7fff;                               /* unlock gesture or buffer size */
                temp64 >>= 4;
            }
//            if (temp16 < 0)
//                temp16 = -temp16;
            return (temp16_result);                              /* return angle error */
    }
    else 
    {                                                            /* return max angle error */
        return 0x7fff;
    }
}

/* Function to calculate the coincidence amount to determine how much 
   was actual gesture like stored one. Algorism is based on summation of captured points coordinates in rows and columns */
uint16_t _nt_control_matrix_gesture_recognition_XY(nt_coordinates64_xy *buffer,
                                                   nt_coordinates64_xy *gesture, uint16_t finger)
{      
    uint64_t temp64;  
    uint16_t temp16;
    int16_t temp16_result=0;
    int16_t temp16sign;   
    int8_t sum_buff_x [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t sum_buff_y [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t sum_gest_x [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
    int8_t sum_gest_y [16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; /* array initialization */
          
    if ((buffer->x | buffer->y) > 0xffff)  /* too short gesture - shorter than 5 point changes will not be recognized */       
    {          
        temp64  = buffer ->x;
        while (temp64)                           /* Calculation of captured points lie in each column for captured gesture */
        {   temp16 = (temp64 & (uint64_t)0xf);
            sum_buff_x[temp16]++;
            temp64 >>= 4;              
        } 
        
        temp64  = buffer ->y;
        while (temp64)                           /* Calculation of captured points lie in each row for captured gesture */
        {   temp16 = (temp64 & (uint64_t)0xf);
            sum_buff_y[temp16]++;
            temp64 >>= 4;              
        } 
        
        temp64  = gesture ->x;
        while (temp64)                           /* Calculation of captured points lie in each column for stored gesture */
        {   temp16 = (temp64 & (uint64_t)0xf);
            sum_gest_x[temp16]++;
            temp64 >>= 4;              
        } 
        
        temp64  = gesture ->y;
        while (temp64)                          /* Calculation of captured points lie in each row for stored gesture */
        {   temp16 = (temp64 & (uint64_t)0xf);
            sum_gest_y[temp16]++;
            temp64 >>= 4;              
        } 
        temp16_result = 0;                     
        for (temp16 = 0; temp16 < 11; temp16++ ) /* Comparing column numbers between captured and stored gestures */
        {    temp16sign = sum_buff_x[temp16] - sum_gest_x[temp16];
            if (temp16sign < 0)                  
                temp16_result -= temp16sign;      
            else                             
                temp16_result += temp16sign;  
        }           
        for (temp16 = 0; temp16 < 11; temp16++ ) /* Comparing row numbers between captured and stored gestures */
        {    temp16sign = sum_buff_y[temp16] - sum_gest_y[temp16];
            if (temp16sign < 0)                 
                temp16_result -= temp16sign;      
            else                             
                temp16_result += temp16sign;  
        }            
        return (temp16_result);                  /* return the diferences */
    }
    else 
    {                                            /* return max error if gestture is too short */
        return 0x7fff;
    }
}


/* Function to calculate the coincidence amount to determine how much 
   was actual unlock gesture like stored one. Sum of point to point distances power of two.*/
uint16_t _nt_control_matrix_gesture_recognition_square(nt_coordinates64_xy *buffer,
                                                       nt_coordinates64_xy *gesture, uint16_t finger)
{      
    uint64_t temp64;
    uint64_t temp_buffer ;
    uint64_t temp_gesture;
    int16_t temp16;
    uint16_t temp_return = 0;
    uint16_t n = 0;
    uint16_t size = 0;
         
    temp_buffer  = buffer ->x;
    temp_gesture = gesture->x;    
     
    if (temp_buffer > 0xffff)        
    {   
        for (n=0; n<2; n++)
        {   temp16 = 0;         
            if (temp_buffer != 0)      /* calculate clb for captured unlock gesture only if not zero */
            {   temp64 = temp_buffer;  
                while (!(temp64 & 0xf000000000000000))
                {   temp64 <<= 4;
                    temp16++;
                }
            }
            else
            {   temp16 += 16;
            }
            if (temp_gesture != 0)     /* calculate clb difference between capture and stored gesture only if not zero */
            {   temp64 = temp_gesture; 
                while (!(temp64 & 0xf000000000000000))
                {    temp64 <<= 4;
                    temp16--;
                }
            }
            else
            {   temp16 += 16;
            }
            if (temp16 < 0)            /* absolute value of clf difference */
                temp16 = -temp16;
            if (temp16 > 31)           /* diference overflow */
                temp16 = 0;
            if (temp16 > 2)            /* diference is too high */
                return 0x7fff;
            
            temp64 = temp_buffer & temp_gesture; 
            while (temp64)
            {   temp16 = (temp_buffer  & (uint64_t)0xf) -
                         (temp_gesture & (uint64_t)0xf);
                temp16 *= temp16;
                temp_return += (uint16_t)temp16;
                temp_buffer  >>= 4;
                temp_gesture >>= 4;
                temp64 >>= 4; 
                size++;
            }
            temp_buffer  = buffer ->y;
            temp_gesture = gesture->y;
                 
        }
        temp_return<<=4; /* multiplication by const to obtain useful result */
        size *= size;    /* div by second power to normalize the number - remove depending of unlock gesture size */
        return (temp_return/size);
    }
    else 
    {
        return 0x7fff;
    }
}

/* Function to calculate the coincidence amount to determine how much 
   was actual unlock gesture like stored one. Sum of point to point distances. */
uint16_t _nt_control_matrix_gesture_recognition(nt_coordinates64_xy *buffer,
                                                nt_coordinates64_xy *gesture, uint16_t finger)
{      
    uint64_t temp64;
    uint64_t temp_buffer;
    uint64_t temp_gesture;
    int16_t temp16;
    uint16_t temp_return = 0;
    uint16_t n;

    temp_buffer  = buffer ->x;
    temp_gesture = gesture->x;    
     
    if (temp_buffer > 0xffff)        
    {   
        for (n=0; n<2; n++)
        {   temp16 = 0;
            if (temp_buffer != 0)      /* calculate clb for captured unlock gesture only if not zero */
            {   temp64 = temp_buffer;  
                while (!(temp64 & 0xf000000000000000))
                {   temp64 <<= 4;
                    temp16++;
                }
            }
            else
            {   temp16 += 16;
            }
            if (temp_gesture != 0)     /* calculate clb difference between capture and stored gesture only if not zero */
            {   temp64 = temp_gesture; 
                while (!(temp64 & 0xf000000000000000))
                {   temp64 <<= 4;
                    temp16--;
                }
            }
            else
            {   temp16 += 16;
            }
            if (temp16 < 0)            /* absolute value of clf difference */
                temp16 = -temp16;
            if (temp16 > 31)           /* diference overflow */
                temp16 = 0;
            if (temp16 > 2)            /* diference is too high */
                return 0x7fff;
            
            temp64 = temp_buffer & temp_gesture; 
            
            while (temp64)                    /* calculate the point to point distance */ 
            {   temp16 = (temp_buffer  & (uint64_t)0xf) -
                         (temp_gesture & (uint64_t)0xf);
                if (temp16 < 0)
                    temp16 = -temp16;
                temp_return += (uint16_t)temp16;
                temp_buffer  >>= 4;
                temp_gesture >>= 4;
                temp64 >>= 4; 
            }
            temp_buffer  = buffer ->y;
            temp_gesture = gesture->y;      
        }
        return (temp_return);
    }
    else 
    {
        return 0x7fff;
    }
}

/* Function to calculate the coincidence amount to determine how much 
   was actual gesture like stored one. Sum of point to point distances related to unlock gesture size. */
uint16_t _nt_control_matrix_gesture_recognition_norm(nt_coordinates64_xy *buffer,
                                                     nt_coordinates64_xy *gesture, uint16_t finger)
{      
    uint64_t temp64;
    uint64_t temp_buffer ;
    uint64_t temp_gesture;
    int16_t temp16;
    uint16_t temp_return = 0;
    uint16_t n;
    uint16_t size = 0;
         
    temp_buffer  = buffer ->x;
    temp_gesture = gesture->x;    
     
    if (temp_buffer > 0xffff)        
    {   
        for (n=0; n<2; n++)
        {   temp16 = 0;
            if (temp_buffer != 0)      /* calculate clb for captured unlock gesture only if not zero */
            {   temp64 = temp_buffer; 
                while (!(temp64 & 0xf000000000000000))
                {   temp64 <<= 4;
                    temp16++;
                }
            }
            else
            {   temp16 += 16;
            }
            if (temp_gesture != 0)     /* calculate clb difference between capture and stored gesture only if not zero */
            {   temp64 = temp_gesture; 
                while (!(temp64 & 0xf000000000000000))
                {    temp64 <<= 4;
                    temp16--;
                }
            }
            else
            {   temp16 += 16;
            }
            if (temp16 < 0)            /* absolute value of clf difference */
                temp16 = -temp16;
            if (temp16 > 31)           /* difference overflow */
                temp16 = 0;
            if (temp16 > 2)            /* difference is too high */
                return 0x7fff;
            
            temp64 = temp_buffer & temp_gesture; 
            
            while (temp64)
            {   temp16 = (temp_buffer  & (uint64_t)0xf) -
                         (temp_gesture & (uint64_t)0xf);
                if (temp16 < 0)
                    temp16 = -temp16;
                temp_return += (uint16_t)temp16;
                temp_buffer  >>= 4;
                temp_gesture >>= 4;
                temp64 >>= 4; 
                size++;
            }
            temp_buffer  = buffer ->y;
            temp_gesture = gesture->y;      
        }
        temp_return<<=4;
        return (temp_return/size);
    }
    else 
    {
        return 0x7fff;
    }
}

static int32_t _nt_control_matrix_init(struct nt_control_data *control)
{
    NT_ASSERT(control != NULL);
    NT_ASSERT(control->rom->interface == &nt_control_matrix_interface);
    NT_ASSERT(control->rom->control_params.matrix != NULL);
    
    uint8_t n,k;
    uint16_t pins;
    const struct nt_control_matrix *matrix = control->rom->control_params.matrix;
    
    if (matrix->touchpad_size_horizontal > 15U ||
        matrix->touchpad_size_horizontal == 0U ||        
        matrix->touchpad_size_vertical > 15 ||
        matrix->touchpad_size_vertical == 0 ||
        matrix->touchpad_size_vertical == 0U || 
        (matrix->touchpad_size_horizontal > 8U && matrix->increase_resolution > 0) ||   
        (matrix->touchpad_size_vertical   > 8U && matrix->increase_resolution > 0) ||   
        matrix->dissimilarity_max > 500)                     
    {
      return NT_FAILURE;
    }
    
    control->data.matrix = _nt_mem_alloc(sizeof(struct nt_control_matrix_data));
    
    if(control->data.matrix == NULL)
    {
        return NT_OUT_OF_MEMORY;
    }
    
    if (_nt_control_check_data(control) != NT_SUCCESS) {
        return NT_FAILURE;
    }
    
    NT_ASSERT(control->data.matrix != NULL);
    struct nt_control_matrix_data *ram = control->data.matrix;

    ram->touchpad_mask_horizontal = 0;
    for (n=0 ; n < control->rom->control_params.matrix->touchpad_size_horizontal ; n++)  
        ram->touchpad_mask_horizontal |= (1 << n);

    /* Calculate touchpad mask */
    ram->touchpad_mask = 0;      
    if (matrix->sensing_mode)
    {   /* mask for mutual-cap touchpad */
        pins = matrix->touchpad_size_horizontal * matrix->touchpad_size_vertical;  
        if (pins > 36) /* Maximum TSI mutual-cap channel restriction */     
            pins = 36;
    }
    else
    {   /* mask for self-cap touchpad */
        pins = matrix->touchpad_size_horizontal + matrix->touchpad_size_vertical;
        if (pins > 25) /* Maximum TSI self-cap channel restriction */     
            pins = 25;    
    }
   /* Set touchpad mask from touchpad sizes and touchpad sensing mode */         
    for (n=0 ; n < pins ; n++)  
        ram->touchpad_mask |= (1 << n);
    
    /* clear the quantities */
    ram->buffer_position         = 0; 
    _nt_control_matrix_clean(&ram->buffer);
    ram->double_tap_next_time    = 0;      
    ram->last_electode_states    = 0;   
    ram->select_gesture          = 0;
    ram->recognized_gesture      = 0;   
    ram->two_points_gesture_flag = 0;
    
    /* Section for gesture storing */
    for (n=0; n<NT_MATRIX_UNLOCK_GESTURES; n++)
    {   for (k=0; k<2; k++)
        {   ram->gesture[n][k].x = matrix->gesture[n][k].x;
            ram->gesture[n][k].y = matrix->gesture[n][k].y;
        }
        /* Set the two-points gesture flag for copied stored gesture from flash */
        if (ram->gesture[n][1].x || ram->gesture[n][1].y)            /* if second finger has data */
        {   ram->two_points_gesture_flag |= 0x1 << (n); /* set two finger gesture flag */ 
        } 
    }
    
    /* Initialize constant for get_position function due to increase_resolution mode selected */    
    if (matrix->increase_resolution)  
    {   
        ram->get_position_add_val[0] = 2;        
        ram->get_position_add_val[1] = 6;
        ram->get_position_add_val[2] = 1;
        ram->get_position_add_val[3] = 4;
        ram->get_position_add_val[4] = 2;
        ram->get_position_add_val[5] = 2;
    }
    else
    {   
        ram->get_position_add_val[0] = 1;    
        ram->get_position_add_val[1] = 3;
        ram->get_position_add_val[2] = 0;
        ram->get_position_add_val[3] = 2;
        ram->get_position_add_val[4] = 1;
        ram->get_position_add_val[5] = 1;        
    }
    return NT_SUCCESS;
}

static int32_t _nt_control_matrix_process(struct nt_control_data *control)
{
    NT_ASSERT(control != NULL); 
    NT_ASSERT(control->rom->interface == &nt_control_matrix_interface);
    
    struct nt_control_matrix_data *ram = control->data.matrix;
    /* Retype to unconst for gestures recognition from both ram and rom */
    struct nt_control_matrix *rom = (struct nt_control_matrix *)control->rom->control_params.matrix;

    NT_ASSERT(ram != NULL); 
    NT_ASSERT(rom != NULL);     
        
    if (!_nt_control_get_flag(control, NT_CONTROL_EN_FLAG)) 
    {   return NT_FAILURE; /* not enabled or data not ready */
    }
    
    if (_nt_control_get_flag(control, NT_CONTROL_NEW_DATA_FLAG))
    {   /* touchpad electode state for matrix processing */ 
        uint32_t state = _nt_control_get_electrodes_state(control) & control->data.matrix->touchpad_mask;
        uint16_t state_x, state_y;
        
        /*** ONE OR MORE TOUCHPAD ELECTRODE CHANGED ***/
        /* Can detect position loss of the finger or one finger movement only,
        and recognize the release event.*/
        if (state ^ ram->last_electode_states)     
        {   /* Store last electrode states, x and y together. */
            ram->last_electode_states = state;
            
            /*** COVER ***/     
            if (state == (uint32_t)ram->touchpad_mask)  /* all touchpad electrode are active */
            {   _nt_control_set_flag(control, NT_MATRIX_GET_CONTROL_GESTURE_FLAG);
                _nt_control_matrix_invoke_callback(control, NT_MATRIX_GET_CONTROL_GESTURE,NULL, NT_MATRIX_COVER);                                   
                _nt_control_clear_flag(control, NT_CONTROL_NEW_DATA_FLAG); /* data processed */
                return NT_SUCCESS;
            }           
            
            /*** X / Y POSITION MUTUAL ***/
            if (rom->sensing_mode)
            {   /* x, y states for mutual-capacitance mode */
                uint16_t line_segment;
                uint16_t line;
                state_x = 0; 
                state_y = 0;  
                /* transfer state mutual value (each bit represents one mutual electrode) to x and y coordinates */ 
                /* line to line calculations */
                for (line = 0; line < rom->touchpad_size_vertical; line++)  
                {   /* cut current state to line segments by right shifts AND operation */
                    line_segment = (state >> (line << 2)) & control->data.matrix->touchpad_mask_horizontal; 
                    state_x |= line_segment;     /* store current state x values for each line */
                    if (line_segment >= 1)       /* store current state y values for line with pressed key */
                    {   state_y |= 1 << line; 
                    }
                }
            }       
            else  /*** X / Y POSITION SELF-CAP ***/
            {   /* x, y states for self-capacitance mode */
                /* Separate each axix to own quantities for simple calculations */              
                state_x = state & ram->touchpad_mask_horizontal;
                state_y = state >> rom->touchpad_size_horizontal;
            }           
            
            /*** TOUCH OR MOVEMENT ***/   
            /* At least one finger is touched or finger is moving. At least one position with x and y values is recognized.*/    
            if (state_x && state_y)
            {   /* Detect position of one or two finger in x and y coordinates. */
                _nt_control_matrix_get_position(&ram->actual_position[0].x, &ram->actual_position[1].x,
                                                state_x, &ram->get_position_add_val[0]);
                _nt_control_matrix_get_position(&ram->actual_position[0].y, &ram->actual_position[1].y,
                                                state_y, &ram->get_position_add_val[0]);               
                /* fill in not complete position for second finger if at lest one component is zero)*/
                if ((ram->actual_position[1].x != 0 ) && (ram->actual_position[1].y == 0))                    
                    ram->actual_position[1].y = ram->actual_position[0].y ;  /* copy y from first finger (shared one) */  
                else if ((ram->actual_position[1].x == 0) && (ram->actual_position[1].y != 0))
                    ram->actual_position[1].x = ram->actual_position[0].x ;  /* copy x from first finger (shared one) */             
                
                /*** TOUCH OR MOVEMENT - UNLOCK GESTURE BUFFERING INITIALIZATINON & DOUBLE TAB RECOGNITION ***/
                if (ram->buffer_position == 0)
                {   /* CONTROL GESTURE DELAY */
                    if (_nt_control_get_flag(control, NT_MATRIX_GET_CONTROL_GESTURE_FLAG))
                    {   /* Control gesture recognized, after push up hand after cover event the electrodes are changing states 
                           randomly therefore some delay is inserted before control flag clear and then matric processes continue */       
                        uint32_t counter = nt_system_get_time_counter(); 
                        if (counter > ram->double_tap_next_time)  /* Get time counter */
                        {   _nt_control_clear_flag(control, NT_MATRIX_GET_CONTROL_GESTURE_FLAG); /* clear flag after delay */
                        }
                        _nt_control_clear_flag(control, NT_CONTROL_NEW_DATA_FLAG); /* data processed */
                        return NT_SUCCESS;                                         /* jump to avoid matrix processes */
                    }
                    _nt_control_clear_flag(control, NT_MATRIX_RELEASE_FLAG); 
                    _nt_control_set_flag  (control, NT_MATRIX_TOUCH_FLAG); 
                    _nt_control_matrix_clean(&ram->buffer);
                    
                    /*** TOUCH OR MOVEMENT - DOUBLE TAB RECOGNITION */ 
                    if (nt_system_get_time_counter() <= ram->double_tap_next_time)
                    {   /* Check if previous and current position are the same plus minus one position */ 
                        if ((ram->actual_position[0].x>>2 == ram->touch_position[0].x>>2) &&
                            (ram->actual_position[0].y>>2 == ram->touch_position[0].y>>2))
                        {   /* delete second finger - only one finger double tab is recognized */                               
                            ram->touch_position[1].x = 0; 
                            ram->touch_position[1].y = 0;
                            _nt_control_set_flag(control, NT_MATRIX_GET_CONTROL_GESTURE_FLAG);
                            _nt_control_matrix_invoke_callback(control, NT_MATRIX_GET_CONTROL_GESTURE,
                                                               &ram->touch_position, NT_MATRIX_DOUBLE_TAP);                                        
                            _nt_control_clear_flag(control, NT_CONTROL_NEW_DATA_FLAG); /* data processed */
                            return NT_SUCCESS;
                        }
                    } 
                    else /*** TOUCH OR MOVEMENT - DOUBLE NOT RECOGNIZED */ 
                    {   _nt_control_matrix_copy (&ram->touch_position, &ram->actual_position);                       
                        _nt_control_matrix_invoke_callback(control, NT_MATRIX_TOUCH, 
                                                           &ram->touch_position, ram->recognized_gesture); 
                    }
                } 
                else  /*** TOUCH OR MOVEMENT - UNLOCK GESTURE BUFFERING PROCESSES OR MOVEMENT ***/
                {   _nt_control_clear_flag(control, NT_MATRIX_TOUCH_FLAG);
                    _nt_control_clear_flag(control, NT_MATRIX_GET_UNLOCK_GESTURE_FLAG);
                    _nt_control_clear_flag(control, NT_MATRIX_SET_UNLOCK_GESTURE_FLAG);   
                    if (_nt_control_get_flag(control, NT_MATRIX_GET_CONTROL_GESTURE_FLAG))
                    {   ram->buffer_position = 0;
                        ram->double_tap_next_time = nt_system_get_time_counter() + rom->double_tap_interval;
                        _nt_control_clear_flag(control, NT_CONTROL_NEW_DATA_FLAG); /* data processed */
                        return NT_SUCCESS; 
                    }
                    else
                    {   _nt_control_set_flag  (control, NT_MATRIX_MOVEMENT_FLAG); 
                        _nt_control_matrix_invoke_callback(control, NT_MATRIX_MOVEMENT,
                                                           &ram->actual_position, ram->recognized_gesture);                    
                    }
                }
                if (ram->buffer_position < 64) /* Store the cached gesture which not exceed the buffer size */
                { /* store recognized positions to buffer */
                    ram->buffer[0].x |= ((uint64_t)ram->actual_position[0].x << ram->buffer_position);
                    ram->buffer[0].y |= ((uint64_t)ram->actual_position[0].y << ram->buffer_position); 
                    ram->buffer[1].x |= ((uint64_t)ram->actual_position[1].x << ram->buffer_position);
                    ram->buffer[1].y |= ((uint64_t)ram->actual_position[1].y << ram->buffer_position);                 
                    ram->buffer_position += 4;
                }
                else        /*** TOUCH OR MOVEMENT - UNLOCK GESTURE IS TOO LONG FOR BUFFER ***/               
                {   _nt_control_matrix_clean(&ram->buffer); 
                    ram->buffer_position = 0;
                    
                    _nt_control_matrix_copy (&ram->release_position, &ram->actual_position);
                    
                    _nt_control_matrix_invoke_callback(control, NT_MATRIX_RELEASE, 
                                                       &ram->release_position, ram->recognized_gesture);
                }                 
            }  /*** END TOUCH OR MOVEMENT ***/ 
            /*** RELEASE ***/
            else if (!(state_x || state_y))
            {   uint16_t n;
                uint16_t temp;
                uint16_t temp1 = 0;
                 
                _nt_control_matrix_copy (&ram->release_position, &ram->actual_position);
                
                _nt_control_clear_flag(control, NT_MATRIX_MOVEMENT_FLAG);
                /* Set time for control gesture delay (avoid random electrode changing during hand pushing up after cover */
                ram->double_tap_next_time = nt_system_get_time_counter() +
                     rom->double_tap_interval; 
                
                if (_nt_control_get_flag(control, NT_MATRIX_GET_CONTROL_GESTURE_FLAG))
                {   ram->buffer_position = 0;  /*Init unlock gesture buffer when control gesture recognized */
                    _nt_control_clear_flag(control, NT_CONTROL_NEW_DATA_FLAG); /* data processed */
                    return NT_SUCCESS;                                         /* jump out gesture recognized */
                }
                else
                {   _nt_control_set_flag  (control, NT_MATRIX_RELEASE_FLAG); 
                }    
                
                /*** RELEASE - STORING UNLOCK GESTURE FROM BUFFER ***/
                /* Unlock gesture recording defined by selected gesture number, if zero value no record */
                if (ram->select_gesture != 0)
                {   /* Gesture for storing must be enough long for reliable recognition and the gesture size must not exceed buffer size */
                    if ((ram->buffer[0].x > 0xffff) && (ram->buffer[0].y > 0xffff) && (ram->buffer_position < 64))
                    {   /* Zero value means no gesture storing therefore gesture minus one */
                        uint16_t select_gesture = ram->select_gesture-1;
                        
                        ram->two_points_gesture_flag &= ~(0x1 << (select_gesture));  /* clear two finger gesture flag */                 
                        _nt_control_set_flag(control, NT_MATRIX_SET_UNLOCK_GESTURE_FLAG);   /* set set_gesture flag */ 
                        
                        ram->gesture[select_gesture][0].x = ram->buffer[0].x;        /* store first x finger coordinate */
                        ram->gesture[select_gesture][0].y = ram->buffer[0].y;        /* store first y finger coordinate */  
                        /* store second x and y finger coordinates */                         
                        ram->gesture[select_gesture][1].x = ram->buffer[1].x;        /* store second x finger coordinate */
                        ram->gesture[select_gesture][1].y = ram->buffer[1].y;                        
                        
                        /* Storing second finger movement */
                        if (ram->buffer[1].x || ram->buffer[1].y)                    /* if second finger has data */
                        {   ram->two_points_gesture_flag |= 0x1 << (select_gesture); /* set two finger gesture flag */ 
                        } 
                        _nt_control_set_flag(control, NT_MATRIX_RELEASE_SET_GESTURE);
                        _nt_control_matrix_invoke_callback(control, NT_MATRIX_RELEASE_SET_GESTURE,
                                                           &ram->release_position, ram->select_gesture);
                        ram->select_gesture = 0;                                   /* clear gesture number value */
                    }
                }
                
                /*** RELEASE - UNLOCK GESTURE RECOGNITION ***/
                /* Unlock gesture recognition algorithm */
                ram->recognized_gesture = 0;                            /* init by setting unknown gesture value */
                temp = rom->dissimilarity_max;           
                /* Compare buffered gesture with stored gestures, one by one */
                for (n=0; n < NT_MATRIX_UNLOCK_GESTURES; n++)  
                {
                    ram->gesture_diff[0][n] = _nt_control_matrix_gesture_recognition(&ram->buffer[0],
                                              (ram->gesture_memory == 0) ? &rom->gesture[n][0] : &ram->gesture[n][0], 0);
                    /* In case of two point gestures compare the second finger movement as well */ 
                    if (ram->two_points_gesture_flag & (0x1 << n)) 
                    {   /* Adding dissimilarity for second finger */
                        ram->gesture_diff[0][n] += _nt_control_matrix_gesture_recognition(&ram->buffer[1], 
                                                                                          &ram->gesture[n][1], 1);                      
                        /* Make average of dissimilaties from first and second finger */
                        ram->gesture_diff[0][n] >>= 1; 
                    }
                    
                    ram->gesture_diff[1][n] = _nt_control_matrix_gesture_recognition_norm(&ram->buffer[0],
                                                                                          &ram->gesture[n][0], 0);
                    /* In case of two point gestures compare the second finger movement as well */ 
                    if (ram->two_points_gesture_flag & (0x1 << n)) 
                    {   /* Adding dissimilarity for second finger */
                        ram->gesture_diff[1][n] += _nt_control_matrix_gesture_recognition_norm(&ram->buffer[1], 
                                                                                               &ram->gesture[n][1], 1);                      
                        /* Make average of dissimilates from first and second finger */
                        ram->gesture_diff[1][n] >>= 1; 
                    }
                                               
                    ram->gesture_diff[2][n] = _nt_control_matrix_gesture_recognition_square(&ram->buffer[0],
                                                                                            &ram->gesture[n][0], 0);
                    /* In case of two point gestures compare the second finger movement as well */ 
                    if (ram->two_points_gesture_flag & (0x1 << n)) 
                    {   /* Adding dissimilarity for second finger */
                        ram->gesture_diff[2][n] += _nt_control_matrix_gesture_recognition_square(&ram->buffer[1], 
                                                                                                 &ram->gesture[n][1], 1);                      
                        /* Make average of dissimilaties from first and second finger */
                        ram->gesture_diff[2][n] >>= 1; 
                    }
                    
                    ram->gesture_diff[3][n] = _nt_control_matrix_gesture_recognition_angle(&ram->buffer[0],
                                                                                            &ram->gesture[n][0], 0);
                    /* In case of two point gestures compare the second finger movement as well */ 
                    if (ram->two_points_gesture_flag & (0x1 << n)) 
                    {   /* Adding dissimilarity for second finger */
                        ram->gesture_diff[3][n] += _nt_control_matrix_gesture_recognition_angle(&ram->buffer[1], 
                                                                                                &ram->gesture[n][1], 1);                      
                        /* Make average of dissimilates from first and second finger */
                        ram->gesture_diff[3][n] >>= 1; 
                    }
                    
                    ram->gesture_diff[4][n] = _nt_control_matrix_gesture_recognition_XY(&ram->buffer[0],
                                                                                        &ram->gesture[n][0], 0);
                    /* In case of two point gestures compare the second finger movement as well */ 
                    if (ram->two_points_gesture_flag & (0x1 << n)) 
                    {   /* Adding dissimilarity for second finger */
                        ram->gesture_diff[4][n] += _nt_control_matrix_gesture_recognition_XY(&ram->buffer[1], 
                                                                                             &ram->gesture[n][1], 1);                      
                        /* Make average of dissimilaties from first and second finger */
                        ram->gesture_diff[4][n] >>= 1; 
                    }
                    
                    /* Dissimilates is under determine level - gesture recognized, if gesture is similar to 
                       more stored gestures the more similar gesture is used, avoid recognition after storing */
                      if (  (ram->gesture_diff[2][n] < temp) && !_nt_control_get_flag(control, NT_MATRIX_RELEASE_SET_GESTURE)) 
                        {   temp = ram->gesture_diff[2][n];
                            ram->recognized_gesture = n + 1;
                        }
                        else 
                        {   if (ram->gesture_diff[2][n] == temp)   /* gestures with same dissimilarity */
                                ram->recognized_gesture = 0;       /* no gesture recognized */  
                        }
                }
                ram->buffer_position = 0; 

                /*** RELEASE - UNLOCK GESTURE NOT RECOGNIZED - SIMILAR GESTURES ***/
                /* Avoiding doubts when two or more gestures have similar dissimilarity */
                if (ram->recognized_gesture > 0)
                {   /* adding dissimilarity others to recognized gesture to assign new level*/ 
                    uint16_t lowDissCount = 0;       /* Counter for low dissimilarity */
                    for (temp1 = 0; temp1 < NT_MATRIX_UNLOCK_GESTURES; temp1++)
                    {   /* new level is compared with all gesture */ 
                        if ((int32_t)ram->gesture_diff[2][temp1] < (int32_t)temp)                               
                        {   lowDissCount++;       /* incrementing counter when dissimilarity is under new level */
                        }    
                    }
                    if (lowDissCount > 1)         /* two gestures are under level (one is always - recognized gesture)*/                          
                    {   _nt_control_clear_flag(control, NT_MATRIX_GET_UNLOCK_GESTURE_FLAG); /* Clear get gesture flag */
                        ram->recognized_gesture = 0;   /* clean gesture information */ 
                    }
                    else 
                    {   _nt_control_set_flag(control, NT_MATRIX_GET_UNLOCK_GESTURE_FLAG);   /* Set get gesture flag */                 
                        /* Callback function with recognized or unrecognized parameter. */
                        _nt_control_matrix_invoke_callback(control, NT_MATRIX_RELEASE_GET_GESTURE,
                                                           &ram->release_position, ram->recognized_gesture);
                    }
                }
                else if (ram->select_gesture == 0)    /*** RELEASE - SWIPE GESTURE RECOGNITION - UNLOCK GESTURE NOT RECOGNIZED ***/
                {   uint16_t swipe_diff = _nt_control_matrix_horiz_vertic_recognition(&ram->buffer[0], 0);
                    /* Check if drawn track is enough horizontal or vertical for swipe gestures, skip after gesture stored*/
                    if (swipe_diff == 0 && !_nt_control_get_flag(control, NT_MATRIX_RELEASE_SET_GESTURE))
                    {   /* calculate the swipe direction */
                        int16_t dir_diff_x = (int16_t)ram->release_position[0].x - (int16_t)ram->touch_position[0].x;
                        int16_t dir_diff_y = (int16_t)ram->release_position[0].y - (int16_t)ram->touch_position[0].y;
                        _nt_control_set_flag(control, NT_MATRIX_GET_CONTROL_GESTURE_FLAG);
                        /* evaluate swipe type according to direction */
                        if (dir_diff_y < dir_diff_x && dir_diff_y > -dir_diff_x && dir_diff_x > 0)
                        {   _nt_control_matrix_invoke_callback(control, NT_MATRIX_GET_CONTROL_GESTURE,
                                                               &ram->release_position, NT_MATRIX_SWIPE_RIGHT);
                        }
                        else if (dir_diff_y < -dir_diff_x && dir_diff_y > dir_diff_x && dir_diff_x < 0)                            
                        {   _nt_control_matrix_invoke_callback(control, NT_MATRIX_GET_CONTROL_GESTURE,
                                                               &ram->release_position, NT_MATRIX_SWIPE_LEFT);
                        }
                        else if (dir_diff_x < dir_diff_y && dir_diff_x > -dir_diff_y && dir_diff_y > 0)            
                        {   _nt_control_matrix_invoke_callback(control, NT_MATRIX_GET_CONTROL_GESTURE,
                                                               &ram->release_position, NT_MATRIX_SWIPE_UP); 
                        }
                        else if (dir_diff_x < -dir_diff_y && dir_diff_x > dir_diff_y && dir_diff_y < 0) 
                        {   _nt_control_matrix_invoke_callback(control, NT_MATRIX_GET_CONTROL_GESTURE,
                                                               &ram->release_position, NT_MATRIX_SWIPE_DOWN);
                        }   
                        _nt_control_matrix_clean(&ram->buffer); 

                    }
                    else    /*** RELEASE - NO GESTURE RECOGNIZED ***/
                    {   /* activate control gesture recognition after storing */
                        _nt_control_clear_flag(control, NT_MATRIX_RELEASE_SET_GESTURE);  
                        _nt_control_matrix_invoke_callback(control, NT_MATRIX_RELEASE,
                                                           &ram->release_position, ram->recognized_gesture); 
                    }
                }     
            } 
            else  /*** UNKNOWN POSITION - TOUCH ON X OR Y ***/
            {   _nt_control_clear_flag(control, NT_MATRIX_RELEASE_FLAG);             /* Clear release flag */
            }
        }    
    _nt_control_clear_flag(control, NT_CONTROL_NEW_DATA_FLAG); /* data processed */
    }
    return NT_SUCCESS;
}

void _nt_control_matrix_clean (nt_points64 *buffer)
{   uint64_t *buffer1 = (uint64_t*)buffer;
    uint8_t n=4;
    while(n--)
    {   
        *buffer1 = 0; 
        buffer1++;
    }
}

void _nt_control_matrix_copy (nt_points16 *buffer_copy, nt_points16 *buffer_orig)
{   uint16_t *buffer_copy1 = (uint16_t*)buffer_copy;
    uint16_t *buffer_orig1 = (uint16_t*)buffer_orig;
    uint8_t n=4;
    while(n--)
    {   
        *buffer_copy1++ = *buffer_orig1++; 
    }
}
