Start point matrix work. Make functions more C like