#!/usr/bin/env python

from __future__ import print_function, absolute_import # NEED TO STAY AS TOP IMPORT
import vtk
import sys
import math
from vmtk import vtkvmtk

def ReadPolyData(filename):
   reader = vtk.vtkXMLPolyDataReader()
   reader.SetFileName(filename)
   reader.Update()
   return reader.GetOutput()

def WritePolyData(input,filename):
   writer = vtk.vtkXMLPolyDataWriter()
   writer.SetFileName(filename)
   writer.SetInputData(input)
   writer.Write()

def ExtractCylindricInterpolationVoronoiDiagram(cellId,pointId,cylinderRadius,voronoi,centerlines):
   isInside = 0

   if (cellId == 0):
      cylinderTop = centerlines.GetPoint(pointId)
      cylinderCenter = centerlines.GetPoint(pointId-interpolationHalfSize)
      cylinderBottom = centerlines.GetPoint(pointId-interpolationHalfSize*2)
   else:
      cylinderTop = centerlines.GetPoint(pointId)
      cylinderCenter = centerlines.GetPoint(pointId+interpolationHalfSize)
      cylinderBottom = centerlines.GetPoint(pointId+interpolationHalfSize*2)

   interpolationDataset = vtk.vtkPolyData()
   interpolationDatasetPoints = vtk.vtkPoints()
   interpolationDatasetCellArray = vtk.vtkCellArray()

   maskArray = vtk.vtkIntArray()
   maskArray.SetNumberOfComponents(1)
   maskArray.SetNumberOfTuples(voronoi.GetNumberOfPoints())
   maskArray.FillComponent(0,0)

   for i in range(voronoi.GetNumberOfPoints()):
      point = voronoi.GetPoint(i)
      isInside = IsPointInsideInterpolationCylinder(point,cylinderTop,cylinderCenter,cylinderBottom,cylinderRadius)

      if (isInside == 1):
         maskArray.SetTuple1(i,1)

   numberOfInterpolationPoints = ComputeNumberOfMaskedPoints(maskArray)

   radiusArray = vtk.vtkDoubleArray()
   radiusArray.SetNumberOfComponents(1)
   radiusArray.SetNumberOfTuples(numberOfInterpolationPoints)
   radiusArray.SetName(radiusArrayName)
   radiusArray.FillComponent(0,0.0)

   count = 0
   for i in range(voronoi.GetNumberOfPoints()):
      value = maskArray.GetTuple1(i)
      if (value == 1):
         interpolationDatasetPoints.InsertNextPoint(voronoi.GetPoint(i))
         interpolationDatasetCellArray.InsertNextCell(1)
         interpolationDatasetCellArray.InsertCellPoint(count)
         radius = voronoi.GetPointData().GetArray(radiusArrayName).GetTuple1(i)
         radiusArray.SetTuple1(count,radius)
         count +=1

   interpolationDataset.SetPoints(interpolationDatasetPoints)
   interpolationDataset.SetVerts(interpolationDatasetCellArray)
   interpolationDataset.GetPointData().AddArray(radiusArray)

   return interpolationDataset

def IsPointInsideInterpolationCylinder(x,t,c,b,r):
  halfheigth = math.sqrt(vtk.vtkMath.Distance2BetweenPoints(b,t))/2.0

  xc = [0.0,0.0,0.0]
  tb = [0.0,0.0,0.0]

  xc[0] = x[0]-c[0]
  xc[1] = x[1]-c[1]
  xc[2] = x[2]-c[2]

  tb[0] = t[0]-b[0]
  tb[1] = t[1]-b[1]
  tb[2] = t[2]-b[2]

  xcnorm = vtk.vtkMath.Norm(xc)
  vtk.vtkMath.Normalize(xc)
  vtk.vtkMath.Normalize(tb)

  alpha = math.acos(vtk.vtkMath.Dot(xc,tb))

  parallelxc = xcnorm*math.cos(alpha)
  perpendicularxc =  xcnorm*math.sin(alpha)

  thetamin = math.atan(r/halfheigth)
  thetamax = thetamin + (math.pi - 2*thetamin)

  inside = 0
  if (thetamin<=alpha<=thetamax):
    if (abs(perpendicularxc)<=r): inside = 1
  else:
    if (abs(parallelxc)<=halfheigth): inside = 1

  return inside

def ComputeNumberOfMaskedPoints(dataArray):
   numberOfPoints = 0
   for i  in range(dataArray.GetNumberOfTuples()):
      value = dataArray.GetTuple1(i)
      if (value ==1): numberOfPoints +=1
   return numberOfPoints

def VoronoiDiagramInterpolation(interpolationcellid,id0,id1,voronoiDataset0,voronoiDataset1,centerlines,direction):
    print('Interpolating cells ',id0,id1)
    cellLine = ExtractLine(interpolationcellid,centerlines)

    startPoint = clippingPoints.GetPoint(id0)
    endPoint = clippingPoints.GetPoint(id1)

    startId = cellLine.FindPoint(startPoint)
    endId = cellLine.FindPoint(endPoint)

    if (direction == 1):
       gapStartId = startId + 1
       gapEndId = endId - 1
       arrivalId = gapEndId + 1
       endSavingInterval = gapEndId + 1
       step = 1
    else:
       gapStartId = startId - 1
       gapEndId = endId + 1
       arrivalId = gapEndId - 1
       endSavingInterval = gapEndId - 1
       step = -1

    numberOfGapPoints = int(math.fabs(gapEndId - gapStartId)) + 1
    numberOfInterpolationPoints = voronoiDataset0.GetNumberOfPoints()
    numberOfCenterlinesPoints = cellLine.GetNumberOfPoints()
    numberOfAddedPoints = numberOfGapPoints*numberOfInterpolationPoints
    print('from id ',gapStartId,'to id ',gapEndId,'; number of points added ',numberOfAddedPoints)

    finalNewVoronoiPoints = vtk.vtkPoints()
    cellArray = vtk.vtkCellArray()
    finalRadiusArray = vtk.vtkDoubleArray()
    finalRadiusArray.SetNumberOfComponents(1)
    finalRadiusArray.SetNumberOfTuples(numberOfAddedPoints)
    finalRadiusArray.SetName(radiusArrayName)
    finalRadiusArray.FillComponent(0,0.0)
    count = 0

    for i in range(numberOfInterpolationPoints):
       voronoiPoint = voronoiDataset0.GetPoint(i)
       voronoiPointRadius = voronoiDataset0.GetPointData().GetArray(radiusArrayName).GetTuple1(i)

       centerlinePointLocator = vtk.vtkPointLocator()
       centerlinePointLocator.SetDataSet(cellLine)
       centerlinePointLocator.BuildLocator()

       closestPointId = centerlinePointLocator.FindClosestPoint(voronoiPoint)
       closestPoint = cellLine.GetPoint(closestPointId)

       voronoiVector = [0.0,0.0,0.0]
       voronoiVector[0] = voronoiPoint[0] - closestPoint[0]
       voronoiVector[1] = voronoiPoint[1] - closestPoint[1]
       voronoiVector[2] = voronoiPoint[2] - closestPoint[2]
       voronoiVectorNorm = vtk.vtkMath.Norm(voronoiVector)

       rotationAngle = ComputeVoronoiVectorToCenterlineAngle(closestPointId,voronoiVector,cellLine)

       PTPoints = vtk.vtkPoints()
       for j in range(closestPointId,arrivalId,step):
          localtangent = [0.0,0.0,0.0]
          localnormal = [0.0,0.0,0.0]
          newVoronoiVector = [0.0,0.0,0.0]
          newVoronoiPoint = [0.0,0.0,0.0]

          transform = vtk.vtkTransform()

          point0 = [0.0,0.0,0.0]
          point0 = cellLine.GetPoint(j)

          if (j<numberOfCenterlinesPoints-1):
             point1 = [0.0,0.0,0.0]
             cellLine.GetPoint(j+1,point1)
             localtangent[0] += point1[0] - point0[0]
             localtangent[1] += point1[1] - point0[1]
             localtangent[2] += point1[2] - point0[2]
          if (j>0):
             point2 = [0.0,0.0,0.0]
             cellLine.GetPoint(j-1,point2)
             localtangent[0] += point0[0] - point2[0]
             localtangent[1] += point0[1] - point2[1]
             localtangent[2] += point0[2] - point2[2]

          localnormal = cellLine.GetPointData().GetArray(parallelTransportNormalsArrayName).GetTuple3(j)
          localnormaldot = vtk.vtkMath.Dot(localtangent,localnormal)

          localtangent[0] -= localnormaldot*localnormal[0]
          localtangent[1] -= localnormaldot*localnormal[1]
          localtangent[2] -= localnormaldot*localnormal[2]
          vtk.vtkMath.Normalize(localtangent)

          transform.RotateWXYZ(rotationAngle,localtangent)
          transform.TransformNormal(localnormal,newVoronoiVector)
          vtk.vtkMath.Normalize(newVoronoiVector)

          newVoronoiPoint[0] = point0[0] + voronoiVectorNorm*newVoronoiVector[0]
          newVoronoiPoint[1] = point0[1] + voronoiVectorNorm*newVoronoiVector[1]
          newVoronoiPoint[2] = point0[2] + voronoiVectorNorm*newVoronoiVector[2]

          PTPoints.InsertNextPoint(newVoronoiPoint)
       numberOfPTPoints = PTPoints.GetNumberOfPoints()

       lastPTPoint = PTPoints.GetPoint(PTPoints.GetNumberOfPoints()-1)

       voronoiPointLocator = vtk.vtkPointLocator()
       voronoiPointLocator.SetDataSet(voronoiDataset1)
       voronoiPointLocator.BuildLocator()

       arrivalVoronoiPointId = voronoiPointLocator.FindClosestPoint(lastPTPoint)
       arrivalVoronoiPoint = voronoiDataset1.GetPoint(arrivalVoronoiPointId)
       arrivalVoronoiPointRadius = voronoiDataset1.GetPointData().GetArray(radiusArrayName).GetTuple1(arrivalVoronoiPointId)

       arrivalCenterlinePointLocator = vtk.vtkPointLocator()
       arrivalCenterlinePointLocator.SetDataSet(cellLine)
       arrivalCenterlinePointLocator.BuildLocator()

       arrivalCenterlineClosestPointId = arrivalCenterlinePointLocator.FindClosestPoint(arrivalVoronoiPoint)
       arrivalCenterlineClosestPoint = cellLine.GetPoint(arrivalCenterlineClosestPointId)

       arrivalVoronoiVector = [0.0,0.0,0.0]
       arrivalVoronoiVector[0] = arrivalVoronoiPoint[0] - arrivalCenterlineClosestPoint[0]
       arrivalVoronoiVector[1] = arrivalVoronoiPoint[1] - arrivalCenterlineClosestPoint[1]
       arrivalVoronoiVector[2] = arrivalVoronoiPoint[2] - arrivalCenterlineClosestPoint[2]
       arrivalVoronoiVectorNorm = vtk.vtkMath.Norm(arrivalVoronoiVector)

       radiusArray = ComputeSpline(voronoiPointRadius,arrivalVoronoiPointRadius,numberOfPTPoints)
       vectorNormArray = ComputeSpline(voronoiVectorNorm,arrivalVoronoiVectorNorm, numberOfPTPoints)

       if (direction==1):
          pointsToGap = gapStartId - closestPointId
       else:
          pointsToGap = closestPointId - gapStartId

       pointId = pointsToGap
       for k in range(gapStartId,endSavingInterval,step):
          ptpoint = PTPoints.GetPoint(pointsToGap)
          clpoint = cellLine.GetPoint(k)

          vector = [0.0,0.0,0.0]
          vector[0] = ptpoint[0] - clpoint[0]
          vector[1] = ptpoint[1] - clpoint[1]
          vector[2] = ptpoint[2] - clpoint[2]
          vtk.vtkMath.Normalize(vector)

          norm = vectorNormArray.GetTuple1(pointsToGap)

          newvector = [0.0,0.0,0.0]
          newvector[0] = norm*vector[0]
          newvector[1] = norm*vector[1]
          newvector[2] = norm*vector[2]

          newpoint = [0.0,0.0,0.0]
          newpoint[0] = clpoint[0] + newvector[0]
          newpoint[1] = clpoint[1] + newvector[1]
          newpoint[2] = clpoint[2] + newvector[2]

          finalNewVoronoiPoints.InsertNextPoint(newpoint)
          cellArray.InsertNextCell(1)
          cellArray.InsertCellPoint(count)
          finalRadiusArray.SetTuple1(count,radiusArray.GetTuple1(pointsToGap))
          pointsToGap +=1
          count +=1

    return finalNewVoronoiPoints,finalRadiusArray

