# -*- coding: utf-8 -*-
"""
Created on Mon Jun 17 17:04:54 2019

@author: X1_Yoga
"""

import forwardkin as fw
import numpy as np

np.random.seed(42)
sizeArr = 1000000
myX = np.zeros((sizeArr,6))

qLimJ1 = np.array( [-170, 170])
qLimJ2 = np.array([-190, 45])
qLimJ3 = np.array([-120, 156])
qLimJ4 = np.array( [-185, 185])
qLimJ5 = np.array([-120, 120])
qLimJ6 = np.array([-350, 350])

j1 = ( np.random.uniform(qLimJ1[0], qLimJ1[1], size=(sizeArr)) )
j2 = ( np.random.uniform(qLimJ2[0], qLimJ2[1], size=(sizeArr)) )
j3 = ( np.random.uniform(qLimJ3[0], qLimJ3[1], size=(sizeArr)) )
j4 = ( np.random.uniform(qLimJ4[0], qLimJ4[1], size=(sizeArr)) )
j5 = ( np.random.uniform(qLimJ5[0], qLimJ5[1], size=(sizeArr)) )
j6 = ( np.random.uniform(qLimJ6[0], qLimJ6[1], size=(sizeArr)) )

for num in range(sizeArr):
    myX[num,:] = fw.forwardkin([j1[num],j2[num],j3[num],j4[num],j5[num],j6[num]])



maxX_Old = myX[:,0].max()
maxY_Old = myX[:,1].max()
maxZ_Old = myX[:,2].max()
maxA_Old = myX[:,3].max()
maxB_Old = myX[:,4].max()
maxC_Old = myX[:,5].max()

minX_Old = myX[:,0].min()
minY_Old = myX[:,1].min()
minZ_Old = myX[:,2].min()
minA_Old = myX[:,3].min()
minB_Old = myX[:,4].min()
minC_Old = myX[:,5].min()


for k in range(5):

    j1 = ( np.random.uniform(qLimJ1[0], qLimJ1[1], size=(sizeArr)) )
    j2 = ( np.random.uniform(qLimJ2[0], qLimJ2[1], size=(sizeArr)) )
    j3 = ( np.random.uniform(qLimJ3[0], qLimJ3[1], size=(sizeArr)) )
    j4 = ( np.random.uniform(qLimJ4[0], qLimJ4[1], size=(sizeArr)) )
    j5 = ( np.random.uniform(qLimJ5[0], qLimJ5[1], size=(sizeArr)) )
    j6 = ( np.random.uniform(qLimJ6[0], qLimJ6[1], size=(sizeArr)) )

    for num in range(sizeArr):
        myX[num,:] = fw.forwardkin([j1[num],j2[num],j3[num],j4[num],j5[num],j6[num]])

    maxX = myX[:,0].max()
    maxY = myX[:,1].max()
    maxZ = myX[:,2].max()
    maxA = myX[:,3].max()
    maxB = myX[:,4].max()
    maxC = myX[:,5].max()

    minX = myX[:,0].min()
    minY = myX[:,1].min()
    minZ = myX[:,2].min()
    minA = myX[:,3].min()
    minB = myX[:,4].min()
    minC = myX[:,5].min()

    if (maxX_Old> maxX):
        maxX_Old = maxX

    if (maxY_Old> maxY):
        maxY_Old = maxY

    if (maxZ_Old> maxZ):
        maxZ_Old = maxZ

    if (maxA_Old> maxA):
        maxA_Old = maxA

    if (maxB_Old> maxB):
        maxB_Old = maxB

    if (maxC_Old> maxC):
        maxC_Old = maxC


print(maxX_Old)
print(maxY_Old)
print(maxZ_Old)
print(maxA_Old)
print(maxB_Old)
print(maxC_Old)

print(minX_Old)
print(minY_Old)
print(minZ_Old)
print(minA_Old)
print(minB_Old)
print(minC_Old)