def ExtractLine(cellid,centerlines):
   cell = vtk.vtkGenericCell()
   centerlines.GetCell(cellid,cell)

   numberOfPoints = cell.GetNumberOfPoints()

   line = vtk.vtkPolyData()
   cellArray = vtk.vtkCellArray()
   cellArray.InsertNextCell(numberOfPoints)

   linePoints = cell.GetPoints()

   ptnArray = vtk.vtkDoubleArray()
   ptnArray.SetNumberOfComponents(3)
   ptnArray.SetNumberOfTuples(numberOfPoints)
   ptnArray.SetName(parallelTransportNormalsArrayName)
   ptnArray.FillComponent(0,0.0)
   ptnArray.FillComponent(1,0.0)
   ptnArray.FillComponent(2,0.0)

   for i in range(numberOfPoints):
      cellArray.InsertCellPoint(i)
      normal = centerlines.GetPointData().GetArray(parallelTransportNormalsArrayName).GetTuple3(cell.GetPointId(i))
      ptnArray.SetTuple3(i,normal[0],normal[1],normal[2])

   line.SetPoints(linePoints)
   line.SetLines(cellArray)
   line.GetPointData().AddArray(ptnArray)
   return line

def ComputeVoronoiVectorToCenterlineAngle(pointId,vector,centerline):
    point0 = [0.0,0.0,0.0]
    point1 = [0.0,0.0,0.0]
    point2 = [0.0,0.0,0.0]
    centerline.GetPoint(pointId,point0)
    centerline.GetPoint(pointId+1,point1)
    centerline.GetPoint(pointId-1,point2)

    tangent = [0.0,0.0,0.0]
    tangent[0] += point1[0]-point0[0]
    tangent[1] += point1[1]-point0[1]
    tangent[2] += point1[2]-point0[2]
    tangent[0] += point0[0]-point2[0]
    tangent[1] += point0[1]-point2[1]
    tangent[2] += point0[2]-point2[2]

    ptnnormal = centerline.GetPointData().GetArray(parallelTransportNormalsArrayName).GetTuple3(pointId)

    alpha = ComputeAngleBetweenVectors(ptnnormal,tangent,vector)
    return alpha

def ComputeAngleBetweenVectors(normal,tangent,vector):
    #compute the tangent component orthogonal to normal
    otangent = [0.0,0.0,0.0]
    normalDot = vtk.vtkMath.Dot(tangent,normal)
    otangent[0] = tangent[0] - normalDot*normal[0]
    otangent[1] = tangent[1] - normalDot*normal[1]
    otangent[2] = tangent[2] - normalDot*normal[2]
    vtk.vtkMath.Normalize(otangent)

    #compute the vector component orthogonal to otangent, i.e. parallel to normal
    vtk.vtkMath.Normalize(vector)
    ovector = [0.0,0.0,0.0]
    vectorDot = vtk.vtkMath.Dot(vector,otangent)
    ovector[0] = vector[0] - vectorDot*otangent[0]
    ovector[1] = vector[1] - vectorDot*otangent[1]
    ovector[2] = vector[2] - vectorDot*otangent[2]
    vtk.vtkMath.Normalize(ovector)

    theta = vtkvmtk.vtkvmtkMath.AngleBetweenNormals(normal,ovector)
    theta = vtk.vtkMath.DegreesFromRadians(theta)

    cross = [0.0,0.0,0.0]
    vtk.vtkMath.Cross(ovector,normal,cross)
    tangentDot = vtk.vtkMath.Dot(otangent,cross)
    if (tangentDot<0.0): theta = -1.0*theta
    angle = -theta
    return angle

def ComputeSpline(startValue,endValue,numberOfPoints):
   averageValue = (startValue + endValue)/2.0
   averageId = int(numberOfPoints/2)

   splineArray = vtk.vtkDoubleArray()
   splineArray.SetNumberOfComponents(1)
   splineArray.SetNumberOfTuples(numberOfPoints)
   splineArray.FillComponent(0,0.0)

   spline = vtk.vtkCardinalSpline()
   spline.AddPoint(float(0),startValue)
   spline.AddPoint(float(averageId),averageValue)
   spline.AddPoint(float(numberOfPoints),endValue)
   spline.Compute()

   for i in range(numberOfPoints):
      splineArray.SetTuple1(i,spline.Evaluate(float(i)))

   return splineArray

def InsertNewVoronoiPoints(oldDataset,newPoints,newArray):
   numberOfDatasetPoints = oldDataset.GetNumberOfPoints()
   numberOfNewPoints = newPoints.GetNumberOfPoints()
   numberOfNewVoronoiPoints = numberOfDatasetPoints + numberOfNewPoints

   newDataset = vtk.vtkPolyData()
   radiusArray = vtk.vtkDoubleArray()
   radiusArray.SetNumberOfComponents(1)
   radiusArray.SetNumberOfTuples(numberOfNewVoronoiPoints)
   radiusArray.SetName(radiusArrayName)
   radiusArray.FillComponent(0,0.0)
   cellArray = vtk.vtkCellArray()
   points = vtk.vtkPoints()

   for i in range(numberOfDatasetPoints):
      point = [0.0,0.0,0.0]
      oldDataset.GetPoint(i,point)
      points.InsertNextPoint(point)
      cellArray.InsertNextCell(1)
      cellArray.InsertCellPoint(i)

      value = oldDataset.GetPointData().GetArray(radiusArrayName).GetTuple1(i)
      radiusArray.SetTuple1(i,value)

   for i in range(numberOfNewPoints):
      point = [0.0,0.0,0.0]
      newPoints.GetPoint(i,point)
      points.InsertNextPoint(point)
      cellArray.InsertNextCell(1)
      cellArray.InsertCellPoint(numberOfDatasetPoints+i)

      value = newArray.GetTuple1(i)
      radiusArray.SetTuple1(numberOfDatasetPoints+i,value)

   newDataset.SetPoints(points)
   newDataset.SetVerts(cellArray)
   newDataset.GetPointData().AddArray(radiusArray)
   return newDataset

# -----------------------------------------------------------------------------------------

## Program:	clipvoronoidiagram.py
## Language:	Python
## Date:	2012/02/27
## Version:	1.0
## Application: Cerebral Aneurysms - Parent Vessel Reconstruction
## Author:	Marina Piccinelli

## Description:	Given the parallel transport normal system defined on the interpolated
##		centerlines computes the trajectory of Voronoi Diagram points from one end
##		to the other of the empty area in the clipped Voronoi Diagram. Saves a new
##		complete Voronoi Diagram and recalculates the model surface by merging of
##		maximal inscribed spheres. Recommended use of smoothed clipped Voronoi
##		Diagram.

# -----------------------------------------------------------------------------------------

#SOME COMMON VMTK DATA ARRAY NAMES
radiusArrayName = 'MaximumInscribedSphereRadius'
parallelTransportNormalsArrayName = 'ParallelTransportNormals'

#OPTIONS TO SET
interpolationHalfSize = 3
polyBallImageSize = [90,90,90]   #size of the image for the evaluation of the polyball function

#inputfilenames:
print('USAGE:')
print('      ./paralleltransportvoronoidiagram.py inputfilesDirectory caseID smoothVoronoi')
print('')

inputfiledirectory = sys.argv[1]
ID = sys.argv[2]
smoothedVoronoi = int(sys.argv[3])

print('Inputfiles Directory	', inputfiledirectory)
print('case ID			', ID)
print('')

interpolatedCenterlinesFilename = inputfiledirectory + '/' + ID + '/' + ID + '_interpolatedcl.vtp'
patchCenterlinesFilename        = inputfiledirectory + '/' + ID + '/' + ID + '_patchcl.vtp'
if smoothedVoronoi == 1:
   clippedVoronoiFilename       = inputfiledirectory + '/' + ID + '/' + ID + '_smoothclippedvoronoi.vtp'
else:
   clippedVoronoiFilename       = inputfiledirectory + '/' + ID + '/' + ID + '_clippedvoronoi.vtp'
clippingPointsFilename          = inputfiledirectory + '/' + ID + '/' + ID + '_clippingpoints.vtp'

#outputfilenames:
completeVoronoiFilename         = inputfiledirectory + '/' + ID + '/' + ID + '_completevoronoi.vtp'
surfaceFilename                 = inputfiledirectory + '/' + ID + '/' + ID + '_reconstructedmodel.vtp'

interpolatedCenterlines = ReadPolyData(interpolatedCenterlinesFilename)
patchCenterlines = ReadPolyData(patchCenterlinesFilename)
clippedVoronoi = ReadPolyData(clippedVoronoiFilename)
clippingPoints = ReadPolyData(clippingPointsFilename)

completeVoronoiDiagram = vtk.vtkPolyData()
completeVoronoiDiagram.DeepCopy(clippedVoronoi)

numberOfInterpolatedCenterlinesCells = interpolatedCenterlines.GetNumberOfCells()

for j in range(1,numberOfInterpolatedCenterlinesCells+1):
   interpolationCellId = j-1
   startId = 0
   endId = j

   startCell = vtk.vtkGenericCell()
   patchCenterlines.GetCell(startId,startCell)

   startCellPointId = startCell.GetPointId(startCell.GetNumberOfPoints()-1)
   startCellPoint = patchCenterlines.GetPoint(startCellPointId)
   startCellPointRadius = patchCenterlines.GetPointData().GetArray(radiusArrayName).GetTuple1(startCellPointId)
   startCellPointHalfRadius = startCellPointRadius/7.0

   startInterpolationDataset = ExtractCylindricInterpolationVoronoiDiagram(startId,startCellPointId,startCellPointRadius,clippedVoronoi,patchCenterlines)
   startHalfInterpolationDataset = ExtractCylindricInterpolationVoronoiDiagram(startId,startCellPointId,startCellPointHalfRadius,clippedVoronoi,patchCenterlines)

   endCell = vtk.vtkGenericCell()
   patchCenterlines.GetCell(endId,endCell)

   endCellPointId = endCell.GetPointId(0)
   endCellPoint = patchCenterlines.GetPoint(endCellPointId)
   endCellPointRadius = patchCenterlines.GetPointData().GetArray(radiusArrayName).GetTuple1(endCellPointId)
   endCellPointHalfRadius = endCellPointRadius/7.0

   endInterpolationDataset = ExtractCylindricInterpolationVoronoiDiagram(endId,endCellPointId,endCellPointRadius,clippedVoronoi,patchCenterlines)
   endHalfInterpolationDataset = ExtractCylindricInterpolationVoronoiDiagram(endId,endCellPointId,endCellPointHalfRadius,clippedVoronoi,patchCenterlines)

   newVoronoiPoints, newVoronoiPointsMISR = VoronoiDiagramInterpolation(interpolationCellId,startId,endId,startInterpolationDataset,endHalfInterpolationDataset,interpolatedCenterlines,1)
   completeVoronoiDiagram = InsertNewVoronoiPoints(completeVoronoiDiagram,newVoronoiPoints,newVoronoiPointsMISR)

   newVoronoiPoints, newVoronoiPointsMISR = VoronoiDiagramInterpolation(interpolationCellId,endId,startId,endInterpolationDataset,startHalfInterpolationDataset,interpolatedCenterlines,0)
   completeVoronoiDiagram = InsertNewVoronoiPoints(completeVoronoiDiagram,newVoronoiPoints,newVoronoiPointsMISR)

WritePolyData(completeVoronoiDiagram,completeVoronoiFilename)

print('Reconstructing Surface from Voronoi Diagram')
modeller = vtkvmtk.vtkvmtkPolyBallModeller()
modeller.SetInputData(completeVoronoiDiagram)
modeller.SetRadiusArrayName(radiusArrayName)
modeller.UsePolyBallLineOff()
modeller.SetSampleDimensions(polyBallImageSize)
modeller.Update()

marchingCube = vtk.vtkMarchingCubes()
marchingCube.SetInputData(modeller.GetOutput())
marchingCube.SetValue(0,0.0)
marchingCube.Update()
envelope = marchingCube.GetOutput()
WritePolyData(envelope,surfaceFilename)


from django import template

from explore.models import saved_item

register = template.Library()

@register.simple_tag(name='is_item_already_saved')
def is_item_already_saved(item, user):
    applied = saved_item.objects.filter(item=item, user=user)
    if applied:
        return True
    else:
        return False


# import flask dependencies
from flask import Flask, request, make_response, jsonify, Response

# initialize the flask app
app = Flask(__name__)

# default route
@app.route('/callback', methods=['POST'])
def index():
    print(request.json)
    data = request.json['challenge']
    print(data)
    return make_response(data, 201)

if __name__ == "__main__":
    # Flask must run on 443 and on HTTPS as per requirement from Twitch
    app.run(ssl_context='adhoc', debug=True, port=443)


from collections import Iterable, Callable
from statistics import mean

import numpy as np

from common.experiment import Example, TestCase
from common.tokens.string_tokens import Drop
from evaluation.experiment_procedure import extract_trans_tokens_from_domain_name, extract_bool_tokens_from_domain_name
from example_parser.parser import Parser
from example_parser.pixel_parser import PixelParser
from example_parser.robot_parser import RobotParser
from example_parser.string_parser import StringParser
from search.abstract_search import SearchAlgorithm

class ParameterTuning:

    def __init__(self,
                 domain: str,
                 files: (Iterable[int], Iterable[int], Iterable[int]),
                 repetitions: int,
                 search_cons: Callable[[float], SearchAlgorithm],
                 values: list[float],
                 debug: bool = False):
        self.search_cons = search_cons
        self.values = values
        self.repetitions = repetitions
        self.files = files
        self.debug = debug

        self.parser = self._get_parser(domain)
        self.test_cases = self._get_test_cases(files)
        self.env_tokens = set([c() for c in extract_trans_tokens_from_domain_name(domain)])
        self.bool_tokens = set([c() for c in extract_bool_tokens_from_domain_name(domain)])

    def run(self):
        for v in self.values:
            res = []
            for _ in range(self.repetitions):
                r = self._test_search(self.search_cons(v))
                res.append(r)

            res = self._process(res)
            res["value"] = v

            print(res)

    def _test_search(self, search: SearchAlgorithm) -> dict:
        total_cost = 0
        total_time = 0
        total_iterations = 0
        total_program_length = 0
        time_destroy = 0
        time_repair = 0
        time_cost = 0
        n_correct = 0
        n_failed = 0
        correct_cases = {}

        for tc in self.test_cases:
            r = search.run(tc.training_examples, self.env_tokens, self.bool_tokens)

            c = search.cost(tc.test_examples, r.dictionary["program"])

            total_time += r.dictionary["execution_time"]
            total_iterations += r.dictionary["iterations"]
            total_program_length += r.dictionary["program_length"]
            time_destroy += r.dictionary["time_destroy"]
            time_repair += r.dictionary["time_repair"]
            time_cost += r.dictionary["time_cost"]

            ca = tc.path_to_result_file.split("-")

            if c == 0:
                n_correct += 1

                if self.debug:
                    print("Solved {}-{}-{} with: {}".format(ca[1],ca[2],ca[3], r.dictionary["program"]))

                if ca[2] not in correct_cases:
                    correct_cases[ca[2]] = 1
                else:
                    correct_cases[ca[2]] += 1
            else:
                if self.debug:
                    print("Failed solving {}-{}-{}. Best found ({}): {}".format(ca[1],ca[2],ca[3], c.__round__(2), r.dictionary["program"]))
            if c == float('inf'):
                n_failed += 1
            else:
                total_cost += c

        size = len(self.test_cases)

        total_cost /= size
        total_time /= size
        total_iterations /= size
        total_program_length /= size
        time_destroy /= size
        time_repair /= size
        time_cost /= size
        n_correct /= size
        n_failed /= size

        return {
            "ave_cost": total_cost,
            "ave_exe_time": total_time,
            "ave_iterations": total_iterations,
            "ave_time_destroy": time_destroy,
            "ave_time_repair": time_repair,
            "ave_time_cost": time_cost,
            "per_correct": n_correct,
            "per_failed": n_failed,
            "average_program_length": total_program_length,
        }

    def _process(self, results: list[dict[str, float]]) -> dict[str, (float, float)]:
        res = {}

        for d in results:
            for k, v in d.items():
                if k not in res:
                    res[k] = []

                res[k].append(v)

        return {k: (mean(v).__round__(4), np.std(v).__round__(4)) for k, v in res.items()}

    def _get_test_cases(self, files: (Iterable[int], Iterable[int], Iterable[int])) -> list[TestCase]:
        res = []

        for i1 in files[0]:
            for i2 in files[1]:
                for i3 in files[2]:
                    res.append(self.parser.parse_file("{}-{}-{}.pl".format(i1, i2, i3)))

        return res

    @staticmethod
    def _get_parser(domain: str) -> Parser:
        if domain == "string":
            return StringParser()
        elif domain == "robot":
            return RobotParser()
        elif domain == "pixel":
            return PixelParser()
        else:
            raise Exception()


from __future__ import annotations
from abc import ABC , abstractmethod
from typing import List

class Strategy(ABC):

    @abstractmethod
    def doAlgorithm(self , data: List):
        pass

class Context():

    def __init__(self , strategy : Strategy)-> None:

        self._strategy = strategy

    @property
    def strategy(self) -> Strategy:

        return self._strategy

    @strategy.setter
    def strategy(self , strategy: Strategy) -> None:

        self._strategy = strategy

    def doSomeBusinessLogic(self) -> None:

        print(" CONTEXT : SORTING DATA USING STRATEGY")

        resultado = self.strategy.doAlgorithm(["BRASIL","ARGENTINA","VENEZUELA" , "URUGAUAI","PARAGUAI","CHILE", "COLOMBIA", "BOLIVIA", "PERU","EQUADOR","GUIANA FRANCESA","SURINAME","GUIANA" ])
        print(" - ".join(resultado))

class ConcreteStrategyA(Strategy):

    def doAlgorithm(self, data: List):
        return sorted(data)

class ConcreteStrategyB(Strategy):

    def doAlgorithm(self, data: List):
        return reversed(sorted(data))

if __name__ == "__main__":

    print("CLIENTE : STRATEGY IS SET TO NORMAL SORTING \n")
    context = Context(ConcreteStrategyA())
    context.doSomeBusinessLogic()
    print("\n")

    print("CLIENTE : STRATEGY IS SET TO REVERSED SORTING \n")
    context = Context(ConcreteStrategyB())
    context.doSomeBusinessLogic()
    print("\n")


#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time    : 2018/10/15 12:34
# @Author  : Zhe Jun
# @File    : US08.py
# @Software: PyCharm
from datetime import datetime
import re

def birth_before_parents_marriage(indi_list, fam_list):

    for fam in fam_list:

        if fam['CHIL'] != 'NONE' and fam['MARR'] != 'NONE':
            marr = datetime.strptime(fam['MARR'], '%Y-%m-%d')

            for child in fam['CHIL']:
                find_child_index = int(re.sub('\D', '', child)) - 1
                birt = datetime.strptime(indi_list[find_child_index]['BIRT'], '%Y-%m-%d')

                if marr > birt:
                    print("ERROR: INDIVIDUAL: US08: {}: {}: Birth {} before parents marriage {}".format( indi_list[find_child_index]['num']+4, indi_list[find_child_index]['INDI'], birt, marr))
                    return False

    return True


#!/usr/bin/python3

import argparse
import os
import re
import pandas as pd
from bokeh.plotting import figure, output_file, show
from bokeh.models import Legend
from bokeh.colors import named
from bokeh.io import export_png
from bokeh.io import export_svgs

parser = argparse.ArgumentParser(description = "Creates an HTML file containing the plot of waves defined by the input file in text table format.")
parser.add_argument("-i", "--input_file", type = str, dest = "wave_input_file", required = True, help = "Specifies the input waveform file in text table format.")
parser.add_argument("-o", "--output_file", type = str, dest = "wave_output_file", help = "Specifies the output plot file.")
parser.add_argument("-hi", "--header_index", type = int, dest = "header_index", default = 1, help = "Specifies the line at which the data starts. Default = line 1.")
parser.add_argument("-t", "--title", type = str, dest = "title", help = "Specifies the output plot title.")
parser.add_argument("-xl", "--x_label", type = str, dest = "x_axis_label", help = "Specifies the label for the x axis.")
parser.add_argument("-yl", "--y_label", type = str, dest = "y_axis_label", help = "Specifies the label for the y axis.")
parser.add_argument("-dp", "--show_data_points", action = "store_true", default = False, help = "Show data point over the plot-line.")
parser.add_argument("-lw", "--line_width", type = int, dest = "line_width", choices = range(1,7), default = 1, help = "Specifies the data line width. Affects all the lines.")
parser.add_argument("--x_axis_log", action = "store_true", default = False, help = "Changes the x axis to log scale.")
parser.add_argument("--y_axis_log", action = "store_true", default = False, help = "Changes the y axis to log scale.")
parser.add_argument("-pm", "--presentation_mode", action = "store_true", default = False, help = "Shows the legend for the different curves inside the plotting area.")
parser.add_argument("--save_svg", action = "store_true", default = False, help = "Saves the plot in a svg file.")
parser.add_argument("--save_png", action = "store_true", default = False, help = "Saves the plot in a png file.")
parser.add_argument("--no_output", action = "store_true", default = False, help = "Does not create the HTML output file and does not open the browser to show it.")
args = parser.parse_args()

# Data configutation section ##################################################

wave_input_file = args.wave_input_file

if (args.wave_output_file == None):
    wave_output_file = wave_input_file
else:
    wave_output_file = args.wave_output_file

# Plot configuration section ##################################################

if (args.title == None):
    _, plot_title = os.path.split(args.wave_input_file)
else:
    plot_title = args.title

line_width = args.line_width

if (args.x_axis_log):
    x_axis_type = "log"
else:
    x_axis_type = "linear"

if (args.y_axis_log):
    y_axis_type = "log"
else:
    y_axis_type = "linear"

x_axis_label = args.x_axis_label
y_axis_label = args.y_axis_label

#colors = [name for name in named.__all__]
colors = ["red", "blue", "green", "yellow", "purple", "black"]

###############################################################################

# Example of a text table
# #format table ## [Custom WaveView] saved 14:46:01 Sat Oct  6 2018
# XVAL gm/id_long_L_6_:_dc id_long_6
#  0.000E+00  1.986E+01 -0.000E+00

table = pd.read_csv(wave_input_file, header = args.header_index, sep = "\s+")

output_file(wave_output_file + ".html")
p = figure(title = plot_title, x_axis_label = x_axis_label, y_axis_label = y_axis_label, x_axis_type = x_axis_type, y_axis_type = y_axis_type, toolbar_location = "above")

legends = []

for signal, data_color in zip(table.columns[1::], colors):
    if (args.show_data_points):
        p.circle(table[table.columns[0]], table[signal], color = data_color, alpha = 0.4, muted_color = data_color, muted_alpha = 0.2)

    if (args.presentation_mode):
        obj = p.line(table[table.columns[0]], table[signal], legend = signal, line_width = line_width, color = data_color, muted_color = data_color, muted_alpha = 0.1)
    else:
        obj = p.line(table[table.columns[0]], table[signal], line_width = line_width, color = data_color, muted_color = data_color, muted_alpha = 0.1)
        legends.append((signal, [obj]))

legend = Legend(items=legends, location=(10, 0))
legend.click_policy = "mute"
p.add_layout(legend, 'right')
p.legend.background_fill_alpha = 1

p.left[0].formatter.use_scientific = False
p.below[0].formatter.use_scientific = False

if (args.save_png):
    p.toolbar_location = None
    export_png(p, filename = wave_output_file + ".png")
    p.toolbar_location = "above"
if (args.save_svg):
    p.output_backend = "svg"
    export_svgs(p, filename = wave_output_file + ".svg")
if (not args.no_output):
    show(p)


import requests, time
from bs4 import BeautifulSoup

# 要爬的網址
allurl = ['https://finance.yahoo.com/quote/%5ESOX/history?p=%5ESOX',
          'https://finance.yahoo.com/quote/%5EIXIC/history?p=%5EIXIC',
          'https://finance.yahoo.com/quote/%5EGSPC/history?p=%5EGSPC',
          'https://finance.yahoo.com/quote/%5EDJI/history?p=%5EDJI']

# 爬蟲設定
ss = requests.session()
headers = {'user-agent': 'Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/86.0.4240.198 Safari/537.36'}

for url in allurl:
    res = ss.get(url=url, headers=headers)
    soup = BeautifulSoup(res.text, 'html.parser')

    # 名稱
    title = soup.select('div[class="D(ib)"]')[0].text.split('^')[1].split(')')[0]
    # 日期
    content = soup.select('tr[class="BdT Bdc($seperatorColor) Ta(end) Fz(s) Whs(nw)"]')[0]
    Date_before = content.select('td[class="Py(10px) Ta(start) Pend(10px)"]')[0].text
    # 轉換時間格式
    Date_change = time.strptime(Date_before, "%b %d, %Y")
    Date = time.strftime("%Y-%m-%d", Date_change)
    # 開盤
    Open = content.select('td[class="Py(10px) Pstart(10px)"]')[0].text
    # 最高
    High = content.select('td[class="Py(10px) Pstart(10px)"]')[1].text
    # 最低
    Low = content.select('td[class="Py(10px) Pstart(10px)"]')[2].text
    # 收盤
    Close = content.select('td[class="Py(10px) Pstart(10px)"]')[3].text
    # 漲跌
    content2 = soup.select('div[class="D(ib) Mend(20px)"]')[0]
    PriceChange = content2.select('span')[1].text.split(' ')[0]
    # 漲跌幅
    PerChange = content2.select('span')[1].text.split('(')[1].split('%')[0]
    print('title:', title)
    print('Date:', Date)
    print('Open:', Open)
    print('High:', High)
    print('Low:', Low)
    print('Close:', Close)
    print('PriceChange:', PriceChange)
    print('PerChange:', PerChange)


import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from PythonTsa.plot_multi_ACF import multi_ACFfig
from PythonTsa.plot_multi_Q_pvalue import MultiQpvalue_plot
mda = pd.read_csv( 'USQgdpunemp.csv', header = 0)
mda = mda[['gdp', 'rate']]
dates = pd.date_range('1948-01', periods = len(mda), freq = 'Q')
mda.index = dates
mda['gdp'] = np.log(mda['gdp'])
mda.columns = ['lgdp', 'rate']
multi_ACFfig(mda, nlags = 16); plt.gcf();
plt.savefig('pyTSA_USUnempGDP_fig7-4.png', dpi = 1200,
             bbox_inches ='tight', transparent = True, legend = None)
dmda = mda.diff(1).dropna()
dmda.columns = ['dlgdp', 'drate']
dmda.plot()
plt.savefig('pyTSA_USUnempGDP_fig7-5.png', dpi = 1200,
             bbox_inches ='tight', transparent = True, legend = None) ; plt.show()
multi_ACFfig(dmda, nlags = 16);
plt.savefig('pyTSA_USUnempGDP_fig7-6.png', dpi = 1200,
            bbox_inches ='tight', transparent = True, legend = None)
qs, pv = MultiQpvalue_plot(dmda, nolags = 16);
plt.savefig('pyTSA_USUnempGDP_fig7-7.png', dpi = 1200, bbox_inches ='tight',
            transparent = True, legend = None)


from src.session.session import Session, acess_session
from src.const.default_header import XML_HTTP_REQ_HEADERS, POST_HEADERS
from src.const.url import DOCUMENT_WRITE_URL, IMAGE_FILTER_AJAX, UPLOAD_HOST_URL, UPLOAD_IMG_PHP
from src.const.const import GA_COOKIE
from src.useful_function.useful_function import quote, unquote, safe_get

import lxml.html
import asyncio
import aiohttp
import re

async def upload_image(board_id='api', file_path=''):
    async def get_write_info():
        url = DOCUMENT_WRITE_URL(board_id)
        async with Session().get(url) as res:
            parsed = lxml.html.fromstring(await res.text())

        csrf_token = parsed.xpath("//meta[@name='csrf-token']")[0].get("content")
        board_name = parsed.xpath("//a[@class='gall-tit-lnk']")[0].text.strip()
        r_key = parsed.xpath("//input[@name='r_key']")[0].get("value")

        return {
            'csrf_token': csrf_token,
            'board_name': board_name,
            'r_key': r_key
        }

    write_info = await get_write_info()

    header = XML_HTTP_REQ_HEADERS.copy()
    header.update({
        "Referer": DOCUMENT_WRITE_URL(board_id),
        "X-CSRF-TOKEN": write_info['csrf_token']
    })

    payload = {
        "id": board_id,
    }

    async with Session().post(IMAGE_FILTER_AJAX, headers=header, data=payload) as res:
        res_json = await res.json()

    header = POST_HEADERS.copy()
    header.update({
        "Host": UPLOAD_HOST_URL,
        "Referer": DOCUMENT_WRITE_URL(board_id)
    })

    payload = aiohttp.FormData({
        "id": board_id,
        "r_key": write_info["r_key"]
    })

    file = open(file_path, 'rb')
    file_ex = re.search(r"[a-z]{3,4}$", file_path).group()

    payload.add_field('upload[]',
                      file,
                      filename=f'beautiful4me.{file_ex}',
                      content_type=f'image/{file_ex}')

    cookies = {
        f"m_dcinside_{board_id}": board_id,
        "m_dcinside_lately": quote(f"{board_id}|{write_info['board_name']},"),
        "_ga": GA_COOKIE,
    }
    #
    async with Session().post(UPLOAD_IMG_PHP, headers=header, data=payload, cookies=cookies) as res:
        res_final = await res.text()

    file.close()

    img_data = re.search(r"(?<='img',').*(?=',0\);)", res_final).group()

    return img_data

if __name__ == '__main__':
    value = asyncio.run(upload_image('api', "C:/Users/dobbyjang/Desktop/PyDCApi/test/article.jpg"))
    print(value)


import requests
import getgodville
from settings import CHATS, BOT_TOKEN, ZST_ID
from jump_counter import autostart_timers
from telebot import TeleBot
from datetime import datetime

now_str = datetime.now().strftime('%d.%m.%Y года')
prognoz = f'Всем доброе утро! Сегодня {now_str}\nУтренняя сводка новостей от бармена:\n {getgodville.god_prognoz()}\n'
coupon = getgodville.get_coupon()
for chat_id in CHATS:
    leaving = getgodville.god_guild(CHATS[chat_id])

    url = f'https://api.telegram.org/bot{BOT_TOKEN}/sendMessage?chat_id={chat_id}&text={prognoz + leaving + coupon}'
    resp = requests.get(url)
    print(f'{now_str}: {CHATS[chat_id]} status {resp.status_code}')
    # print(prognoz + leaving + coupon)

# ------<<<------ Перезапуск таймеров ------>>>------
chatlist = [ZST_ID]
user_timers = []
bot_token = BOT_TOKEN
bot = TeleBot(bot_token)
for chat in chatlist:
    autostart_timers(bot, chat, user_timers)


import json
import os

import numpy as np

from pybamocs.four_box_model.box_model import box_model
from pybamocs.four_box_model.box_model_args import dict_from_box_args

def run_and_save_box_run(box_args, save_path, run_name, compressed=False):
    M_n, M_upw, M_eddy, D_Low, T, S, sigma_0 = box_model(**box_args).unpack()
    os.makedirs(os.path.join(save_path, run_name), exist_ok=True)
    params = dict_from_box_args(**box_args)
    json.dump(params, open(os.path.join(save_path, run_name, 'args.json'), 'w'))
    if compressed:
        np.savez_compressed(os.path.join(save_path, run_name, 'results.npz'),
                            M_n=M_n, M_upw=M_upw, M_eddy=M_eddy, D_Low=D_Low, T=T, S=S, sigma_0=sigma_0)
    else:
        np.savez(os.path.join(save_path, run_name, 'results.npz'),
                 M_n=M_n, M_upw=M_upw, M_eddy=M_eddy, D_Low=D_Low, T=T, S=S, sigma_0=sigma_0)
    return True


#!/usr/bin/env python
from collections import defaultdict
from argparse import ArgumentParser

parser = ArgumentParser()
parser.add_argument("-w", "--week", type=int, required=False)
parser.add_argument("-t", "--todoist", action='store_true', required=False)
args = parser.parse_args()

def todoist_format(week, day, exercise):
    parts = exercise.split(' ')
    duration = 0
    durations = defaultdict(lambda : 0)
    for p in parts:
        if 'x' in p:
            rep, ex = p.split('x')
            rep = int(rep)
            ex = ex.strip('[]')
            for p2 in ex.split(","):
                letter = p2[-1]
                nums = float(p2[:-1])
                duration += rep * nums
                durations[letter] += rep * nums
        else:
            letter = p[-1]
            nums = float(p[:-1])
            duration += nums
            durations[letter] += nums
    ' '.join([f"{mins}{l}" for l, mins in durations.items()])
    print(f"w{week}.{day} -- 5m warm + `{exercise}` + 5m cool -> `{duration + 10}m`") #, -- {durns}")

c25k = {
    1: {
        1: "8x[1j,1.5w]",
        2: "8x[1j,1.5w]",
        3: "8x[1j,1.5w]",
    },
    2: {
        1: "6x[1.5j,2w]",
        2: "6x[1.5j,2w]",
        3: "6x[1.5j,2w]",
    },
    3: {
        1: "2x[1.5j,1.5w,3j,3w]",
        2: "2x[1.5j,1.5w,3j,3w]",
        3: "2x[1.5j,1.5w,3j,3w]",
    },
    4: {
        1: "1x[3j,1.5w,5j,2.5w,3j,1.5w]",
        2: "1x[3j,1.5w,5j,2.5w,3j,1.5w]",
        3: "1x[3j,1.5w,5j,2.5w,3j,1.5w]",
    },
    5: {
        1: "1x[5j,3w,5j,3w,5j]",
        2: "1x[8j,5w,8j]",
        3: "1x[20j]",
    },
    6: {
        1: "1x[5j,3w,8j,3w,5j]",
        2: "1x[10j,3w,10j]",
        3: "1x[22j]",
    },
    7: {
        1: "1x[25j]",
        2: "1x[25j]",
        3: "1x[25j]",
    },
    8: {
        1: "1x[28j]",
        2: "1x[28j]",
        3: "1x[28j]",
    },
    9: {
        1: "1x[30j]",
        2: "1x[30j]",
        3: "1x[30j]",
    },
}

print("Always with 5m warmup and cooldown")
if args.week:
    c25k = {args.week: c25k[args.week]}
for week, days in c25k.items():
    for day, exercise in days.items():
        if args.todoist:
            todoist_format(week, day, exercise)
        else:
            print(f"w{week}.{day} -- {exercise}")


from typing import List
from fastapi.params import Depends
from sqlalchemy.orm.session import Session
from app import models
from fastapi import FastAPI, Response, status, HTTPException, APIRouter
from fastapi.exceptions import RequestValidationError
from .. import schemas
from ..database import SessionLocal, engine, get_db

# create note in database
def create(db: Session, request: schemas.Note):
    new_note = models.Note(title = request.title, note = request.note, owner_id = 1)
    db.add(new_note)
    db.commit()
    db.refresh(new_note)
    return new_note

# get all  notes form database
def read_all(db: Session):
    return db.query(models.Note).all()

# get a single note with id from database
def read(db: Session, id: int):
    return db.query(models.Note).filter(models.Note.id == id).first()

# update note in database
def update(db:Session, id: int, request: schemas.Note):
    updated_note = db.query(models.Note).filter(models.Note.id == id)
    if not updated_note:
        raise HTTPException(status_code=404, detail=f"Cannot find note with id {id}")
    updated_note.update({"title": request.title, "note": request.note})
    db.commit()
    return "updated"

def delete(db:Session, id: int):
    note = db.query(models.Note).filter(models.Note.id == id)
    if not note.first():
        raise HTTPException(status_code=404, detail=f"Cannot find note with id {id}")
    note.delete(synchronize_session=False)
    db.commit()
    return "Deleted"


from typing import List
import heapq

# LeetCode / 787. Cheapest Flights Within K Stops
class Solution:
    def findCheapestPrice(self, n: int, flights: List[List[int]], src: int, dst: int, k: int) -> int:
        adj_list = [[] for _ in range(n)]
        for u, v, w in flights:
            adj_list[u].append((w, v))

        heap = []
        dist = [-1] * n
        dist_k = [-1] * n
        heapq.heappush(heap, (0, src, 0))
        while heap:
            temp_cost, temp_dst, temp_k = heapq.heappop(heap)
            if dist[temp_dst] == -1:
                dist[temp_dst] = temp_cost
            if dist_k[temp_dst] == -1 or dist_k[temp_dst] > temp_k:
                dist_k[temp_dst] = temp_k
            if dist[temp_dst] < temp_cost and dist_k[temp_dst] < temp_k:
                continue

            if temp_k <= k:
                for input_cost, input_dst in adj_list[temp_dst]:
                #while adj_list[temp_dst]:
                #    input_cost, input_dst = adj_list[temp_dst].pop()
                #    if temp_k != k or input_dst == dst:
                    heapq.heappush(heap, (temp_cost + input_cost, input_dst, temp_k + 1))

        return dist[dst]

n = 3
flights = [[0,1,100],[1,2,100],[0,2,500]]
src = 0
dst = 2
k = 0
print(Solution().findCheapestPrice(n, flights, src, dst, k))


import time

import cv2
import numpy as np
from numba import njit
from scipy.interpolate import CubicSpline
from scipy.optimize import minimize
from agv import AAGV
from utils import _differential_drive_inverse_kinematics, _differential_drive_forward_kinematics, \
    my_clip, get_orientation_difference, get_orientation_between_points, _differential_drive_update_state

debug = False

# @njit
def predict_trajectory(robot_position: np.ndarray, robot_orientation: np.ndarray, current_wheel_vel: np.ndarray,
                       controls: np.ndarray, dt: float, wheel_radius: float,
                       wheel_distance: float,
                       max_angluar_velocity: float, max_acceleration: float):
    """
    Predict the trajectory of a differential drive robot given its initial state and a sequence of control inputs.

    Args:
        robot_position (np.ndarray): The initial 2D position of the robot (x, y).
        robot_orientation (np.ndarray): The initial orientation of the robot (theta) as a 1D array.
        current_wheel_vel (np.ndarray): The initial left and right wheel angular velocities.
        controls (np.ndarray): An array of control inputs, each consisting of linear velocity (v) and angular velocity (w).
        dt (float): The time step between control inputs in seconds.
        wheel_radius (float): The radius of the wheels in meters.
        wheel_distance (float): The distance between the wheels in meters.
        max_angluar_velocity (float): The maximum allowable angular velocity for the wheels in rad/s.
        max_acceleration (float): The maximum allowable linear acceleration for the wheels in m/s^2.

    Returns:
        predicted_trajectory (np.ndarray): An array of predicted 2D robot positions for each time step.
        actual_controls (np.ndarray): An array of the actual control inputs (v, w) applied at each time step,
                                      considering the constraints on wheel velocities and accelerations.
    """
    horizon = len(controls)
    robot_pos = robot_position.astype(np.float64)  # Cast robot_position to float64
    lw_vel = current_wheel_vel[0]
    rw_vel = current_wheel_vel[1]
    predicted_trajectory = np.zeros((horizon, 3))
    actual_controls = np.zeros((horizon, 2))
    robot_state = np.array([
        [robot_pos[0]],
        [robot_pos[1]],
        [robot_orientation[0]]])
    for t, (v, w) in enumerate(controls):
        w_left, w_right = _differential_drive_inverse_kinematics(v, w, wheel_radius, wheel_distance)

        # check limits
        w_left = my_clip(w_left, -max_angluar_velocity, max_angluar_velocity)
        w_right = my_clip(w_right, -max_angluar_velocity, max_angluar_velocity)
        w_left = my_clip(w_left, lw_vel - max_acceleration * dt, lw_vel + max_acceleration * dt)
        w_right = my_clip(w_right, rw_vel - max_acceleration * dt, rw_vel + max_acceleration * dt)

        lw_vel = w_left
        rw_vel = w_right

        a_v, a_w = _differential_drive_forward_kinematics(w_left, w_right, wheel_radius, wheel_distance)
        actual_controls[t] = a_v, a_w
        robot_state = _differential_drive_update_state(robot_state, a_v, a_w, dt)
        predicted_trajectory[t] = np.array([robot_state[0, 0], robot_state[1, 0], robot_state[2, 0]])

    return predicted_trajectory, actual_controls

@njit
def find_closest_point(point, points):
    """Finds the closest point to a given point from a list of points"""
    min_distance = np.inf
    closest_point = None
    for p in points:
        distance = np.sqrt(np.sum((point - p) ** 2))
        if distance < min_distance:
            min_distance = distance
            closest_point = p
    return closest_point, min_distance

# @njit
def _cost_function(controls, robot_position, robot_orientation, current_wheel_vel, path, dt, horizon, wheel_radius,
                   wheel_distance, max_angluar_velocity, max_acceleration, weights):
    """
    Calculate the cost of a sequence of control inputs for a differential drive robot.

    Args:
        controls (np.ndarray): An array of control inputs, each consisting of linear velocity (v) and angular velocity (w).
        robot_position (np.ndarray): The initial 2D position of the robot (x, y).
        robot_orientation (np.ndarray): The initial orientation of the robot (theta) as a 1D array.
        current_wheel_vel (np.ndarray): The initial left and right wheel angular velocities.
        path (np.ndarray): An array of the desired 2D robot positions for each time step.
        dt (float): The time step between control inputs in seconds.
        horizon (int): The number of control inputs to consider.
        wheel_radius (float): The radius of the wheels in meters.
        wheel_distance (float): The distance between the wheels in meters.
        max_angluar_velocity (float): The maximum allowable angular velocity for the wheels in rad/s.
        max_acceleration (float): The maximum allowable linear acceleration for the wheels in m/s^2.
        weights (tuple): A tuple containing the weights for different cost components:
                         (R: weight for control inputs, Q: weight for position error, Rd: weight for change in controls)

    Returns:
        cost (float): The total cost of the given sequence of control inputs.
    """
    controls = controls.reshape((horizon, 2))  # The controls in terms of linear velocity and angular velocity
    R = weights[0]
    Q = weights[1]
    Rd = weights[2]
    trajectory, actual_controls = predict_trajectory(robot_position, robot_orientation, current_wheel_vel, controls, dt,
                                                     wheel_radius, wheel_distance, max_angluar_velocity,
                                                     max_acceleration)
    # Calculate the distance from the predicted trajectory to the path
    # Reshape the trajectory array to have shape (horizon, 1, 2)
    reshaped_trajectory = trajectory.reshape(horizon, 1, 3)

    # Subtract the path array from the reshaped trajectory array
    diffs = trajectory - path
    sqr_diffs = diffs ** 2
    # abs_diffs = np.abs(diffs)
    # abs_sum_dif = sqr_diffs[:, 0] + sqr_diffs[:, 1]
    distance_cost = np.zeros(horizon, dtype=np.float64)
    heading_cost = np.zeros(horizon, dtype=np.float64)
    control_cost = np.zeros(horizon, dtype=np.float64)
    control_change_cost = np.zeros(horizon - 1, dtype=np.float64)
    for i in range(horizon):
        distance_cost[i] = np.sum(Q[:2, :2] @ sqr_diffs[i, :2])  # * ((horizon - i) / horizon)
        # heading_cost[i] = Q[-1, -1] * sqr_diffs[i, -1] * ((horizon - i) / horizon)  # prioritize the first heading error

        diff_orientation = get_orientation_difference(trajectory[i, -1],
                                                      get_orientation_between_points(trajectory[i, :2],
                                                                                path[i, :2])) ** 2

        heading_cost[i] = Q[-1, -1] * diff_orientation * ((horizon - i) / horizon)  # prioritize the first heading error

        control_cost[i] = np.sum(R @ controls[i] ** 2)
        # cost += np.sum(Q @ ((actual_controls[i] - controls[i]) ** 2)) * 10000
        if i < horizon - 1:
            control_change_cost[i] = np.sum(Rd @ (controls[i + 1] - controls[i]) ** 2)
    # for debugging:
    dist_cost = np.sum(distance_cost)
    head_cost = np.sum(heading_cost) * 0
    cont_cost = np.sum(control_cost) * 0
    cont_change_cost = np.sum(control_change_cost) * 0
    total_cost = dist_cost + head_cost + cont_cost + cont_change_cost
    # if debug:
    #     visualize_robot_and_target(trajectory[0], path[0, :2])
    return total_cost

def cost_function(controls, robot_position, robot_orientation, current_wheel_vel, path, dt, horizon, wheel_radius,
                  wheel_distance, max_angluar_velocity, max_acceleration, weights):
    """
    Wrapper for the numba implementation of the cost function for a differential drive robot.

    Args:
        Same as the _cost_function (see its docstring for details).

    Returns:
        cost (float): The total cost of the given sequence of control inputs.
    """
    return float(
        _cost_function(controls, robot_position, robot_orientation, current_wheel_vel, path, dt, horizon, wheel_radius,
                       wheel_distance, max_angluar_velocity, max_acceleration, weights))

def mpc_controller(path, robot_position, robot_orientation, dt, horizon, wheel_radius, wheel_distance, w_left, w_right,
                   max_angular_velocity, max_accel, weights):
    """
    Model Predictive Control (MPC) for a differential drive robot.

    Args:
        path (np.ndarray): An array of the desired 2D robot positions for each time step.
        robot_position (np.ndarray): The current 2D position of the robot (x, y).
        robot_orientation (np.ndarray): The current orientation of the robot (theta) as a 1D array.
        dt (float): The time step between control inputs in seconds.
        horizon (int): The number of control inputs to consider.
        wheel_radius (float): The radius of the wheels in meters.
        wheel_distance (float): The distance between the wheels in meters.
        w_left (float): The current left wheel angular velocity in rad/s.
        w_right (float): The current right wheel angular velocity in rad/s.
        max_angluar_velocity (float): The maximum allowable angular velocity for the wheels in rad/s.
        max_accel (float): The maximum allowable linear acceleration for the wheels in m/s^2.
        weights (tuple): A tuple containing the weights for different cost components:
                         (R: weight for control inputs, Q: weight for position error, Rd: weight for change in controls)

    Returns:
        optimal_controls (np.ndarray): An array of the optimal control inputs, each consisting of linear velocity (v)
                                       and angular velocity (w).
        cost (float): The total cost of the optimal sequence of control inputs.
    """
    horizon = min(len(path), horizon)

    # min and max linear velocity)]
    bounds = [(0, 5),  # min and max linear velocity
              (np.deg2rad(-180), np.deg2rad(180))  # min and max angular velocity
              ] * horizon
    # Initialize the controls is linear velocity and angular velocity
    initial_controls = np.zeros((2 * horizon))

    result = minimize(cost_function,
                      x0=initial_controls,
                      args=(
                          robot_position, robot_orientation, np.array([w_left, w_right]), path, dt, horizon,
                          wheel_radius,
                          wheel_distance, max_angular_velocity, max_accel, weights),
                      bounds=bounds,
                      method='SLSQP',
                      options={'maxiter': 1000}
                      )

    optimal_controls = result.x.reshape((horizon, 2))
    # Return the first control input of the optimal sequence
    return optimal_controls, result.fun

def body_to_world(robot_pos, robot_orientation, v, w, dt):
    updated_orientation = robot_orientation + w * dt
    updated_position = robot_pos + np.array(
        [v * np.cos(updated_orientation), v * np.sin(updated_orientation)]) * dt
    return updated_position, updated_orientation

def update_path(path, robot_position, threshold_distance=0.25):
    distances = np.linalg.norm(path[:, :2] - robot_position, axis=1)
    remaining_points = distances > threshold_distance
    indx = np.argwhere(distances <= threshold_distance)
    if len(indx) > 0:
        path = path[indx[-1, 0] + 1:]
    return path

def generate_trajectory(path, distance_between_points=0.1):
    # Calculate the length of the path
    length = len(path)

    # Check if the path has at least 2 points
    if length < 2:
        raise ValueError("Path must have at least 2 points")

    # Calculate the distances between consecutive points in the path
    distances = np.linalg.norm(np.diff(path, axis=0), axis=1)

    # Calculate the cumulative distances along the path
    cumulative_distances = np.insert(np.cumsum(distances), 0, 0)

    # Create a cubic spline interpolator for x and y coordinates
    x_spline = CubicSpline(cumulative_distances, path[:, 0])
    y_spline = CubicSpline(cumulative_distances, path[:, 1])

    # Calculate the total distance of the path
    total_distance = cumulative_distances[-1]

    # Generate the interpolated distances based on the specified distance between points
    interpolated_distances = np.arange(0, total_distance, distance_between_points)

    # Evaluate the spline interpolator at the interpolated distances
    x_trajectory = x_spline(interpolated_distances)
    y_trajectory = y_spline(interpolated_distances)

    # Combine the x and y coordinates into the final trajectory
    trajectory = np.column_stack((x_trajectory, y_trajectory))
    orientations = calculate_orientations(trajectory)
    trajectory = np.column_stack((trajectory, orientations))
    return trajectory

@njit(cache=True)
def calculate_orientations(coords):
    n = coords.shape[0]
    orientations = np.zeros(n, dtype=np.float64)

    for i in range(n - 1):
        dx = coords[i + 1, 0] - coords[i, 0]
        dy = coords[i + 1, 1] - coords[i, 1]
        orientations[i] = np.arctan2(dy, dx)

    orientations[-1] = orientations[-2]

    return orientations

import matplotlib.pyplot as plt

def plot_path_and_trajectory(path, trajectory):
    plt.figure(figsize=(8, 6))
    plt.plot(path[:, 0], path[:, 1], 'o-', label='Original Path', markersize=8)
    plt.plot(trajectory[:, 0], trajectory[:, 1], '.-', label='Smoothed Trajectory', markersize=4)
    plt.xlabel('X')
    plt.ylabel('Y')
    plt.legend()
    plt.title('Path and Smoothed Trajectory')
    plt.grid()
    plt.show()

def get_global_to_local_transform(robot_state):
    """
    Get a 3x3 NumPy array representing the global to local coordinate transform.

    :param robot_state: A 3x1 NumPy array representing the current state of the robot, with
                        [x_position, y_position, orientation].
    :return: A 3x3 NumPy array representing the global to local coordinate transform.
    """
    x, y, theta = robot_state.flatten()
    transform = np.array([
        [np.cos(theta), -np.sin(theta), -x],
        [np.sin(theta), np.cos(theta), -y],
        [0, 0, 1]
    ])
    t = np.array([
        [1, 0, -x],
        [0, 1, -y],
        [0, 0, 1]
    ])
    r = np.array([
        [np.cos(theta), -np.sin(theta), 0],
        [np.sin(theta), np.cos(theta), 0],
        [0, 0, 1]
    ])
    return transform

def visualize_robot_and_target(robot_state, target_point, scale=100, img_size=(500, 500)):
    """
    Visualize the robot's current position and a target point in local coordinates using OpenCV (cv2).

    :param robot_state: A 3x1 NumPy array representing the current state of the robot, with
                        [x_position, y_position, orientation].
    :param target_point: A 2x1 NumPy array representing the target point in global coordinates (x, y).
    :param scale: A float representing the scale factor for visualization (default: 100).
    :param img_size: A tuple representing the size of the output image (width, height).
    :return: None
    """
    img = np.zeros((*img_size, 3), dtype=np.uint8)

    # Convert global coordinates to local coordinates
    target_local = get_global_to_local_transform(robot_state)
    target_local_coords = np.dot(target_local, np.append(target_point, 1))

    # Calculate positions in the image
    robot_pos_img = np.array(img_size) // 2
    target_pos_img = (target_local_coords[:2] * scale + robot_pos_img).astype(int)
    target_pos_img[1] = img_size[1] - target_pos_img[1]  # Flip the y-coordinate

    # Draw the robot and the target point
    cv2.circle(img, tuple(robot_pos_img), 10, (0, 255, 0), -1)  # Robot (green)
    cv2.circle(img, tuple(target_pos_img), 5, (0, 0, 255), -1)  # Target point (red)

    diff = get_orientation_between_points((0, 0), target_local_coords)
    # display the difference between the robot's orientation and the orientation to the target point
    cv2.putText(img, f"Orientation difference: deg={np.rad2deg(diff):.2f}, rad={diff:.2f}", (10, 30),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)

    # Show the image
    cv2.imshow("Robot and Target", img)

def visualize_trajectories(path, agv_states, agv_trajectories, optimal_controls_list, wheel_radius, wheel_distance, dt,
                           total_time, scale=250, radius=5, thickness=-1, wait_time=0
                           ):
    global debug, frames
    # print(agv_states[0])
    num_agvs = len(agv_states)

    # Set the dimensions for the visualization window
    buffer = 100
    width = int(max(np.max(path[:, 0]),
                    max([np.max(traj[:, 0]) for traj in agv_trajectories])) * scale) + 2 * radius + buffer * 2
    height = int(max(np.max(path[:, 1]),
                     max([np.max(traj[:, 1]) for traj in agv_trajectories])) * scale) + 2 * radius + buffer * 2

    # width = int(max(np.max(path[:, 0]), np.max(agv_trajectories[0][:, 0])) * scale) + 2 * radius + buffer * 2
    # height = int(max(np.max(path[:, 1]), np.max(agv_trajectories[0][:, 1])) * scale) + 2 * radius + buffer * 2

    # Create a black background
    img = np.zeros((height, width, 3), dtype=np.uint8)
    cv2.line(img, (buffer, 0), (buffer, height), (255, 255, 255), 1)
    cv2.line(img, (0, buffer), (width, buffer), (255, 255, 255), 1)

    # Define colors
    path_color = (255, 0, 0)  # Blue
    trajectory_color = (0, 255, 0)  # Green
    robot_color = (0, 0, 255)  # Red
    pred_path_color = (255, 255, 255)

    # Draw path points
    for point in path:
        x, y = point[:2]
        x = int(scale * x + buffer)
        y = int(scale * y + buffer)
        cv2.circle(img, (x, y), radius, path_color, thickness)

    # Draw trajectory points and robot positions for each AGV
    for i in range(num_agvs):
        trajectory = agv_trajectories[i]
        robot_position, robot_orientation = agv_states[i][:2, 0], agv_states[i][-1]

        for point in trajectory:
            x, y = point[:2]
            x = int(scale * x + buffer)
            y = int(scale * y + buffer)
            cv2.circle(img, (x, y), radius, trajectory_color, thickness)
        if optimal_controls_list is not None:
            pred_path = predict_trajectory(robot_position, robot_orientation, optimal_controls_list[i], dt,
                                           wheel_radius, wheel_distance)
            with_start = np.concatenate((robot_position.reshape(1, 2), pred_path[:, :2]), axis=0)
            for j, point in enumerate(with_start):
                x, y = point
                x = int(scale * x + buffer)
                y = int(scale * y + buffer)
                if j == 0:
                    last_point = (x, y)
                    continue
                cv2.line(img, last_point, (x, y), pred_path_color, 2)
                last_point = (x, y)

        # Draw robot position
        x, y = robot_position
        x = int(scale * x + buffer)
        y = int(scale * y + buffer)
        cv2.circle(img, (x, y), radius * 2, robot_color, thickness)

        # Draw robot orientation line
        orientation_x = x + int(radius * 2 * np.cos(robot_orientation))
        orientation_y = y + int(radius * 2 * np.sin(robot_orientation))
        cv2.line(img, (x, y), (orientation_x, orientation_y), (0, 0, 0), 2)

    img = np.flipud(img)
    img = img.astype(np.uint8)
    cv2.putText(img, total_time, (width - 200, height - 30),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
    # Save the frame as an image
    frame = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)  # Convert the frame to RGB format
    frames.append(frame)  # Add the frame to the list of frames

    # Display the image
    cv2.imshow("Trajectory Visualization", img)

    # Wait for a key press and close the window
    key = cv2.waitKey(wait_time)
    if key == ord('q'):
        cv2.destroyAllWindows()
        exit(0)
    if key == ord('p'):
        wait_time = 0 if wait_time == 1 else 1
        debug = False if debug else True

    return wait_time

@njit
def get_distance(x1, y1, x2, y2):
    return np.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)

# Function to calculate desired heading and velocity
def calculate_desired_heading_and_velocity(position_error, kp_position, max_velocity):
    distance = np.linalg.norm(position_error)
    desired_heading = np.arctan2(position_error[1], position_error[0])

    desired_velocity = kp_position * distance
    if desired_velocity > max_velocity:
        desired_velocity = max_velocity

    return desired_heading, desired_velocity

# Function to convert desired heading and velocity to wheel velocities
def convert_to_wheel_velocities(desired_heading, desired_velocity, wheel_radius, wheel_distance):
    # Calculate the required angular velocity
    angular_error = desired_heading - robot.get_orientation()
    kp_angular = 0.5  # Tune this parameter
    desired_angular_velocity = kp_angular * angular_error

    desired_lw_vel, desired_rw_vel = _differential_drive_inverse_kinematics(desired_velocity, desired_angular_velocity,
                                                                            wheel_radius, wheel_distance)
    # Calculate the desired linear velocity
    wheel_radius = robot.get_wheel_radius()
    wheel_distance = robot.get_wheel_distance()

    # Calculate desired wheel velocities
    desired_left_wheel_velocity = (2 * desired_velocity - desired_angular_velocity * wheel_distance) / (
            2 * wheel_radius)
    desired_right_wheel_velocity = (2 * desired_velocity + desired_angular_velocity * wheel_distance) / (
            2 * wheel_radius)

    return desired_left_wheel_velocity, desired_right_wheel_velocity

# @njit
def pid_controller(Kp: float, Ki: float, Kd: float, desired_position: np.ndarray,
                   current_states: np.ndarray, prev_errors: np.ndarray, integral_errors: np.ndarray):
    position_error = desired_position[:, :2] - current_states[:, :2, 0]
    distance_error = np.linalg.norm(position_error, axis=1)

    integral_errors += distance_error
    derivative_errors = (distance_error - prev_errors)
    desired_velocity = Kp * distance_error + Ki * integral_errors + Kd * derivative_errors
    return desired_velocity, distance_error, integral_errors

# @njit
def pid_controller_angle(Kp: float, Ki: float, Kd: float, desired_position: np.ndarray,
                         current_states: np.ndarray, prev_errors: np.ndarray, integral_errors: np.ndarray):
    position_error = desired_position[:, :2] - current_states[:, :2, 0]
    desired_heading = np.arctan2(position_error[:, 1], position_error[:, 0])
    angular_error = desired_heading - current_states[:, -1, 0]
    angular_error = np.arctan2(np.sin(angular_error), np.cos(angular_error))

    integral_errors += angular_error
    derivative_errors = (angular_error - prev_errors)
    desired_angular_velocity = Kp * angular_error + Ki * integral_errors + Kd * derivative_errors
    return desired_angular_velocity, angular_error, integral_errors

if __name__ == '__main__':
    visualize = True

    # Test the controller with a simple path
    path = np.array([[1., 0.], [0., 1.], [1., 1.], [2., 0.], [3., 1.], [4., 0.]])
    traj = generate_trajectory(path, distance_between_points=0.1)
    agv_config = {
        "wheel_radius": 0.1,
        "wheel_distance": 0.5,
        "max_speed": 5.,
        "max_acceleration": 5,
        "max_deceleration": 5.,
        "max_battery": 1000,
        "move_cost": 0,
        "idle_cost": 0,
        "radius": 1,
    }
    number_of_robots = 100

    agvs = [AAGV(0, None, agv_config) for _ in range(number_of_robots)]
    for agv in agvs:
        agv.robot_state = np.array([
            [np.random.uniform(0.5, 1.5)],  # x position
            [np.random.uniform(-0.2, 0.2)],  # y position
            [np.random.uniform(-np.pi, np.pi)],  # orientation
        ])
    trajs = [traj for _ in range(number_of_robots)]
    # agv.orientation = agv.orientation

    dt = 1 / 240
    dt = 1 / 10
    wait_time = 1
    simulated_time = 0
    prev_errors, integral_errors = np.zeros((number_of_robots,)), np.zeros((number_of_robots,))
    a_prev_errors, a_integral_errors = np.zeros((number_of_robots,)), np.zeros((number_of_robots,))
    optimal_controls = None
    dones = np.zeros(number_of_robots, dtype=np.bool)
    for t in range(10000):
        # Update the path before passing it to the MPC controller
        start = time.perf_counter()

        current_states = np.array([agv.robot_state for agv in agvs])
        desired_points = np.array([traj[0] for traj in trajs])
        lin_vel, prev_errors, integral_errors = pid_controller(1., 0., 1., desired_points, current_states,
                                                               prev_errors,
                                                               integral_errors)
        angl_vel, a_prev_errors, a_integral_errors = pid_controller_angle(1., 0., 1., desired_points, current_states,
                                                                          a_prev_errors, a_integral_errors)

        lw_speed, rw_speed = _differential_drive_inverse_kinematics(lin_vel, angl_vel, agv.wheel_radius,
                                                                    agv.wheel_distance)
        # find the ratio so max speed is 1
        max_speed = np.max(np.array([np.abs(lw_speed), np.abs(rw_speed)]).reshape(100, 2), axis=1)
        indx = np.argwhere(max_speed > agv.max_wheel_speed)
        lw_speed[indx] /= max_speed[indx]
        rw_speed[indx] /= max_speed[indx]
        simulated_time += dt

        for i, (lw_speed, rw_speed) in enumerate(zip(lw_speed, rw_speed)):
            agv = agvs[i]
            agv.set_wheel_velocity(lw_speed, rw_speed)
            agv.update(dt)
            new_traj = update_path(trajs[i], agv.robot_state[:2, 0], threshold_distance=0.1)
            current_states[i] = agv.robot_state
            if len(new_traj) == 0:
                new_traj = np.array([[agv.x, agv.y, agv.orientation]])
                dones[i] = True
            trajs[i] = new_traj
        end = time.perf_counter()
        total_time = f"Time: {(end - start):.2f} seconds"
        print(total_time)
        if visualize:
            wait_time = visualize_trajectories(path, current_states, trajs, None,
                                               agv_config["wheel_radius"], agv_config["wheel_distance"], dt, total_time,
                                               wait_time=wait_time)


import sys

import os
from mqtt.client.factory import MQTTFactory
from twisted.application.internet import ClientService
from twisted.internet.defer import inlineCallbacks
from twisted.internet.endpoints import clientFromString
from twisted.internet import reactor

from twisted.logger import Logger, LogLevel, globalLogBeginner, textFileLogObserver, \
    FilteringLogObserver, LogLevelFilterPredicate

# ----------------
# Global variables
# ----------------
from ledslie.config import Config
from ledslie.definitions import LEDSLIE_TOPIC_STATS_BASE, LEDSLIE_TOPIC_SEQUENCES_PROGRAMS

logLevelFilterPredicate = LogLevelFilterPredicate(defaultLogLevel=LogLevel.info)

log = Logger(__file__.split(os.sep)[-1])

# -----------------
# Utility Functions
# -----------------

def startLogging(console=True, filepath=None):
    '''
    Starts the global Twisted logger subsystem with maybe
    stdout and/or a file specified in the config file
    '''
    global logLevelFilterPredicate

    observers = []
    if console:
        observers.append(FilteringLogObserver(observer=textFileLogObserver(sys.stdout),
                                              predicates=[logLevelFilterPredicate]))

    if filepath is not None and filepath != "":
        observers.append(FilteringLogObserver(observer=textFileLogObserver(open(filepath, 'a')),
                                              predicates=[logLevelFilterPredicate]))
    globalLogBeginner.beginLoggingTo(observers)

def setLogLevel(namespace=None, levelStr='info'):
    '''
    Set a new log level for a given namespace
    LevelStr is: 'critical', 'error', 'warn', 'info', 'debug'
    '''
    level = LogLevel.levelWithName(levelStr)
    logLevelFilterPredicate.setLogLevelForNamespace(namespace=namespace, level=level)

def CreateContent(contentCls):
    config = Config()
    startLogging()
    log_level = 'info'
    if config['DEBUG']:
        log_level = 'debug'
    setLogLevel(namespace='mqtt', levelStr=log_level)
    setLogLevel(namespace=contentCls.__name__, levelStr=log_level)

    factory = MQTTFactory(profile=MQTTFactory.PUBLISHER)
    myEndpoint = clientFromString(reactor, Config().get('MQTT_BROKER_CONN_STRING'))
    serv = contentCls(myEndpoint, factory)
    serv.startService(contentCls.__name__)
    return serv

class GenericContent(ClientService):
    def __init__(self, endpoint, factory, reactor=None):
        super().__init__(endpoint, factory)
        if reactor is None:
            from twisted.internet import reactor
        self.reactor = reactor
        self.config = Config()
        self.protocol = None
        self._system_name = None

    def startService(self, name):
        log.info("starting MQTT Content Publisher Service")
        # invoke whenConnected() inherited method
        self._system_name = name
        self.whenConnected().addCallback(self.connectToBroker)
        ClientService.startService(self)

    @inlineCallbacks
    def connectToBroker(self, protocol):
        '''
        Connect to MQTT broker
        '''
        self.protocol = protocol
        self.protocol.onDisconnection = self.onDisconnection
        self.protocol.setWindowSize(3)
        try:
            yield self.protocol.connect(self._system_name, keepalive=60)
        except Exception as e:
            self.log.error("Connecting to {broker} raised {excp!s}",
                      broker=self.config.get('MQTT_BROKER_CONN_STRING'), excp=e)
        else:
            self.log.info("Connected to {broker}", broker=self.config.get('MQTT_BROKER_CONN_STRING'))
            self.reactor.callLater(0, self.onBrokerConnected)
        self_name = self.__class__.__name__
        self.publish(topic=LEDSLIE_TOPIC_STATS_BASE+self_name, message="%s now (re-)connected" % self_name)

    def onBrokerConnected(self):
        log.info("onBrokerConnected called")

    def onDisconnection(self, reason):
        '''
        get notfied of disconnections
        and get a deferred for a new protocol object (next retry)
        '''
        log.debug("<Connection was lost !> <reason={r}>", r=reason)
        self.whenConnected().addCallback(self.connectToBroker)

    def publish(self, topic, message, qos=0, retain=False):
        if hasattr(message, 'serialize'):
            message = message.serialize()
        self.log.debug("To '{topic}', Published: '{data}'", topic=topic, data=message)
        return self.protocol.publish(topic, message, qos, retain)

    def remove_display(self, program_name):
        """
        Remove the program from the display.
        :param program_name: The name of the program to remove
        :type program_name: str
        :return: The deferred that's called when the command has been send.
        :rtype: deferred
        """
        return self.publish(LEDSLIE_TOPIC_SEQUENCES_PROGRAMS[:-1] + program_name, "")


from langchain import PromptTemplate
from langchain.chat_models import ChatOpenAI
from langchain.chains import LLMChain

information = """
Fund Name is Top 200 Fund provide returns of 33.07% in 3 Years, 21.52% in 5 Years and 19.21% in 10 Years.

Another Fund Name is CREST (Thematic Fund) provide returns of 27.58%% in 3 Years, 14.33% in 5 Years and 11.12% in 10 Years.
"""

if __name__ == "__main__":

    summary_template = """
        given information {information} about a mutual funds from I want you to create:
        1. a short summary
        2. two interesting facts about them
        3. Which of them is a better option
    """

    summary_prompt_template = PromptTemplate(input_variables=["information"], template=summary_template)
    llm = ChatOpenAI(temperature=0, model_name="gpt-3.5-turbo")
    chain = LLMChain(llm=llm, prompt=summary_prompt_template)

    print(chain.run(information=information))


import numpy as np
from data.dataPreparation import load_data
from core.optimize import optimize
from core.sigmoid_func import sigmoid

def initialize_with_zeros(dim):
    """
    This function creates a vector of zeros of shape (dim, 1) for w and initializes b to 0.

    Argument:
    dim -- size of the w vector we want (or number of parameters in this case)

    Returns:
    w -- initialized vector of shape (dim, 1)
    b -- initialized scalar (corresponds to the bias)
    """
    w = np.zeros((dim, 1))
    b = 0

    # small tests to make sure variables are correct type
    assert (w.shape == (dim, 1))
    assert (isinstance(b, float) or isinstance(b, int))

    return w, b

def predict(w, b, X):
    '''
    Predict whether the label is 0 or 1 using learned logistic regression parameters (w, b)

    Arguments:
    w -- weights, a numpy array of size (number of features, 1)
    b -- bias, a scalar
    X -- data of size (number of features, number of examples)

    Returns:
    Y_prediction -- a numpy array (vector) containing all predictions (0/1) for the examples in X
    '''

    m = X.shape[1]
    Y_prediction = np.zeros((1, m))

    # Compute vector "A" predicting the probabilities of a cat being present in the picture
    A = sigmoid((np.dot(w.transpose(), X) + b))

    # Convert probabilities A[0,i] to actual predictions p[0,i]
    Y_prediction = np.where(A >= 0.5, 1, 0)

    # small tests to make sure variables are correct type
    assert (Y_prediction.shape == (1, m))

    return Y_prediction

def model(X_train, Y_train, X_test, Y_test, num_iterations=2000, learning_rate=0.5, print_cost=False):
    """
    Builds the logistic regression model by calling the function you've implemented previously

    Arguments:
    X_train -- training set represented by a numpy array of shape (number of features, m_train)
    Y_train -- training labels represented by a numpy array (vector) of shape (1, m_train)
    X_test -- test set represented by a numpy array of shape (number of features, m_test)
    Y_test -- test labels represented by a numpy array (vector) of shape (1, m_test)
    num_iterations -- hyperparameter representing the number of iterations to optimize the parameters
    learning_rate -- hyperparameter representing the learning rate used in the update rule of optimize()
    print_cost -- Set to true to print the cost every 100 iterations

    Returns:
    d -- dictionary containing information about the model.
    """

    # initialize parameters with zeros
    w, b = initialize_with_zeros(X_train.shape[0])

    # Gradient descent
    parameters, grads, costs = optimize(w, b, X_train, Y_train, num_iterations, learning_rate, print_cost)

    # Retrieve parameters w and b from dictionary "parameters"
    w = parameters["w"]
    b = parameters["b"]

    # Predict test/train set examples
    Y_prediction_test = predict(w, b, X_test)
    Y_prediction_train = predict(w, b, X_train)

    # Print train/test Errors
    print("train accuracy: {} %".format(100 - np.mean(np.abs(Y_prediction_train - Y_train)) * 100))
    print("test accuracy: {} %".format(100 - np.mean(np.abs(Y_prediction_test - Y_test)) * 100))

    d = {"costs": costs,
         "Y_prediction_test": Y_prediction_test,
         "Y_prediction_train": Y_prediction_train,
         "w": w,
         "b": b,
         "learning_rate": learning_rate,
         "num_iterations": num_iterations}

    return d

train_set_x, train_set_y, test_set_x, test_set_y, classes = load_data()
d = model(train_set_x, train_set_y, test_set_x, test_set_y, num_iterations=2000, learning_rate=0.005, print_cost=True)